更新到psdk 3.13.0,适配M400
This commit is contained in:
@ -155,7 +155,7 @@ T_DjiReturnCode DjiMediaFile_GetMediaFileAttr(T_DjiMediaFileHandle mediaFileHand
|
||||
}
|
||||
|
||||
T_DjiReturnCode DjiMediaFile_GetDataOrg(struct _DjiMediaFile *mediaFileHandle, uint32_t offset, uint16_t len,
|
||||
uint8_t *data, uint16_t *realLen)
|
||||
uint8_t *data, uint32_t *realLen)
|
||||
{
|
||||
if (mediaFileHandle->mediaFileOptItem.getDataOrgFunc == NULL) {
|
||||
USER_LOG_ERROR("Media file handle getDataOrgFunc null error");
|
||||
|
||||
@ -57,7 +57,7 @@ typedef struct {
|
||||
T_DjiReturnCode (*getAttrFunc)(struct _DjiMediaFile *mediaFileHandle, T_DjiCameraMediaFileAttr *mediaFileAttr);
|
||||
|
||||
T_DjiReturnCode (*getDataOrgFunc)(struct _DjiMediaFile *mediaFileHandle, uint32_t offset, uint16_t len,
|
||||
uint8_t *data, uint16_t *realLen);
|
||||
uint8_t *data, uint32_t *realLen);
|
||||
T_DjiReturnCode (*getFileSizeOrgFunc)(struct _DjiMediaFile *mediaFileHandle, uint32_t *fileSize);
|
||||
|
||||
T_DjiReturnCode (*createThmFunc)(struct _DjiMediaFile *mediaFileHandle);
|
||||
@ -91,7 +91,7 @@ T_DjiReturnCode DjiMediaFile_GetMediaFileAttr(T_DjiMediaFileHandle mediaFileHand
|
||||
T_DjiCameraMediaFileAttr *mediaFileAttr);
|
||||
|
||||
T_DjiReturnCode DjiMediaFile_GetDataOrg(struct _DjiMediaFile *mediaFileHandle, uint32_t offset, uint16_t len,
|
||||
uint8_t *data, uint16_t *realLen);
|
||||
uint8_t *data, uint32_t *realLen);
|
||||
T_DjiReturnCode DjiMediaFile_GetFileSizeOrg(struct _DjiMediaFile *mediaFileHandle, uint32_t *fileSize);
|
||||
|
||||
T_DjiReturnCode DjiMediaFile_CreateThm(T_DjiMediaFileHandle mediaFileHandle);
|
||||
|
||||
@ -82,7 +82,7 @@ T_DjiReturnCode DjiMediaFile_GetAttrFunc_JPG(struct _DjiMediaFile *mediaFileHand
|
||||
}
|
||||
|
||||
T_DjiReturnCode DjiMediaFile_GetDataOrigin_JPG(struct _DjiMediaFile *mediaFileHandle, uint32_t offset, uint16_t len,
|
||||
uint8_t *data, uint16_t *realLen)
|
||||
uint8_t *data, uint32_t *realLen)
|
||||
{
|
||||
return UtilFile_GetFileDataByPath(mediaFileHandle->filePath, offset, len, data, realLen);
|
||||
}
|
||||
|
||||
@ -49,7 +49,7 @@ T_DjiReturnCode DjiMediaFile_GetAttrFunc_JPG(struct _DjiMediaFile *mediaFileHand
|
||||
T_DjiCameraMediaFileAttr *mediaFileAttr);
|
||||
|
||||
T_DjiReturnCode DjiMediaFile_GetDataOrigin_JPG(struct _DjiMediaFile *mediaFileHandle, uint32_t offset, uint16_t len,
|
||||
uint8_t *data, uint16_t *realLen);
|
||||
uint8_t *data, uint32_t *realLen);
|
||||
T_DjiReturnCode DjiMediaFile_GetFileSizeOrigin_JPG(struct _DjiMediaFile *mediaFileHandle, uint32_t *fileSize);
|
||||
|
||||
T_DjiReturnCode DjiMediaFile_CreateThumbNail_JPG(struct _DjiMediaFile *mediaFileHandle);
|
||||
|
||||
@ -115,7 +115,7 @@ out:
|
||||
}
|
||||
|
||||
T_DjiReturnCode DjiMediaFile_GetDataOrigin_MP4(struct _DjiMediaFile *mediaFileHandle, uint32_t offset, uint16_t len,
|
||||
uint8_t *data, uint16_t *realLen)
|
||||
uint8_t *data, uint32_t *realLen)
|
||||
{
|
||||
return UtilFile_GetFileDataByPath(mediaFileHandle->filePath, offset, len, data, realLen);
|
||||
}
|
||||
|
||||
@ -49,7 +49,7 @@ T_DjiReturnCode DjiMediaFile_GetAttrFunc_MP4(struct _DjiMediaFile *mediaFileHand
|
||||
T_DjiCameraMediaFileAttr *mediaFileAttr);
|
||||
|
||||
T_DjiReturnCode DjiMediaFile_GetDataOrigin_MP4(struct _DjiMediaFile *mediaFileHandle, uint32_t offset, uint16_t len,
|
||||
uint8_t *data, uint16_t *realLen);
|
||||
uint8_t *data, uint32_t *realLen);
|
||||
T_DjiReturnCode DjiMediaFile_GetFileSizeOrigin_MP4(struct _DjiMediaFile *mediaFileHandle, uint32_t *fileSize);
|
||||
|
||||
T_DjiReturnCode DjiMediaFile_CreateThumbNail_MP4(struct _DjiMediaFile *mediaFileHandle);
|
||||
|
||||
@ -33,6 +33,7 @@
|
||||
#include "dji_gimbal.h"
|
||||
#include "dji_xport.h"
|
||||
#include "gimbal_emu/test_payload_gimbal_emu.h"
|
||||
#include <widget_interaction_test/test_widget_interaction.h>
|
||||
|
||||
/* Private constants ---------------------------------------------------------*/
|
||||
#define PAYLOAD_CAMERA_EMU_TASK_FREQ (100)
|
||||
@ -118,6 +119,7 @@ static T_TestCameraGimbalRotationArgument s_tapZoomNewestGimbalRotationArgument
|
||||
static uint32_t s_tapZoomNewestTargetHybridFocalLength = 0; // unit: 0.1mm
|
||||
static T_DjiMutexHandle s_tapZoomMutex = NULL;
|
||||
static E_DjiCameraVideoStreamType s_cameraVideoStreamType;
|
||||
static uint32_t s_currentVideoRecordTimeInSeconds = 0;
|
||||
|
||||
static int sockfd;
|
||||
static struct sockaddr_in server;
|
||||
@ -222,6 +224,7 @@ static T_DjiReturnCode SetMode(E_DjiCameraMode mode)
|
||||
|
||||
s_cameraState.cameraMode = mode;
|
||||
USER_LOG_INFO("set camera mode:%d", mode);
|
||||
DjiTest_WidgetLogAppend("set cam mode %d", mode);
|
||||
|
||||
returnCode = osalHandler->MutexUnlock(s_commonMutex);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
@ -252,6 +255,7 @@ static T_DjiReturnCode StartRecordVideo(void)
|
||||
|
||||
s_cameraState.isRecording = true;
|
||||
USER_LOG_INFO("start record video");
|
||||
DjiTest_WidgetLogAppend("start record video");
|
||||
|
||||
out:
|
||||
djiStat = osalHandler->MutexUnlock(s_commonMutex);
|
||||
@ -284,6 +288,7 @@ static T_DjiReturnCode StopRecordVideo(void)
|
||||
s_cameraState.isRecording = false;
|
||||
s_cameraState.currentVideoRecordingTimeInSeconds = 0;
|
||||
USER_LOG_INFO("stop record video");
|
||||
DjiTest_WidgetLogAppend("stop record video");
|
||||
|
||||
out:
|
||||
djiStat = osalHandler->MutexUnlock(s_commonMutex);
|
||||
@ -307,6 +312,7 @@ static T_DjiReturnCode StartShootPhoto(void)
|
||||
}
|
||||
|
||||
USER_LOG_INFO("start shoot photo");
|
||||
DjiTest_WidgetLogAppend("start shoot photo");
|
||||
s_cameraState.isStoring = true;
|
||||
|
||||
if (s_cameraShootPhotoMode == DJI_CAMERA_SHOOT_PHOTO_MODE_SINGLE) {
|
||||
@ -317,6 +323,7 @@ static T_DjiReturnCode StartShootPhoto(void)
|
||||
s_cameraState.shootingState = DJI_CAMERA_SHOOTING_INTERVAL_PHOTO;
|
||||
s_cameraState.isShootingIntervalStart = true;
|
||||
s_cameraState.currentPhotoShootingIntervalTimeInSeconds = s_cameraPhotoTimeIntervalSettings.timeIntervalSeconds;
|
||||
s_cameraState.currentPhotoShootingIntervalTimeInMs = s_cameraPhotoTimeIntervalSettings.timeIntervalMilliseconds;
|
||||
}
|
||||
|
||||
returnCode = osalHandler->MutexUnlock(s_commonMutex);
|
||||
@ -340,6 +347,7 @@ static T_DjiReturnCode StopShootPhoto(void)
|
||||
}
|
||||
|
||||
USER_LOG_INFO("stop shoot photo");
|
||||
DjiTest_WidgetLogAppend("stop shoot photo");
|
||||
s_cameraState.shootingState = DJI_CAMERA_SHOOTING_PHOTO_IDLE;
|
||||
s_cameraState.isStoring = false;
|
||||
s_cameraState.isShootingIntervalStart = false;
|
||||
@ -456,8 +464,9 @@ static T_DjiReturnCode SetPhotoTimeIntervalSettings(T_DjiCameraPhotoTimeInterval
|
||||
|
||||
s_cameraPhotoTimeIntervalSettings.captureCount = settings.captureCount;
|
||||
s_cameraPhotoTimeIntervalSettings.timeIntervalSeconds = settings.timeIntervalSeconds;
|
||||
USER_LOG_INFO("set photo interval settings count:%d seconds:%d", settings.captureCount,
|
||||
settings.timeIntervalSeconds);
|
||||
s_cameraPhotoTimeIntervalSettings.timeIntervalMilliseconds = settings.timeIntervalMilliseconds;
|
||||
USER_LOG_INFO("set photo interval settings count:%d seconds:%d.%d", settings.captureCount,
|
||||
settings.timeIntervalSeconds, settings.timeIntervalMilliseconds / 100);
|
||||
s_cameraState.currentPhotoShootingIntervalCount = settings.captureCount;
|
||||
|
||||
returnCode = osalHandler->MutexUnlock(s_commonMutex);
|
||||
@ -929,6 +938,8 @@ static T_DjiReturnCode DjiTest_CameraRotationGimbal(T_TestCameraGimbalRotationAr
|
||||
}
|
||||
|
||||
if (aircraftBaseInfo.djiAdapterType == DJI_SDK_ADAPTER_TYPE_SKYPORT_V2 ||
|
||||
aircraftBaseInfo.djiAdapterType == DJI_SDK_ADAPTER_TYPE_SKYPORT_V3 ||
|
||||
aircraftBaseInfo.djiAdapterType == DJI_SDK_ADAPTER_TYPE_EPORT_V2_RIBBON_CABLE ||
|
||||
aircraftBaseInfo.djiAdapterType == DJI_SDK_ADAPTER_TYPE_NONE) {
|
||||
returnCode = DjiTest_GimbalRotate(gimbalRotationArgument.rotationMode, gimbalRotationArgument.rotationProperty,
|
||||
gimbalRotationArgument.rotationValue);
|
||||
@ -967,6 +978,7 @@ static void *UserCamera_Task(void *arg)
|
||||
uint32_t currentTime = 0;
|
||||
bool isStartIntervalPhotoAction = false;
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
uint32_t intervalFreq = 10;
|
||||
|
||||
USER_UTIL_UNUSED(arg);
|
||||
|
||||
@ -1164,8 +1176,7 @@ out:
|
||||
}
|
||||
}
|
||||
|
||||
// 1Hz
|
||||
if (USER_UTIL_IS_WORK_TURN(step, 1, PAYLOAD_CAMERA_EMU_TASK_FREQ)) {
|
||||
if (USER_UTIL_IS_WORK_TURN(step, intervalFreq, PAYLOAD_CAMERA_EMU_TASK_FREQ)) {
|
||||
returnCode = osalHandler->MutexLock(s_commonMutex);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("lock mutex error: 0x%08llX.", returnCode);
|
||||
@ -1174,13 +1185,21 @@ out:
|
||||
|
||||
// 预估可拍照张数和可录像时长的功能。
|
||||
if (s_cameraState.isRecording) {
|
||||
s_cameraState.currentVideoRecordingTimeInSeconds++;
|
||||
s_cameraSDCardState.remainSpaceInMB =
|
||||
s_cameraSDCardState.remainSpaceInMB - SDCARD_PER_SECONDS_RECORD_SPACE_IN_MB;
|
||||
if (s_cameraSDCardState.remainSpaceInMB > SDCARD_TOTAL_SPACE_IN_MB) {
|
||||
s_cameraSDCardState.remainSpaceInMB = 0;
|
||||
s_cameraSDCardState.isFull = true;
|
||||
uint16_t preTimeInSeconds = s_cameraState.currentVideoRecordingTimeInSeconds;
|
||||
|
||||
s_currentVideoRecordTimeInSeconds++;
|
||||
|
||||
s_cameraState.currentVideoRecordingTimeInSeconds = s_currentVideoRecordTimeInSeconds / intervalFreq;
|
||||
if (s_cameraState.currentVideoRecordingTimeInSeconds > preTimeInSeconds) {
|
||||
s_cameraSDCardState.remainSpaceInMB =
|
||||
s_cameraSDCardState.remainSpaceInMB - SDCARD_PER_SECONDS_RECORD_SPACE_IN_MB;
|
||||
if (s_cameraSDCardState.remainSpaceInMB > SDCARD_TOTAL_SPACE_IN_MB) {
|
||||
s_cameraSDCardState.remainSpaceInMB = 0;
|
||||
s_cameraSDCardState.isFull = true;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
s_currentVideoRecordTimeInSeconds = 0;
|
||||
}
|
||||
|
||||
if (s_cameraState.isShootingIntervalStart == false) {
|
||||
@ -1189,10 +1208,16 @@ out:
|
||||
|
||||
if (s_cameraShootPhotoMode == DJI_CAMERA_SHOOT_PHOTO_MODE_INTERVAL
|
||||
&& s_cameraState.isShootingIntervalStart == true && s_cameraPhotoTimeIntervalSettings.captureCount > 0
|
||||
&& s_cameraPhotoTimeIntervalSettings.timeIntervalSeconds > 0) {
|
||||
s_cameraState.currentPhotoShootingIntervalTimeInSeconds--;
|
||||
&& (s_cameraPhotoTimeIntervalSettings.timeIntervalSeconds > 0 || s_cameraPhotoTimeIntervalSettings.timeIntervalMilliseconds > 0)) {
|
||||
uint16_t currentPhotoShootingIntervalTimeInMs = s_cameraState.currentPhotoShootingIntervalTimeInSeconds * 1000 +
|
||||
s_cameraState.currentPhotoShootingIntervalTimeInMs;
|
||||
|
||||
if ((s_cameraState.currentPhotoShootingIntervalTimeInSeconds == 0 &&
|
||||
currentPhotoShootingIntervalTimeInMs -= 1000 / intervalFreq;
|
||||
|
||||
s_cameraState.currentPhotoShootingIntervalTimeInSeconds = currentPhotoShootingIntervalTimeInMs / 1000;
|
||||
s_cameraState.currentPhotoShootingIntervalTimeInMs = currentPhotoShootingIntervalTimeInMs % 1000;
|
||||
|
||||
if ((currentPhotoShootingIntervalTimeInMs == 0 &&
|
||||
s_cameraState.currentPhotoShootingIntervalCount > 0) ||
|
||||
(s_cameraState.isShootingIntervalStart == true && isStartIntervalPhotoAction == false)) {
|
||||
|
||||
@ -1200,13 +1225,14 @@ out:
|
||||
|
||||
s_cameraState.shootingState = DJI_CAMERA_SHOOTING_INTERVAL_PHOTO;
|
||||
s_cameraState.isStoring = true;
|
||||
s_cameraState.currentPhotoShootingIntervalTimeInSeconds
|
||||
= s_cameraPhotoTimeIntervalSettings.timeIntervalSeconds;
|
||||
s_cameraState.currentPhotoShootingIntervalTimeInSeconds = s_cameraPhotoTimeIntervalSettings.timeIntervalSeconds;
|
||||
s_cameraState.currentPhotoShootingIntervalTimeInMs = s_cameraPhotoTimeIntervalSettings.timeIntervalMilliseconds;
|
||||
if (s_cameraState.currentPhotoShootingIntervalCount < INTERVAL_PHOTOGRAPH_ALWAYS_COUNT) {
|
||||
USER_LOG_INFO("interval taking photograph count:%d interval_time:%ds",
|
||||
USER_LOG_INFO("interval taking photograph count:%d interval_time:%d.%ds",
|
||||
(s_cameraPhotoTimeIntervalSettings.captureCount -
|
||||
s_cameraState.currentPhotoShootingIntervalCount + 1),
|
||||
s_cameraPhotoTimeIntervalSettings.timeIntervalSeconds);
|
||||
s_cameraPhotoTimeIntervalSettings.timeIntervalSeconds,
|
||||
s_cameraPhotoTimeIntervalSettings.timeIntervalMilliseconds / 100);
|
||||
s_cameraState.currentPhotoShootingIntervalCount--;
|
||||
if (s_cameraState.currentPhotoShootingIntervalCount == 0) {
|
||||
s_cameraState.shootingState = DJI_CAMERA_SHOOTING_PHOTO_IDLE;
|
||||
@ -1214,8 +1240,9 @@ out:
|
||||
s_cameraState.isShootingIntervalStart = false;
|
||||
}
|
||||
} else {
|
||||
USER_LOG_INFO("interval taking photograph always, interval_time:%ds",
|
||||
s_cameraPhotoTimeIntervalSettings.timeIntervalSeconds);
|
||||
USER_LOG_INFO("interval taking photograph always, interval_time:%d.%ds",
|
||||
s_cameraPhotoTimeIntervalSettings.timeIntervalSeconds,
|
||||
s_cameraPhotoTimeIntervalSettings.timeIntervalMilliseconds / 100);
|
||||
s_cameraState.currentPhotoShootingIntervalCount--;
|
||||
}
|
||||
}
|
||||
|
||||
@ -23,6 +23,7 @@
|
||||
*/
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <stdlib.h>
|
||||
#include "dji_logger.h"
|
||||
@ -132,6 +133,7 @@ static T_DjiPlaybackInfo s_playbackInfo = {0};//非常重要的变量!!!!!!!!!!!
|
||||
static T_DjiTaskHandle s_userSendVideoThread;
|
||||
static T_UtilBuffer s_mediaPlayCommandBufferHandler = {0};//非常重要的变量,用户自定义功能读取此变量中的命令执行相应的操作!!!!!!!!!!!!!!!!!!!
|
||||
static T_DjiMutexHandle s_mediaPlayCommandBufferMutex = {0};
|
||||
static T_DjiSemaHandle s_mediaPlayWorkSem = NULL;
|
||||
static uint8_t s_mediaPlayCommandBuffer[sizeof(T_TestPayloadCameraPlaybackCommand) * 32] = {0};
|
||||
static const char *s_frameKeyChar = "[PACKET]";
|
||||
static const char *s_frameDurationTimeKeyChar = "duration_time";
|
||||
@ -190,6 +192,11 @@ T_DjiReturnCode DjiTest_CameraEmuMediaStartService(void)
|
||||
s_psdkCameraMedia.StartDownloadNotification = StartDownloadNotification;
|
||||
s_psdkCameraMedia.StopDownloadNotification = StopDownloadNotification;
|
||||
|
||||
if (DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS != osalHandler->SemaphoreCreate(0, &s_mediaPlayWorkSem)) {
|
||||
USER_LOG_ERROR("SemaphoreCreate(\"%s\") error.", "s_mediaPlayWorkSem");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
}
|
||||
|
||||
if (osalHandler->MutexCreate(&s_mediaPlayCommandBufferMutex) != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("mutex create error");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
@ -199,7 +206,8 @@ T_DjiReturnCode DjiTest_CameraEmuMediaStartService(void)
|
||||
|
||||
// 注册相机类负载设备的下载回放功能
|
||||
if (aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M300_RTK ||
|
||||
aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M350_RTK) {
|
||||
aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M350_RTK ||
|
||||
aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M400) {
|
||||
returnCode = DjiPayloadCamera_RegMediaDownloadPlaybackHandler(&s_psdkCameraMedia);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("psdk camera media function init error.");
|
||||
@ -327,6 +335,7 @@ static T_DjiReturnCode DjiPlayback_PausePlay(T_DjiPlaybackInfo *playbackInfo)
|
||||
USER_LOG_ERROR("mutex unlock error");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
}
|
||||
osalHandler->SemaphorePost(s_mediaPlayWorkSem);
|
||||
}
|
||||
|
||||
playbackInfo->isInPlayProcess = 0;
|
||||
@ -511,7 +520,7 @@ static T_DjiReturnCode DjiPlayback_StartPlayProcess(const char *filePath, uint32
|
||||
USER_LOG_ERROR("mutex unlock error");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
}
|
||||
|
||||
osalHandler->SemaphorePost(s_mediaPlayWorkSem);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
@ -540,7 +549,7 @@ static T_DjiReturnCode DjiPlayback_StopPlayProcess(void)
|
||||
USER_LOG_ERROR("mutex unlock error");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
}
|
||||
|
||||
osalHandler->SemaphorePost(s_mediaPlayWorkSem);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
@ -751,7 +760,7 @@ static T_DjiReturnCode DjiPlayback_GetFrameRateOfVideoFile(const char *path, flo
|
||||
|
||||
ret = fscanf(fpCommand, "r_frame_rate=%d/%d", &frameRateMolecule, &frameRateDenominator);
|
||||
if (ret <= 0) {
|
||||
USER_LOG_ERROR("can not find frame rate.");
|
||||
USER_LOG_ERROR("can not find frame rate form \"%s\".", path);
|
||||
returnCode = DJI_ERROR_SYSTEM_MODULE_CODE_NOT_FOUND;
|
||||
goto out;
|
||||
}
|
||||
@ -802,7 +811,7 @@ static T_DjiReturnCode GetMediaFileDir(char *dirPath)
|
||||
static T_DjiReturnCode GetMediaFileOriginData(const char *filePath, uint32_t offset, uint32_t length, uint8_t *data)
|
||||
{
|
||||
T_DjiReturnCode returnCode;
|
||||
uint16_t realLen = 0;
|
||||
uint32_t realLen = 0;
|
||||
T_DjiMediaFileHandle mediaFileHandle;
|
||||
|
||||
returnCode = DjiMediaFile_CreateHandle(filePath, &mediaFileHandle);
|
||||
@ -1198,6 +1207,9 @@ static void *UserCameraMedia_SendVideoTask(void *arg)
|
||||
char *videoFilePath = NULL;
|
||||
char *transcodedFilePath = NULL;//转换视频编码后的文件路径
|
||||
float frameRate = 1.0f;
|
||||
uint32_t waitDuration = 1000 / SEND_VIDEO_TASK_FREQ;
|
||||
uint32_t rightNow = 0;
|
||||
uint32_t sendExpect = 0;
|
||||
T_TestPayloadCameraVideoFrameInfo *frameInfo = NULL;//时长,大小,此帧在文件中的位置
|
||||
uint32_t frameNumber = 0;
|
||||
uint32_t frameCount = 0;
|
||||
@ -1221,7 +1233,7 @@ static void *UserCameraMedia_SendVideoTask(void *arg)
|
||||
exit(1);
|
||||
}
|
||||
if (s_isMediaFileDirPathConfigured == true) {
|
||||
snprintf(tempPath, DJI_FILE_PATH_SIZE_MAX, "%sPSDK_0005.h264", s_mediaFileDirPath);
|
||||
snprintf(tempPath, DJI_FILE_PATH_SIZE_MAX, "%smedia_file/PSDK_0005.h264", s_mediaFileDirPath);
|
||||
} else {
|
||||
snprintf(tempPath, DJI_FILE_PATH_SIZE_MAX, "%smedia_file/PSDK_0005.h264", curFileDirPath);
|
||||
}
|
||||
@ -1255,8 +1267,16 @@ static void *UserCameraMedia_SendVideoTask(void *arg)
|
||||
|
||||
int frameNumberSendCounter=0;
|
||||
|
||||
(void)osalHandler->GetTimeMs(&rightNow);
|
||||
sendExpect = rightNow + waitDuration;
|
||||
while (1) {
|
||||
osalHandler->TaskSleepMs(1000 / SEND_VIDEO_TASK_FREQ);
|
||||
(void)osalHandler->GetTimeMs(&rightNow);
|
||||
if (sendExpect > rightNow) {
|
||||
waitDuration = sendExpect - rightNow;
|
||||
} else {
|
||||
waitDuration = 1000 / SEND_VIDEO_TASK_FREQ;
|
||||
}
|
||||
(void)osalHandler->SemaphoreTimedWait(s_mediaPlayWorkSem, waitDuration);
|
||||
|
||||
// response playback command
|
||||
if (osalHandler->MutexLock(s_mediaPlayCommandBufferMutex) != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
@ -1335,13 +1355,13 @@ static void *UserCameraMedia_SendVideoTask(void *arg)
|
||||
|
||||
fpFile = fopen(transcodedFilePath, "rb+");//打开编码后的文件
|
||||
if (fpFile == NULL) {
|
||||
USER_LOG_ERROR("open video file fail.111111111111111111111111");
|
||||
USER_LOG_ERROR("open video file:\"%s\" fail:%d.", transcodedFilePath, errno);
|
||||
continue;
|
||||
}
|
||||
|
||||
send:
|
||||
if (fpFile == NULL) {
|
||||
USER_LOG_ERROR("open video file fail.222222222222222222222222222");
|
||||
USER_LOG_ERROR("open video file fail.");
|
||||
continue;
|
||||
}
|
||||
|
||||
@ -1363,16 +1383,10 @@ send:
|
||||
continue;
|
||||
}
|
||||
|
||||
//????????????????????????????????
|
||||
if (!USER_UTIL_IS_WORK_TURN(sendVideoStep++, frameRate, SEND_VIDEO_TASK_FREQ))
|
||||
continue;
|
||||
|
||||
|
||||
//3. 解析视频流文件
|
||||
//基于PSDK 开发的相机类负载设备获取视频流文件的信息后,将解析视频流文件的内容,识别视频流文件的帧头。
|
||||
frameBufSize = frameInfo[frameNumber].size;//这行几行代码的意思:每一帧的大小不一样
|
||||
printf("第 %d 帧大小:%d\n", frameNumberSendCounter, frameBufSize);
|
||||
frameNumberSendCounter++;
|
||||
if (videoStreamType == DJI_CAMERA_VIDEO_STREAM_TYPE_H264_DJI_FORMAT) {
|
||||
frameBufSize = frameBufSize + VIDEO_FRAME_AUD_LEN;
|
||||
}
|
||||
@ -1406,7 +1420,7 @@ send:
|
||||
|
||||
// for(int i=0;i<dataLength - VIDEO_FRAME_AUD_LEN;i++)
|
||||
// {
|
||||
//// printf("%c",data[i]);
|
||||
// //printf("%c",data[i]);
|
||||
// printf("%02X ",dataBuffer[i]);
|
||||
// }
|
||||
// printf("\n");
|
||||
@ -1423,6 +1437,9 @@ send:
|
||||
lengthOfDataHaveBeenSent += lengthOfDataToBeSent;
|
||||
}
|
||||
|
||||
(void)osalHandler->GetTimeMs(&sendExpect);
|
||||
sendExpect += (1000 / frameRate);
|
||||
|
||||
if ((frameNumber++) >= frameCount) {
|
||||
USER_LOG_DEBUG("reach file tail.");
|
||||
frameNumber = 0;
|
||||
@ -1436,10 +1453,10 @@ send:
|
||||
returnCode = DjiPayloadCamera_GetVideoStreamState(&videoStreamState);
|
||||
if (returnCode == DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_DEBUG(
|
||||
"video stream state: realtimeBandwidthLimit: %d, realtimeBandwidthBeforeFlowController: %d, realtimeBandwidthAfterFlowController:%d busyState: %d.",
|
||||
videoStreamState.realtimeBandwidthLimit, videoStreamState.realtimeBandwidthBeforeFlowController,
|
||||
videoStreamState.realtimeBandwidthAfterFlowController,
|
||||
videoStreamState.busyState);
|
||||
"video stream state: realtimeBandwidthLimit: %d, realtimeBandwidthBeforeFlowController: %d, realtimeBandwidthAfterFlowController:%d busyState: %d.",
|
||||
videoStreamState.realtimeBandwidthLimit, videoStreamState.realtimeBandwidthBeforeFlowController,
|
||||
videoStreamState.realtimeBandwidthAfterFlowController,
|
||||
videoStreamState.busyState);
|
||||
} else {
|
||||
USER_LOG_ERROR("get video stream state error.");
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -47,7 +47,6 @@ typedef enum {
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SET_CAMERA_TAP_ZOOM_POINT,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SET_CAMERA_ZOOM_PARAM,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SHOOT_SINGLE_PHOTO,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SHOOT_AEB_PHOTO,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SHOOT_BURST_PHOTO,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SHOOT_INTERVAL_PHOTO,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_RECORD_VIDEO,
|
||||
@ -56,6 +55,23 @@ typedef enum {
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_THERMOMETRY,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_GET_LIDAR_RANGING_INFO,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_IR_CAMERA_ZOOM_PARAM,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SET_NIGHT_SCENE_MODE,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SET_CAPTURE_RECORDING_STREAMS,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SHOW_STORAGE_INFO,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_FORMAT_SD_CARD,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SET_LINK_ZOOM,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SET_USER_CUSTOM_DIR_FILE_NAME,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_RESET_CAMERA_SETTINGS,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SET_AE_LOCK_MODE,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SET_FOCUS_RING_VALUE,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_CONNECT_STATUS_TEST,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SET_GET_PHOTO_VIDEO_PARAM,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SET_METERING_MODE,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SET_METERING_POINT,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_FFC_MODE_AND_TRRIGER,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SET_GAIN_MODE,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_GET_CAMERA_STATUS,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SUBSCRIBE_POINT_CLOUD,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_INDEX_MAX
|
||||
} E_DjiTestCameraManagerSampleSelect;
|
||||
/* Exported functions --------------------------------------------------------*/
|
||||
@ -206,18 +222,6 @@ T_DjiReturnCode DjiTest_CameraManagerStartShootSinglePhoto(E_DjiMountPosition po
|
||||
T_DjiReturnCode DjiTest_CameraManagerStartShootBurstPhoto(E_DjiMountPosition position,
|
||||
E_DjiCameraBurstCount burstCount);
|
||||
|
||||
/*! @brief Sample to shoot AEB photo, using async api
|
||||
*
|
||||
* @note In this interface, camera will be set to be the SHOOT_PHOTO mode
|
||||
* then start to shoot a AEB photo.
|
||||
* @param index payload node index, input limit see enum
|
||||
* DJI::OSDK::PayloadIndexType
|
||||
* @param photoNum The number of pictures in each AEB shooting
|
||||
* @return T_DjiReturnCode error code
|
||||
*/
|
||||
T_DjiReturnCode DjiTest_CameraManagerStartShootAEBPhoto(E_DjiMountPosition position,
|
||||
E_DjiCameraManagerPhotoAEBCount aebCount);
|
||||
|
||||
/*! @brief Sample to start shooting interval photo, using async api
|
||||
*
|
||||
* @note In this interface, camera will be set to be the SHOOT_PHOTO mode
|
||||
|
||||
@ -0,0 +1,98 @@
|
||||
/**
|
||||
********************************************************************
|
||||
* @file test_cloud_api_by_web_socket.c
|
||||
* @brief
|
||||
*
|
||||
* @copyright (c) 2021 DJI. All rights reserved.
|
||||
*
|
||||
* All information contained herein is, and remains, the property of DJI.
|
||||
* The intellectual and technical concepts contained herein are proprietary
|
||||
* to DJI and may be covered by U.S. and foreign patents, patents in process,
|
||||
* and protected by trade secret or copyright law. Dissemination of this
|
||||
* information, including but not limited to data and other proprietary
|
||||
* material(s) incorporated within the information, in any form, is strictly
|
||||
* prohibited without the express written consent of DJI.
|
||||
*
|
||||
* If you receive this source code without DJI’s authorization, you may not
|
||||
* further disseminate the information, and you must immediately remove the
|
||||
* source code and notify DJI of its removal. DJI reserves the right to pursue
|
||||
* legal actions against you for any loss(es) or damage(s) caused by your
|
||||
* failure to do so.
|
||||
*
|
||||
*********************************************************************
|
||||
*/
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
|
||||
#include <utils/util_misc.h>
|
||||
#include <stdio.h>
|
||||
#include <utils/util_md5.h>
|
||||
#include "dji_cloud_api_by_websockt.h"
|
||||
#include "dji_logger.h"
|
||||
#include "dji_platform.h"
|
||||
#include "test_cloud_api_by_web_socket.h"
|
||||
|
||||
/* Private constants ---------------------------------------------------------*/
|
||||
#define TEST_MOP_CHANNEL_NORMAL_TRANSFOR_SEND_BUFFER (64 * 1024)
|
||||
|
||||
/* Private types -------------------------------------------------------------*/
|
||||
|
||||
/* Private values -------------------------------------------------------------*/
|
||||
|
||||
/* Private functions declaration ---------------------------------------------*/
|
||||
static T_DjiTaskHandle s_testCloudApiByWebSocketSendTask;
|
||||
|
||||
/* Private functions definition-----------------------------------------------*/
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wmissing-noreturn"
|
||||
#pragma GCC diagnostic ignored "-Wreturn-type"
|
||||
|
||||
static void *DjiTest_CloudApiByWebSocketSendTask(void *arg)
|
||||
{
|
||||
uint8_t *sendBuf = NULL;
|
||||
uint32_t realLen = 0;
|
||||
T_DjiReturnCode returnCode;
|
||||
uint32_t sendDataCount = 0;
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
|
||||
USER_UTIL_UNUSED(arg);
|
||||
|
||||
sendBuf = osalHandler->Malloc(TEST_MOP_CHANNEL_NORMAL_TRANSFOR_SEND_BUFFER);
|
||||
if (sendBuf == NULL) {
|
||||
USER_LOG_ERROR("cloud_api over mop malloc send buffer error");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
while (1) {
|
||||
sendDataCount++;
|
||||
memset(sendBuf, 'A' - 1 + (sendDataCount % 26), TEST_MOP_CHANNEL_NORMAL_TRANSFOR_SEND_BUFFER);
|
||||
returnCode = DjiCloudApi_SendDataByWebSocket(sendBuf, TEST_MOP_CHANNEL_NORMAL_TRANSFOR_SEND_BUFFER, &realLen);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_WARN("cloud_api over mop send data to channel error,stat:0x%08llX", returnCode);
|
||||
} else {
|
||||
USER_LOG_INFO("cloud_api over mop send data to channel length:%d count:%d", realLen, sendDataCount);
|
||||
}
|
||||
|
||||
osalHandler->TaskSleepMs(1000);
|
||||
}
|
||||
}
|
||||
|
||||
#pragma GCC diagnostic pop
|
||||
|
||||
/* Exported functions definition ---------------------------------------------*/
|
||||
T_DjiReturnCode DjiTest_CloudApiByWebSocketStartService(void)
|
||||
{
|
||||
T_DjiReturnCode returnCode = DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
|
||||
returnCode = osalHandler->TaskCreate("CloudApiByWebSocket", DjiTest_CloudApiByWebSocketSendTask,
|
||||
2048, NULL, &s_testCloudApiByWebSocketSendTask);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("cloud_api over mop send task create error, stat:0x%08llX.", returnCode);
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
}
|
||||
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/
|
||||
@ -0,0 +1,49 @@
|
||||
/**
|
||||
********************************************************************
|
||||
* @file test_mop_channel.h
|
||||
* @brief This is the header file for "test_mop_channel.c", defining the structure and
|
||||
* (exported) function prototypes.
|
||||
*
|
||||
* @copyright (c) 2021 DJI. All rights reserved.
|
||||
*
|
||||
* All information contained herein is, and remains, the property of DJI.
|
||||
* The intellectual and technical concepts contained herein are proprietary
|
||||
* to DJI and may be covered by U.S. and foreign patents, patents in process,
|
||||
* and protected by trade secret or copyright law. Dissemination of this
|
||||
* information, including but not limited to data and other proprietary
|
||||
* material(s) incorporated within the information, in any form, is strictly
|
||||
* prohibited without the express written consent of DJI.
|
||||
*
|
||||
* If you receive this source code without DJI’s authorization, you may not
|
||||
* further disseminate the information, and you must immediately remove the
|
||||
* source code and notify DJI of its removal. DJI reserves the right to pursue
|
||||
* legal actions against you for any loss(es) or damage(s) caused by your
|
||||
* failure to do so.
|
||||
*
|
||||
*********************************************************************
|
||||
*/
|
||||
|
||||
/* Define to prevent recursive inclusion -------------------------------------*/
|
||||
#ifndef TEST_CLOUD_API_BY_WEEBSOCKET_H
|
||||
#define TEST_CLOUD_API_BY_WEEBSOCKET_H
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include "dji_typedef.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Exported constants --------------------------------------------------------*/
|
||||
|
||||
/* Exported types ------------------------------------------------------------*/
|
||||
|
||||
/* Exported functions --------------------------------------------------------*/
|
||||
T_DjiReturnCode DjiTest_CloudApiByWebSocketStartService(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // TEST_CLOUD_API_BY_WEEBSOCKET_H
|
||||
/************************ (C) COPYRIGHT DJI Innovations *******END OF FILE******/
|
||||
@ -42,8 +42,12 @@
|
||||
/* Private functions declaration ---------------------------------------------*/
|
||||
static void *UserDataTransmission_Task(void *arg);
|
||||
static T_DjiReturnCode ReceiveDataFromMobile(const uint8_t *data, uint16_t len);
|
||||
static T_DjiReturnCode ReceiveDataFromOnboardComputer(const uint8_t *data, uint16_t len);
|
||||
static T_DjiReturnCode ReceiveDataFromCloud(const uint8_t *data, uint16_t len);
|
||||
static T_DjiReturnCode ReceiveDataFromExtensionPort(const uint8_t *data, uint16_t len);
|
||||
static T_DjiReturnCode ReceiveDataFromPayload(const uint8_t *data, uint16_t len);
|
||||
static T_DjiReturnCode ReceiveDataFromPayload1(const uint8_t *data, uint16_t len);
|
||||
static T_DjiReturnCode ReceiveDataFromPayload2(const uint8_t *data, uint16_t len);
|
||||
static T_DjiReturnCode ReceiveDataFromPayload3(const uint8_t *data, uint16_t len);
|
||||
|
||||
/* Private variables ---------------------------------------------------------*/
|
||||
static T_DjiTaskHandle s_userDataTransmissionThread;//线程句柄
|
||||
@ -80,13 +84,31 @@ T_DjiReturnCode DjiTest_DataTransmissionStartService(void)
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
}
|
||||
|
||||
if (s_aircraftInfoBaseInfo.mountPosition == DJI_MOUNT_POSITION_PAYLOAD_PORT_NO1 ||
|
||||
if ((s_aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M30 ||
|
||||
s_aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M30T ||
|
||||
s_aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M3D ||
|
||||
s_aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M3TD ||
|
||||
s_aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M4D ||
|
||||
s_aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M4TD) &&
|
||||
s_aircraftInfoBaseInfo.mountPositionType != DJI_MOUNT_POSITION_TYPE_MANIFOLD3_ONBOARD) {
|
||||
channelAddress = DJI_CHANNEL_ADDRESS_CLOUD_API;
|
||||
djiStat = DjiLowSpeedDataChannel_RegRecvDataCallback(channelAddress, ReceiveDataFromCloud);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("register receive data from cloud error.");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
}
|
||||
}
|
||||
|
||||
if (s_aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M400) {
|
||||
USER_LOG_INFO("Only supports small data transmission between PSDK and MSDK.");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
} else if (s_aircraftInfoBaseInfo.mountPosition == DJI_MOUNT_POSITION_PAYLOAD_PORT_NO1 ||
|
||||
s_aircraftInfoBaseInfo.mountPosition == DJI_MOUNT_POSITION_PAYLOAD_PORT_NO2 ||
|
||||
s_aircraftInfoBaseInfo.mountPosition == DJI_MOUNT_POSITION_PAYLOAD_PORT_NO3) {
|
||||
channelAddress = DJI_CHANNEL_ADDRESS_EXTENSION_PORT;
|
||||
djiStat = DjiLowSpeedDataChannel_RegRecvDataCallback(channelAddress, ReceiveDataFromOnboardComputer);
|
||||
djiStat = DjiLowSpeedDataChannel_RegRecvDataCallback(channelAddress, ReceiveDataFromExtensionPort);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("register receive data from onboard coputer error.");
|
||||
USER_LOG_ERROR("register receive data from extension port error.");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
}
|
||||
|
||||
@ -103,9 +125,24 @@ T_DjiReturnCode DjiTest_DataTransmissionStartService(void)
|
||||
USER_LOG_ERROR("get data stream remote address error.");
|
||||
}
|
||||
|
||||
} else if (s_aircraftInfoBaseInfo.mountPosition == DJI_MOUNT_POSITION_EXTENSION_PORT) {
|
||||
} else if (s_aircraftInfoBaseInfo.mountPosition == DJI_MOUNT_POSITION_EXTENSION_PORT
|
||||
|| DJI_MOUNT_POSITION_EXTENSION_LITE_PORT == s_aircraftInfoBaseInfo.mountPosition) {
|
||||
channelAddress = DJI_CHANNEL_ADDRESS_PAYLOAD_PORT_NO1;
|
||||
djiStat = DjiLowSpeedDataChannel_RegRecvDataCallback(channelAddress, ReceiveDataFromPayload);
|
||||
djiStat = DjiLowSpeedDataChannel_RegRecvDataCallback(channelAddress, ReceiveDataFromPayload1);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("register receive data from payload NO1 error.");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
}
|
||||
|
||||
channelAddress = DJI_CHANNEL_ADDRESS_PAYLOAD_PORT_NO2;
|
||||
djiStat = DjiLowSpeedDataChannel_RegRecvDataCallback(channelAddress, ReceiveDataFromPayload2);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("register receive data from payload NO1 error.");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
}
|
||||
|
||||
channelAddress = DJI_CHANNEL_ADDRESS_PAYLOAD_PORT_NO3;
|
||||
djiStat = DjiLowSpeedDataChannel_RegRecvDataCallback(channelAddress, ReceiveDataFromPayload3);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("register receive data from payload NO1 error.");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
@ -154,7 +191,7 @@ T_DjiReturnCode DjiTest_DataTransmissionStopService(void)
|
||||
static void *UserDataTransmission_Task(void *arg)
|
||||
{
|
||||
T_DjiReturnCode djiStat;
|
||||
const uint8_t dataToBeSent[] = "DJI Data Transmission Test Data.\r\n";
|
||||
const uint8_t dataToBeSent[] = "DJI Data Transmission Test Data.";
|
||||
T_DjiDataChannelState state = {0};
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
E_DjiChannelAddress channelAddress;
|
||||
@ -179,25 +216,48 @@ static void *UserDataTransmission_Task(void *arg)
|
||||
USER_LOG_ERROR("get send to mobile channel state error.");
|
||||
}
|
||||
|
||||
if (s_aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M30 ||
|
||||
s_aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M30T ||
|
||||
s_aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M3D ||
|
||||
s_aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M3TD ||
|
||||
s_aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M4D ||
|
||||
s_aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M4TD ) {
|
||||
channelAddress = DJI_CHANNEL_ADDRESS_CLOUD_API;
|
||||
djiStat = DjiLowSpeedDataChannel_SendData(channelAddress, dataToBeSent, sizeof(dataToBeSent));
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
|
||||
USER_LOG_ERROR("send data to cloud error.");
|
||||
|
||||
djiStat = DjiLowSpeedDataChannel_GetSendDataState(channelAddress, &state);
|
||||
if (djiStat == DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_DEBUG(
|
||||
"send to cloud state: realtimeBandwidthBeforeFlowController: %d, realtimeBandwidthAfterFlowController: %d, busyState: %d.",
|
||||
state.realtimeBandwidthBeforeFlowController, state.realtimeBandwidthAfterFlowController,
|
||||
state.busyState);
|
||||
} else {
|
||||
USER_LOG_ERROR("get send to cloud channel state error.");
|
||||
}
|
||||
}
|
||||
|
||||
if (s_aircraftInfoBaseInfo.mountPosition == DJI_MOUNT_POSITION_PAYLOAD_PORT_NO1 ||
|
||||
s_aircraftInfoBaseInfo.mountPosition == DJI_MOUNT_POSITION_PAYLOAD_PORT_NO2 ||
|
||||
s_aircraftInfoBaseInfo.mountPosition == DJI_MOUNT_POSITION_PAYLOAD_PORT_NO3) {
|
||||
channelAddress = DJI_CHANNEL_ADDRESS_EXTENSION_PORT;
|
||||
djiStat = DjiLowSpeedDataChannel_SendData(channelAddress, dataToBeSent, sizeof(dataToBeSent));
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
|
||||
USER_LOG_ERROR("send data to onboard computer error.");
|
||||
USER_LOG_ERROR("send data to extension port error.");
|
||||
|
||||
djiStat = DjiLowSpeedDataChannel_GetSendDataState(channelAddress, &state);
|
||||
if (djiStat == DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_DEBUG(
|
||||
"send to onboard computer state: realtimeBandwidthBeforeFlowController: %d, realtimeBandwidthAfterFlowController: %d, busyState: %d.",
|
||||
"send to extension port state: realtimeBandwidthBeforeFlowController: %d, realtimeBandwidthAfterFlowController: %d, busyState: %d.",
|
||||
state.realtimeBandwidthBeforeFlowController, state.realtimeBandwidthAfterFlowController,
|
||||
state.busyState);
|
||||
} else {
|
||||
USER_LOG_ERROR("get send to onboard computer channel state error.");
|
||||
USER_LOG_ERROR("get send to extension port channel state error.");
|
||||
}
|
||||
|
||||
if (DjiPlatform_GetSocketHandler() != NULL) {
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
djiStat = DjiHighSpeedDataChannel_SendDataStreamData(dataToBeSent, sizeof(dataToBeSent));
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
|
||||
USER_LOG_ERROR("send data to data stream error.");
|
||||
@ -210,21 +270,22 @@ static void *UserDataTransmission_Task(void *arg)
|
||||
} else {
|
||||
USER_LOG_ERROR("get data stream state error.");
|
||||
}
|
||||
#endif
|
||||
}
|
||||
} else if (s_aircraftInfoBaseInfo.mountPosition == DJI_MOUNT_POSITION_EXTENSION_PORT) {
|
||||
channelAddress = DJI_CHANNEL_ADDRESS_PAYLOAD_PORT_NO1;
|
||||
djiStat = DjiLowSpeedDataChannel_SendData(channelAddress, dataToBeSent, sizeof(dataToBeSent));
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
|
||||
USER_LOG_ERROR("send data to onboard computer error.");
|
||||
USER_LOG_ERROR("send data to extension port error.");
|
||||
|
||||
djiStat = DjiLowSpeedDataChannel_GetSendDataState(channelAddress, &state);
|
||||
if (djiStat == DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_DEBUG(
|
||||
"send to onboard computer state: realtimeBandwidthBeforeFlowController: %d, realtimeBandwidthAfterFlowController: %d, busyState: %d.",
|
||||
"send to extension port state: realtimeBandwidthBeforeFlowController: %d, realtimeBandwidthAfterFlowController: %d, busyState: %d.",
|
||||
state.realtimeBandwidthBeforeFlowController, state.realtimeBandwidthAfterFlowController,
|
||||
state.busyState);
|
||||
} else {
|
||||
USER_LOG_ERROR("get send to onboard computer channel state error.");
|
||||
USER_LOG_ERROR("get send to extension port channel state error.");
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -255,7 +316,7 @@ static T_DjiReturnCode ReceiveDataFromMobile(const uint8_t *data, uint16_t len)
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
static T_DjiReturnCode ReceiveDataFromOnboardComputer(const uint8_t *data, uint16_t len)
|
||||
static T_DjiReturnCode ReceiveDataFromCloud(const uint8_t *data, uint16_t len)
|
||||
{
|
||||
char *printData = NULL;
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
@ -268,7 +329,28 @@ static T_DjiReturnCode ReceiveDataFromOnboardComputer(const uint8_t *data, uint1
|
||||
|
||||
strncpy(printData, (const char *) data, len);
|
||||
printData[len] = '\0';
|
||||
USER_LOG_INFO("receive data from onboard computer: %s, len:%d.", printData, len);
|
||||
USER_LOG_INFO("receive data from cloud: %s, len:%d.", printData, len);
|
||||
DjiTest_WidgetLogAppend("receive data: %s, len:%d.", printData, len);
|
||||
|
||||
osalHandler->Free(printData);
|
||||
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
static T_DjiReturnCode ReceiveDataFromExtensionPort(const uint8_t *data, uint16_t len)
|
||||
{
|
||||
char *printData = NULL;
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
|
||||
printData = osalHandler->Malloc(len + 1);
|
||||
if (printData == NULL) {
|
||||
USER_LOG_ERROR("malloc memory for printData fail.");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_MEMORY_ALLOC_FAILED;
|
||||
}
|
||||
|
||||
strncpy(printData, (const char *) data, len);
|
||||
printData[len] = '\0';
|
||||
USER_LOG_INFO("receive data from extension port: %s, len:%d.", printData, len);
|
||||
DjiTest_WidgetLogAppend("receive data: %s, len:%d.", printData, len);
|
||||
|
||||
osalHandler->Free(printData);
|
||||
@ -297,4 +379,22 @@ static T_DjiReturnCode ReceiveDataFromPayload(const uint8_t *data, uint16_t len)
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
static T_DjiReturnCode ReceiveDataFromPayload1(const uint8_t *data, uint16_t len)
|
||||
{
|
||||
USER_LOG_INFO("Receive from payload on port 1");
|
||||
return ReceiveDataFromPayload(data, len);
|
||||
}
|
||||
|
||||
static T_DjiReturnCode ReceiveDataFromPayload2(const uint8_t *data, uint16_t len)
|
||||
{
|
||||
USER_LOG_INFO("Receive from payload on port 2");
|
||||
return ReceiveDataFromPayload(data, len);
|
||||
}
|
||||
|
||||
static T_DjiReturnCode ReceiveDataFromPayload3(const uint8_t *data, uint16_t len)
|
||||
{
|
||||
USER_LOG_INFO("Receive from payload on port 3");
|
||||
return ReceiveDataFromPayload(data, len);
|
||||
}
|
||||
|
||||
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/
|
||||
|
||||
@ -32,7 +32,7 @@
|
||||
|
||||
/* Private constants ---------------------------------------------------------*/
|
||||
#define FC_SUBSCRIPTION_TASK_FREQ (1)
|
||||
#define FC_SUBSCRIPTION_TASK_STACK_SIZE (1024)
|
||||
#define FC_SUBSCRIPTION_TASK_STACK_SIZE (2048)
|
||||
|
||||
/* Private types -------------------------------------------------------------*/
|
||||
|
||||
@ -168,9 +168,9 @@ T_DjiReturnCode DjiTest_FcSubscriptionRunSample(void)
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
}
|
||||
|
||||
USER_LOG_INFO("--> Step 3: Get latest value of the subscribed topics in the next 20 seconds\r\n");
|
||||
USER_LOG_INFO("--> Step 3: Get latest value of the subscribed topics in the next 10 seconds\r\n");
|
||||
|
||||
for (int i = 0; i < 20; ++i) {
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
osalHandler->TaskSleepMs(1000 / FC_SUBSCRIPTION_TASK_FREQ);
|
||||
djiStat = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_VELOCITY,
|
||||
(uint8_t *) &velocity,
|
||||
@ -194,8 +194,8 @@ T_DjiReturnCode DjiTest_FcSubscriptionRunSample(void)
|
||||
USER_LOG_INFO("gps position: x = %d y = %d z = %d.", gpsPosition.x, gpsPosition.y, gpsPosition.z);
|
||||
}
|
||||
|
||||
//Attention: if you want to subscribe the single battery info on M300 RTK, you need connect USB cable to
|
||||
//OSDK device or use topic DJI_FC_SUBSCRIPTION_TOPIC_BATTERY_INFO instead.
|
||||
// Attention: if you want to subscribe the single battery info on M300 RTK, you need connect USB cable to
|
||||
// OSDK device or use topic DJI_FC_SUBSCRIPTION_TOPIC_BATTERY_INFO instead.
|
||||
djiStat = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_BATTERY_SINGLE_INFO_INDEX1,
|
||||
(uint8_t *) &singleBatteryInfo,
|
||||
sizeof(T_DjiFcSubscriptionSingleBatteryInfo),
|
||||
@ -225,7 +225,26 @@ T_DjiReturnCode DjiTest_FcSubscriptionRunSample(void)
|
||||
}
|
||||
}
|
||||
|
||||
USER_LOG_INFO("--> Step 4: Deinit fc subscription module");
|
||||
USER_LOG_INFO("--> Step 4: Unsubscribe the topics of quaternion, velocity and gps position");
|
||||
djiStat = DjiFcSubscription_UnSubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_QUATERNION);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("UnSubscribe topic quaternion error.");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
}
|
||||
|
||||
djiStat = DjiFcSubscription_UnSubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_VELOCITY);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("UnSubscribe topic quaternion error.");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
}
|
||||
|
||||
djiStat = DjiFcSubscription_UnSubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("UnSubscribe topic quaternion error.");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
}
|
||||
|
||||
USER_LOG_INFO("--> Step 5: Deinit fc subscription module");
|
||||
|
||||
djiStat = DjiFcSubscription_DeInit();
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
|
||||
@ -34,14 +34,6 @@
|
||||
/* Private constants ---------------------------------------------------------*/
|
||||
|
||||
/* Private types -------------------------------------------------------------*/
|
||||
#pragma pack(1)
|
||||
typedef struct {
|
||||
dji_f32_t x;
|
||||
dji_f32_t y;
|
||||
dji_f32_t z;
|
||||
} T_DjiTestFlightControlVector3f; // pack(1)
|
||||
#pragma pack()
|
||||
|
||||
typedef struct {
|
||||
E_DjiFcSubscriptionDisplayMode displayMode;
|
||||
char *displayModeStr;
|
||||
@ -51,6 +43,8 @@ typedef struct {
|
||||
static T_DjiOsalHandler *s_osalHandler = NULL;
|
||||
static const double s_earthCenter = 6378137.0;
|
||||
static const double s_degToRad = 0.01745329252;
|
||||
static bool s_isFtsCallbackRegistered = false;
|
||||
static int32_t s_ftsTriggerCount = 0;
|
||||
|
||||
static const T_DjiTestFlightControlDisplayModeStr s_flightControlDisplayModeStr[] = {
|
||||
{.displayMode = DJI_FC_SUBSCRIPTION_DISPLAY_MODE_ATTITUDE, .displayModeStr = "attitude mode"},
|
||||
@ -96,8 +90,6 @@ static bool DjiTest_FlightControlMoveByPositionOffset(T_DjiTestFlightControlVect
|
||||
float yawDesiredInDeg,
|
||||
float posThresholdInM,
|
||||
float yawThresholdInDeg);
|
||||
static void DjiTest_FlightControlVelocityAndYawRateCtrl(T_DjiTestFlightControlVector3f offsetDesired, float yawRate,
|
||||
uint32_t timeMs);
|
||||
static T_DjiReturnCode DjiTest_FlightControlInit(void);
|
||||
static T_DjiReturnCode DjiTest_FlightControlDeInit(void);
|
||||
static void DjiTest_FlightControlTakeOffLandingSample(void);
|
||||
@ -105,6 +97,10 @@ static void DjiTest_FlightControlPositionControlSample(void);
|
||||
static void DjiTest_FlightControlGoHomeForceLandingSample(void);
|
||||
static void DjiTest_FlightControlVelocityControlSample(void);
|
||||
static void DjiTest_FlightControlArrestFlyingSample(void);
|
||||
static void DjiTest_FlightControlSetGetParamSample(void);
|
||||
static void DjiTest_FlightControlPassiveTriggerFtsSample(void);
|
||||
static void DjiTest_FlightControlSlowRotateMotorSample(void);
|
||||
static T_DjiReturnCode DjiTest_TriggerFtsEventCallback(void);
|
||||
static void DjiTest_FlightControlSample(E_DjiTestFlightCtrlSampleSelect flightCtrlSampleSelect);
|
||||
|
||||
/* Exported functions definition ---------------------------------------------*/
|
||||
@ -138,11 +134,16 @@ T_DjiReturnCode DjiTest_FlightControlRunSample(E_DjiTestFlightCtrlSampleSelect f
|
||||
T_DjiReturnCode DjiTest_FlightControlInit(void)
|
||||
{
|
||||
T_DjiReturnCode returnCode;
|
||||
T_DjiFlightControllerRidInfo ridInfo = {0};
|
||||
|
||||
s_osalHandler = DjiPlatform_GetOsalHandler();
|
||||
if (!s_osalHandler) return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
|
||||
returnCode = DjiFlightController_Init();
|
||||
ridInfo.latitude = 22.542812;
|
||||
ridInfo.longitude = 113.958902;
|
||||
ridInfo.altitude = 10;
|
||||
|
||||
returnCode = DjiFlightController_Init(ridInfo);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Init flight controller module failed, error code:0x%08llX", returnCode);
|
||||
return returnCode;
|
||||
@ -236,7 +237,7 @@ T_DjiReturnCode DjiTest_FlightControlDeInit(void)
|
||||
{
|
||||
T_DjiReturnCode returnCode;
|
||||
|
||||
returnCode = DjiFlightController_Deinit();
|
||||
returnCode = DjiFlightController_DeInit();
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Deinit flight controller module failed, error code:0x%08llX",
|
||||
returnCode);
|
||||
@ -325,25 +326,25 @@ void DjiTest_FlightControlPositionControlSample()
|
||||
USER_LOG_INFO("Successful take off\r\n");
|
||||
DjiTest_WidgetLogAppend("Successful take off\r\n");
|
||||
|
||||
USER_LOG_INFO("--> Step 3: Move to north:0(m), earth:6(m), up:6(m) , yaw:30(degree) from current point");
|
||||
DjiTest_WidgetLogAppend("--> Step 3: Move to north:0(m), earth:6(m), up:6(m) , yaw:30(degree) from current point");
|
||||
USER_LOG_INFO("--> Step 3: Move to north:0(m), east:6(m), up:6(m) , yaw:30(degree) from current point");
|
||||
DjiTest_WidgetLogAppend("--> Step 3: Move to north:0(m), east:6(m), up:6(m) , yaw:30(degree) from current point");
|
||||
if (!DjiTest_FlightControlMoveByPositionOffset((T_DjiTestFlightControlVector3f) {0, 6, 6}, 30, 0.8, 1)) {
|
||||
USER_LOG_ERROR("Move to north:0(m), earth:6(m), up:6(m) , yaw:30(degree) from current point failed");
|
||||
USER_LOG_ERROR("Move to north:0(m), east:6(m), up:6(m) , yaw:30(degree) from current point failed");
|
||||
goto out;
|
||||
};
|
||||
|
||||
USER_LOG_INFO("--> Step 4: Move to north:6(m), earth:0(m), up:-3(m) , yaw:-30(degree) from current point");
|
||||
USER_LOG_INFO("--> Step 4: Move to north:6(m), east:0(m), up:-3(m) , yaw:-30(degree) from current point");
|
||||
DjiTest_WidgetLogAppend(
|
||||
"--> Step 4: Move to north:6(m), earth:0(m), up:-3(m) , yaw:-30(degree) from current point");
|
||||
"--> Step 4: Move to north:6(m), east:0(m), up:-3(m) , yaw:-30(degree) from current point");
|
||||
if (!DjiTest_FlightControlMoveByPositionOffset((T_DjiTestFlightControlVector3f) {6, 0, -3}, -30, 0.8, 1)) {
|
||||
USER_LOG_ERROR("Move to north:6(m), earth:0(m), up:-3(m) , yaw:-30(degree) from current point failed");
|
||||
USER_LOG_ERROR("Move to north:6(m), east:0(m), up:-3(m) , yaw:-30(degree) from current point failed");
|
||||
goto out;
|
||||
};
|
||||
|
||||
USER_LOG_INFO("--> Step 5: Move to north:-6(m), earth:-6(m), up:0(m) , yaw:0(degree) from current point");
|
||||
DjiTest_WidgetLogAppend("--> Step 5: Move to north:-6(m), earth:-6(m), up:0(m) , yaw:0(degree) from current point");
|
||||
USER_LOG_INFO("--> Step 5: Move to north:-6(m), east:-6(m), up:0(m) , yaw:0(degree) from current point");
|
||||
DjiTest_WidgetLogAppend("--> Step 5: Move to north:-6(m), east:-6(m), up:0(m) , yaw:0(degree) from current point");
|
||||
if (!DjiTest_FlightControlMoveByPositionOffset((T_DjiTestFlightControlVector3f) {-6, -6, 0}, 0, 0.8, 1)) {
|
||||
USER_LOG_ERROR("Move to north:-6(m), earth:-6(m), up:0(m) , yaw:0(degree) from current point failed");
|
||||
USER_LOG_ERROR("Move to north:-6(m), east:-6(m), up:0(m) , yaw:0(degree) from current point failed");
|
||||
goto out;
|
||||
}
|
||||
|
||||
@ -394,17 +395,17 @@ void DjiTest_FlightControlGoHomeForceLandingSample()
|
||||
USER_LOG_INFO("Successful take off\r\n");
|
||||
DjiTest_WidgetLogAppend("Successful take off\r\n");
|
||||
|
||||
USER_LOG_INFO("--> Step 3: Move to north:0(m), earth:0(m), up:30(m) , yaw:0(degree) from current point");
|
||||
DjiTest_WidgetLogAppend("--> Step 3: Move to north:0(m), earth:0(m), up:30(m) , yaw:0(degree) from current point");
|
||||
USER_LOG_INFO("--> Step 3: Move to north:0(m), east:0(m), up:30(m) , yaw:0(degree) from current point");
|
||||
DjiTest_WidgetLogAppend("--> Step 3: Move to north:0(m), east:0(m), up:30(m) , yaw:0(degree) from current point");
|
||||
if (!DjiTest_FlightControlMoveByPositionOffset((T_DjiTestFlightControlVector3f) {0, 0, 30}, 0, 0.8, 1)) {
|
||||
USER_LOG_ERROR("Move to north:0(m), earth:0(m), up:30(m) , yaw:0(degree) from current point failed");
|
||||
USER_LOG_ERROR("Move to north:0(m), east:0(m), up:30(m) , yaw:0(degree) from current point failed");
|
||||
goto out;
|
||||
}
|
||||
|
||||
USER_LOG_INFO("--> Step 4: Move to north:10(m), earth:0(m), up:0(m) , yaw:0(degree) from current point");
|
||||
DjiTest_WidgetLogAppend("--> Step 4: Move to north:10(m), earth:0(m), up:0(m) , yaw:0(degree) from current point");
|
||||
USER_LOG_INFO("--> Step 4: Move to north:10(m), east:0(m), up:0(m) , yaw:0(degree) from current point");
|
||||
DjiTest_WidgetLogAppend("--> Step 4: Move to north:10(m), east:0(m), up:0(m) , yaw:0(degree) from current point");
|
||||
if (!DjiTest_FlightControlMoveByPositionOffset((T_DjiTestFlightControlVector3f) {10, 0, 0}, 0, 0.8, 1)) {
|
||||
USER_LOG_ERROR("Move to north:10(m), earth:0(m), up:0(m) , yaw:0(degree) from current point failed");
|
||||
USER_LOG_ERROR("Move to north:10(m), east:0(m), up:0(m) , yaw:0(degree) from current point failed");
|
||||
goto out;
|
||||
}
|
||||
|
||||
@ -434,10 +435,10 @@ void DjiTest_FlightControlGoHomeForceLandingSample()
|
||||
USER_LOG_INFO("Current go home altitude is %d m\r\n", goHomeAltitude);
|
||||
DjiTest_WidgetLogAppend("Current go home altitude is %d m\r\n", goHomeAltitude);
|
||||
|
||||
USER_LOG_INFO("--> Step 7: Move to north:20(m), earth:0(m), up:0(m) , yaw:0(degree) from current point");
|
||||
DjiTest_WidgetLogAppend("--> Step 7: Move to north:20(m), earth:0(m), up:0(m) , yaw:0(degree) from current point");
|
||||
USER_LOG_INFO("--> Step 7: Move to north:20(m), east:0(m), up:0(m) , yaw:0(degree) from current point");
|
||||
DjiTest_WidgetLogAppend("--> Step 7: Move to north:20(m), east:0(m), up:0(m) , yaw:0(degree) from current point");
|
||||
if (!DjiTest_FlightControlMoveByPositionOffset((T_DjiTestFlightControlVector3f) {20, 0, 0}, 0, 0.8, 1)) {
|
||||
USER_LOG_ERROR("Move to north:20(m), earth:0(m), up:0(m) , yaw:0(degree) from current point failed");
|
||||
USER_LOG_ERROR("Move to north:20(m), east:0(m), up:0(m) , yaw:0(degree) from current point failed");
|
||||
goto out;
|
||||
}
|
||||
|
||||
@ -489,9 +490,9 @@ void DjiTest_FlightControlVelocityControlSample()
|
||||
DjiTest_WidgetLogAppend("Successful take off\r\n");
|
||||
|
||||
USER_LOG_INFO(
|
||||
"--> Step 3: Move with north:0(m/s), earth:0(m/s), up:5(m/s), yaw:0(deg/s) from current point for 2s!");
|
||||
"--> Step 3: Move with north:0(m/s), east:0(m/s), up:5(m/s), yaw:0(deg/s) from current point for 2s!");
|
||||
DjiTest_WidgetLogAppend(
|
||||
"--> Step 3: Move with north:0(m/s), earth:0(m/s), up:5(m/s), yaw:0(deg/s) from current point for 2s!");
|
||||
"--> Step 3: Move with north:0(m/s), east:0(m/s), up:5(m/s), yaw:0(deg/s) from current point for 2s!");
|
||||
DjiTest_FlightControlVelocityAndYawRateCtrl((T_DjiTestFlightControlVector3f) {0, 0, 5.0}, 0, 2000);
|
||||
|
||||
USER_LOG_INFO("--> Step 4: Emergency brake for 2s");
|
||||
@ -509,9 +510,9 @@ void DjiTest_FlightControlVelocityControlSample()
|
||||
}
|
||||
|
||||
USER_LOG_INFO(
|
||||
"--> Step 5: Move with north:-1.5(m/s), earth:2(m/s), up:0(m/s), yaw:20(deg/s) from current point for 2s!");
|
||||
"--> Step 5: Move with north:-1.5(m/s), east:2(m/s), up:0(m/s), yaw:20(deg/s) from current point for 2s!");
|
||||
DjiTest_WidgetLogAppend(
|
||||
"--> Step 5: Move with north:-1.5(m/s), earth:2(m/s), up:0(m/s), yaw:20(deg/s) from current point for 2s!");
|
||||
"--> Step 5: Move with north:-1.5(m/s), east:2(m/s), up:0(m/s), yaw:20(deg/s) from current point for 2s!");
|
||||
DjiTest_FlightControlVelocityAndYawRateCtrl((T_DjiTestFlightControlVector3f) {-1.5, 2, 0}, 20, 2000);
|
||||
|
||||
USER_LOG_INFO("--> Step 6: Emergency brake for 2s");
|
||||
@ -529,9 +530,9 @@ void DjiTest_FlightControlVelocityControlSample()
|
||||
}
|
||||
|
||||
USER_LOG_INFO(
|
||||
"--> Step 7: Move with north:3(m/s), earth:0(m/s), up:0(m/s), yaw:0(deg/s) from current point for 2.5s!");
|
||||
"--> Step 7: Move with north:3(m/s), east:0(m/s), up:0(m/s), yaw:0(deg/s) from current point for 2.5s!");
|
||||
DjiTest_WidgetLogAppend(
|
||||
"--> Step 7: Move with north:3(m/s), earth:0(m/s), up:0(m/s), yaw:0(deg/s) from current point for 2.5s!");
|
||||
"--> Step 7: Move with north:3(m/s), east:0(m/s), up:0(m/s), yaw:0(deg/s) from current point for 2.5s!");
|
||||
DjiTest_FlightControlVelocityAndYawRateCtrl((T_DjiTestFlightControlVector3f) {3, 0, 0}, 0, 2500);
|
||||
|
||||
USER_LOG_INFO("--> Step 8: Emergency brake for 2s");
|
||||
@ -549,9 +550,9 @@ void DjiTest_FlightControlVelocityControlSample()
|
||||
}
|
||||
|
||||
USER_LOG_INFO(
|
||||
"--> Step 9: Move with north:-1.6(m/s), earth:-2(m/s), up:0(m/s), yaw:0(deg/s) from current point for 2.2s!");
|
||||
"--> Step 9: Move with north:-1.6(m/s), east:-2(m/s), up:0(m/s), yaw:0(deg/s) from current point for 2.2s!");
|
||||
DjiTest_WidgetLogAppend(
|
||||
"--> Step 9: Move with north:-1.6(m/s), earth:-2(m/s), up:0(m/s), yaw:0(deg/s) from current point for 2.2s!");
|
||||
"--> Step 9: Move with north:-1.6(m/s), east:-2(m/s), up:0(m/s), yaw:0(deg/s) from current point for 2.2s!");
|
||||
DjiTest_FlightControlVelocityAndYawRateCtrl((T_DjiTestFlightControlVector3f) {-1.6, -2, 0}, 0, 2200);
|
||||
|
||||
USER_LOG_INFO("--> Step 10: Emergency brake for 2s");
|
||||
@ -669,6 +670,7 @@ void DjiTest_FlightControlSetGetParamSample()
|
||||
E_DjiFlightControllerRtkPositionEnableStatus rtkEnableStatus;
|
||||
E_DjiFlightControllerRCLostAction rcLostAction;
|
||||
T_DjiAircraftInfoBaseInfo aircraftInfoBaseInfo;
|
||||
uint16_t countryCode;
|
||||
|
||||
returnCode = DjiAircraftInfo_GetBaseInfo(&aircraftInfoBaseInfo);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
@ -678,6 +680,12 @@ void DjiTest_FlightControlSetGetParamSample()
|
||||
USER_LOG_INFO("Flight control set-get-param sample start");
|
||||
DjiTest_WidgetLogAppend("Flight control set-get-param sample start");
|
||||
|
||||
returnCode = DjiFlightController_GetCountryCode(&countryCode);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Get aircraft country code error.");
|
||||
}
|
||||
USER_LOG_INFO("country code: %hd", countryCode);
|
||||
|
||||
/*! Turn on horizontal vision avoid enable */
|
||||
USER_LOG_INFO("--> Step 1: Turn on horizontal visual obstacle avoidance");
|
||||
DjiTest_WidgetLogAppend("--> Step 1: Turn on horizontal visual obstacle avoidance");
|
||||
@ -881,6 +889,97 @@ out:
|
||||
DjiTest_WidgetLogAppend("Flight control set-get-param sample end");
|
||||
}
|
||||
|
||||
T_DjiReturnCode DjiTest_TriggerFtsEventCallback(void)
|
||||
{
|
||||
USER_LOG_INFO("Received FTS Trigger event, count = %d.", s_ftsTriggerCount);
|
||||
if (s_ftsTriggerCount == 0) {
|
||||
USER_LOG_WARN("Note: Simulate a trigger failure scenario and return an error value, this function will be invoked again.");
|
||||
s_ftsTriggerCount++;
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SYSTEM_ERROR;
|
||||
} else {
|
||||
USER_LOG_WARN("Note: This is an empty implementation, and the FTS signal needs to be triggered by the PWM signal.");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
}
|
||||
|
||||
void DjiTest_FlightControlPassiveTriggerFtsSample(void)
|
||||
{
|
||||
T_DjiReturnCode returnCode;
|
||||
|
||||
USER_LOG_INFO("Flight control passive trigger FTS sample start.");
|
||||
|
||||
if (s_isFtsCallbackRegistered == false) {
|
||||
returnCode = DjiFlightController_RegTriggerFtsEventCallback(DjiTest_TriggerFtsEventCallback);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Register trigger FTS event callback failed.");
|
||||
return;
|
||||
} else {
|
||||
s_isFtsCallbackRegistered = true;
|
||||
USER_LOG_INFO("Register trigger FTS event callback successfully."
|
||||
"Please wait for the aircraft to trigger the payload to execute FTS action.");
|
||||
}
|
||||
} else {
|
||||
USER_LOG_WARN("FTS trigger event callback has been registered, no need to register again.");
|
||||
}
|
||||
}
|
||||
|
||||
static void DjiTest_FlightControlSlowRotateMotorSample(void)
|
||||
{
|
||||
E_DjiFlightControllerElectronicSpeedControllerStatus escStatus;
|
||||
|
||||
T_DjiReturnCode returnCode = 0;
|
||||
|
||||
USER_LOG_INFO("Start rotating.");
|
||||
returnCode = DjiFlightController_StartSlowRotateMotor();
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Start slow rotate blade failed, error code is 0x%08X", returnCode);
|
||||
return ;
|
||||
}
|
||||
|
||||
returnCode = DjiFlightController_GetElectronicSpeedControllerStatus(&escStatus);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Fail to get ESC status, error code is 0x%08X", returnCode);
|
||||
}
|
||||
|
||||
USER_LOG_INFO("The ESC status is: %s motor(s) in rotate mode",
|
||||
escStatus == DJI_FLIGHT_CONTROLLER_NO_MOTOR_IN_SLOW_ROTATE_MODE ? "no" :
|
||||
escStatus == DJI_FLIGHT_CONTROLLER_SOME_MOTOR_IN_SLOW_ROTATE_MODE ? "some" :
|
||||
escStatus == DJI_FLIGHT_CONTROLLER_ALL_MOTOR_IN_SLOW_ROTATE_MODE ? "all" : "(error)");
|
||||
|
||||
for (int32_t i = 0; i < 8; i++) {
|
||||
s_osalHandler->TaskSleepMs(1000);
|
||||
|
||||
returnCode = DjiFlightController_GetElectronicSpeedControllerStatus(&escStatus);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Fail to get ESC status, error code is 0x%08X", returnCode);
|
||||
}
|
||||
|
||||
USER_LOG_INFO("The ESC status is: %s motor(s) in rotate mode",
|
||||
escStatus == DJI_FLIGHT_CONTROLLER_NO_MOTOR_IN_SLOW_ROTATE_MODE ? "no" :
|
||||
escStatus == DJI_FLIGHT_CONTROLLER_SOME_MOTOR_IN_SLOW_ROTATE_MODE ? "some" :
|
||||
escStatus == DJI_FLIGHT_CONTROLLER_ALL_MOTOR_IN_SLOW_ROTATE_MODE ? "all" : "(error)");
|
||||
}
|
||||
|
||||
USER_LOG_INFO("Stop rotating.");
|
||||
returnCode = DjiFlightController_StopSlowRotateMotor();
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_INFO("Stop slow rotate blade failed, error code is 0x%08X", returnCode);
|
||||
return ;
|
||||
}
|
||||
|
||||
s_osalHandler->TaskSleepMs(1000);
|
||||
|
||||
returnCode = DjiFlightController_GetElectronicSpeedControllerStatus(&escStatus);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Fail to get ESC status, error code is 0x%08X", returnCode);
|
||||
}
|
||||
|
||||
USER_LOG_INFO("The ESC status is: %s motor(s) in rotate mode",
|
||||
escStatus == DJI_FLIGHT_CONTROLLER_NO_MOTOR_IN_SLOW_ROTATE_MODE ? "no" :
|
||||
escStatus == DJI_FLIGHT_CONTROLLER_SOME_MOTOR_IN_SLOW_ROTATE_MODE ? "some" :
|
||||
escStatus == DJI_FLIGHT_CONTROLLER_ALL_MOTOR_IN_SLOW_ROTATE_MODE ? "all" : "(error)");
|
||||
}
|
||||
|
||||
void DjiTest_FlightControlSample(E_DjiTestFlightCtrlSampleSelect flightCtrlSampleSelect)
|
||||
{
|
||||
switch (flightCtrlSampleSelect) {
|
||||
@ -908,6 +1007,14 @@ void DjiTest_FlightControlSample(E_DjiTestFlightCtrlSampleSelect flightCtrlSampl
|
||||
DjiTest_FlightControlSetGetParamSample();
|
||||
break;
|
||||
}
|
||||
case E_DJI_TEST_FLIGHT_CTRL_SAMPLE_SELECT_FTS_TRIGGER: {
|
||||
DjiTest_FlightControlPassiveTriggerFtsSample();
|
||||
break;
|
||||
}
|
||||
case E_DJI_TEST_FLIGHT_CTRL_SAMPLE_SELECT_SLOW_ROTATE_BLADE: {
|
||||
DjiTest_FlightControlSlowRotateMotorSample();
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
@ -1218,6 +1325,12 @@ bool DjiTest_FlightControlGoHomeAndConfirmLanding(void)
|
||||
{
|
||||
T_DjiReturnCode djiStat;
|
||||
T_DjiAircraftInfoBaseInfo aircraftInfoBaseInfo;
|
||||
E_DjiFlightControllerObstacleAvoidanceEnableStatus enableStatus;
|
||||
|
||||
djiStat = DjiFlightController_GetDownwardsVisualObstacleAvoidanceEnableStatus(&enableStatus);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("get downwards visual obstacle avoidance enable status error");
|
||||
}
|
||||
|
||||
djiStat = DjiAircraftInfo_GetBaseInfo(&aircraftInfoBaseInfo);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
@ -1251,8 +1364,16 @@ bool DjiTest_FlightControlGoHomeAndConfirmLanding(void)
|
||||
DjiTest_FlightControlGetValueOfFlightStatus() == DJI_FC_SUBSCRIPTION_FLIGHT_STATUS_IN_AIR) {
|
||||
T_DjiFcSubscriptionHeightFusion heightFusion = DjiTest_FlightControlGetValueOfHeightFusion();
|
||||
s_osalHandler->TaskSleepMs(1000);
|
||||
if (aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M3E ||
|
||||
aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M3T) {
|
||||
if (DJI_AIRCRAFT_TYPE_M3E == aircraftInfoBaseInfo.aircraftType || DJI_AIRCRAFT_TYPE_M3T == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M3D == aircraftInfoBaseInfo.aircraftType || DJI_AIRCRAFT_TYPE_M3TD == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M3TA == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M4T == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M4TD == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M4D == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M4E == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M4TD == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M4D == aircraftInfoBaseInfo.aircraftType
|
||||
) {
|
||||
if ((dji_f64_t) 0.45 < heightFusion && heightFusion < (dji_f64_t) 0.55) {
|
||||
break;
|
||||
}
|
||||
@ -1272,11 +1393,20 @@ bool DjiTest_FlightControlGoHomeAndConfirmLanding(void)
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!DjiTest_FlightControlCheckActionStarted(DJI_FC_SUBSCRIPTION_DISPLAY_MODE_FORCE_AUTO_LANDING)) {
|
||||
return false;
|
||||
if (enableStatus == DJI_FLIGHT_CONTROLLER_ENABLE_OBSTACLE_AVOIDANCE) {
|
||||
if (!DjiTest_FlightControlCheckActionStarted(DJI_FC_SUBSCRIPTION_DISPLAY_MODE_FORCE_AUTO_LANDING)) {
|
||||
return false;
|
||||
} else {
|
||||
while (DjiTest_FlightControlGetValueOfFlightStatus() == DJI_FC_SUBSCRIPTION_FLIGHT_STATUS_IN_AIR &&
|
||||
DjiTest_FlightControlGetValueOfDisplayMode() ==
|
||||
DJI_FC_SUBSCRIPTION_DISPLAY_MODE_FORCE_AUTO_LANDING) {
|
||||
s_osalHandler->TaskSleepMs(1000);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
while (DjiTest_FlightControlGetValueOfFlightStatus() == DJI_FC_SUBSCRIPTION_FLIGHT_STATUS_IN_AIR &&
|
||||
DjiTest_FlightControlGetValueOfDisplayMode() == DJI_FC_SUBSCRIPTION_DISPLAY_MODE_FORCE_AUTO_LANDING) {
|
||||
DjiTest_FlightControlGetValueOfDisplayMode() ==
|
||||
DJI_FC_SUBSCRIPTION_DISPLAY_MODE_FORCE_AUTO_LANDING) {
|
||||
s_osalHandler->TaskSleepMs(1000);
|
||||
}
|
||||
}
|
||||
@ -1454,11 +1584,12 @@ DjiTest_FlightControlMoveByPositionOffset(const T_DjiTestFlightControlVector3f o
|
||||
void DjiTest_FlightControlVelocityAndYawRateCtrl(const T_DjiTestFlightControlVector3f offsetDesired, float yawRate,
|
||||
uint32_t timeMs)
|
||||
{
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
uint32_t originTime = 0;
|
||||
uint32_t currentTime = 0;
|
||||
uint32_t elapsedTimeInMs = 0;
|
||||
s_osalHandler->GetTimeMs(&originTime);
|
||||
s_osalHandler->GetTimeMs(¤tTime);
|
||||
osalHandler->GetTimeMs(&originTime);
|
||||
osalHandler->GetTimeMs(¤tTime);
|
||||
elapsedTimeInMs = currentTime - originTime;
|
||||
T_DjiFlightControllerJoystickMode joystickMode = {
|
||||
DJI_FLIGHT_CONTROLLER_HORIZONTAL_VELOCITY_CONTROL_MODE,
|
||||
@ -1474,8 +1605,8 @@ void DjiTest_FlightControlVelocityAndYawRateCtrl(const T_DjiTestFlightControlVec
|
||||
|
||||
while (elapsedTimeInMs <= timeMs) {
|
||||
DjiFlightController_ExecuteJoystickAction(joystickCommand);
|
||||
s_osalHandler->TaskSleepMs(2);
|
||||
s_osalHandler->GetTimeMs(¤tTime);
|
||||
osalHandler->TaskSleepMs(2);
|
||||
osalHandler->GetTimeMs(¤tTime);
|
||||
elapsedTimeInMs = currentTime - originTime;
|
||||
}
|
||||
}
|
||||
@ -1542,4 +1673,57 @@ DjiTest_FlightControlJoystickCtrlAuthSwitchEventCallback(T_DjiFlightControllerJo
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
static T_DjiReturnCode DjiTest_FlightControlSetFtsTrigger(E_DjiMountPosition position, const char* desc)
|
||||
{
|
||||
T_DjiReturnCode djiStat;
|
||||
T_DjiFtsPwmEscTriggerStatus esc_status;
|
||||
|
||||
djiStat = DjiFlightController_SelectFtsPwmTrigger(position);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("select fts pwm trigger E-PORT error, error code: 0x%08X", djiStat);
|
||||
return djiStat;
|
||||
}
|
||||
|
||||
djiStat = DjiFlightController_GetFtsPwmTriggerStatus(&esc_status);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("get pwm trigger status error, error code: 0x%08X", djiStat);
|
||||
return djiStat;
|
||||
}
|
||||
|
||||
if (esc_status.ESC[0].fts_select != position || esc_status.ESC[1].fts_select != position)
|
||||
{
|
||||
USER_LOG_ERROR("pwm trigger status incorrect");
|
||||
return djiStat;
|
||||
}
|
||||
|
||||
if (esc_status.ESC[0].fts_status == DJI_FLIGHT_CONTROLLER_FTS_NOT_TRIGGERD && esc_status.ESC[1].fts_status == DJI_FLIGHT_CONTROLLER_FTS_NOT_TRIGGERD)
|
||||
|
||||
USER_LOG_INFO("Set fts trigger position %s success", desc);
|
||||
|
||||
return djiStat;
|
||||
}
|
||||
|
||||
T_DjiReturnCode DjiTest_FlightControlFtsPwmTriggerSample(E_DjiMountPosition position, const char* port_name)
|
||||
{
|
||||
T_DjiReturnCode returnCode;
|
||||
|
||||
returnCode = DjiTest_FlightControlInit();
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Init flight Control sample failed,error code:0x%08llX", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
returnCode = DjiTest_FlightControlSetFtsTrigger(position, port_name);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Test select %s fts pwm trigger failed", port_name);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
returnCode = DjiTest_FlightControlDeInit();
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Deinit Flight Control sample failed,error code:0x%08llX", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/
|
||||
|
||||
@ -42,12 +42,25 @@ typedef enum {
|
||||
E_DJI_TEST_FLIGHT_CTRL_SAMPLE_SELECT_TAKE_OFF_VELOCITY_CTRL_LANDING,
|
||||
E_DJI_TEST_FLIGHT_CTRL_SAMPLE_SELECT_ARREST_FLYING,
|
||||
E_DJI_TEST_FLIGHT_CTRL_SAMPLE_SELECT_SET_GET_PARAM,
|
||||
E_DJI_TEST_FLIGHT_CTRL_SAMPLE_SELECT_FTS_TRIGGER,
|
||||
E_DJI_TEST_FLIGHT_CTRL_SAMPLE_SELECT_SLOW_ROTATE_BLADE,
|
||||
} E_DjiTestFlightCtrlSampleSelect;
|
||||
|
||||
#pragma pack(1)
|
||||
typedef struct {
|
||||
dji_f32_t x;
|
||||
dji_f32_t y;
|
||||
dji_f32_t z;
|
||||
} T_DjiTestFlightControlVector3f; // pack(1)
|
||||
#pragma pack()
|
||||
|
||||
/* Exported types ------------------------------------------------------------*/
|
||||
|
||||
/* Exported functions --------------------------------------------------------*/
|
||||
T_DjiReturnCode DjiTest_FlightControlRunSample(E_DjiTestFlightCtrlSampleSelect flightCtrlSampleSelect);
|
||||
void DjiTest_FlightControlVelocityAndYawRateCtrl(const T_DjiTestFlightControlVector3f offsetDesired, float yawRate,
|
||||
uint32_t timeMs);
|
||||
T_DjiReturnCode DjiTest_FlightControlFtsPwmTriggerSample(E_DjiMountPosition position, const char* port_name);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
@ -30,6 +30,7 @@
|
||||
#include "dji_logger.h"
|
||||
#include "dji_platform.h"
|
||||
#include "utils/util_misc.h"
|
||||
#include "widget_interaction_test/test_widget_interaction.h"
|
||||
|
||||
/* Private constants ---------------------------------------------------------*/
|
||||
#define PAYLOAD_GIMBAL_EMU_TASK_STACK_SIZE (2048)
|
||||
@ -277,6 +278,7 @@ T_DjiReturnCode DjiTest_GimbalRotate(E_DjiGimbalRotationMode rotationMode,
|
||||
|
||||
break;
|
||||
case DJI_GIMBAL_ROTATION_MODE_ABSOLUTE_ANGLE:
|
||||
DjiTest_WidgetLogAppend("abs mode (%d %d %d)", rotationValue.pitch, rotationValue.roll, rotationValue.yaw);
|
||||
USER_LOG_INFO("gimbal absolute rotate angle: pitch %d, roll %d, yaw %d.", rotationValue.pitch,
|
||||
rotationValue.roll, rotationValue.yaw);
|
||||
USER_LOG_DEBUG("gimbal absolute rotate action time: %d.",
|
||||
@ -325,6 +327,7 @@ T_DjiReturnCode DjiTest_GimbalRotate(E_DjiGimbalRotationMode rotationMode,
|
||||
|
||||
break;
|
||||
case DJI_GIMBAL_ROTATION_MODE_SPEED:
|
||||
DjiTest_WidgetLogAppend("speed mode (%d %d %d)", rotationValue.pitch, rotationValue.roll, rotationValue.yaw);
|
||||
USER_LOG_INFO("gimbal rotate speed: pitch %d, roll %d, yaw %d.", rotationValue.pitch,
|
||||
rotationValue.roll, rotationValue.yaw);
|
||||
|
||||
|
||||
@ -29,6 +29,8 @@
|
||||
#include "dji_platform.h"
|
||||
#include "dji_logger.h"
|
||||
#include "dji_gimbal_manager.h"
|
||||
#include "dji_fc_subscription.h"
|
||||
#include "dji_aircraft_info.h"
|
||||
|
||||
/* Private constants ---------------------------------------------------------*/
|
||||
|
||||
@ -73,10 +75,34 @@ T_DjiReturnCode DjiTest_GimbalManagerRunSample(E_DjiMountPosition mountPosition,
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
T_DjiReturnCode returnCode;
|
||||
T_DjiGimbalManagerRotation rotation;
|
||||
T_DjiAircraftInfoBaseInfo baseInfo;
|
||||
E_DjiAircraftSeries aircraftSeries;
|
||||
bool gimbalAnglesSubscribedFlag = false;
|
||||
|
||||
returnCode = DjiAircraftInfo_GetBaseInfo(&baseInfo);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Failed to get aircraft base info, return code 0x%08X", returnCode);
|
||||
goto out;
|
||||
}
|
||||
|
||||
aircraftSeries = baseInfo.aircraftSeries;
|
||||
|
||||
USER_LOG_INFO("Gimbal manager sample start");
|
||||
DjiTest_WidgetLogAppend("Gimbal manager sample start");
|
||||
|
||||
if (DJI_AIRCRAFT_SERIES_M30 == aircraftSeries ||
|
||||
DJI_AIRCRAFT_SERIES_M3 == aircraftSeries ||
|
||||
DJI_AIRCRAFT_SERIES_M3D == aircraftSeries ||
|
||||
DJI_AIRCRAFT_SERIES_M4 == aircraftSeries ||
|
||||
DJI_AIRCRAFT_SERIES_M4D == aircraftSeries) {
|
||||
returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GIMBAL_ANGLES, DJI_DATA_SUBSCRIPTION_TOPIC_50_HZ, NULL);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Failed to subscribe topic %d, 0x%08X", DJI_FC_SUBSCRIPTION_TOPIC_GIMBAL_ANGLES, returnCode);
|
||||
goto out;
|
||||
}
|
||||
gimbalAnglesSubscribedFlag = true;
|
||||
}
|
||||
|
||||
USER_LOG_INFO("--> Step 1: Init gimbal manager module");
|
||||
DjiTest_WidgetLogAppend("--> Step 1: Init gimbal manager module");
|
||||
returnCode = DjiGimbalManager_Init();
|
||||
@ -99,7 +125,7 @@ T_DjiReturnCode DjiTest_GimbalManagerRunSample(E_DjiMountPosition mountPosition,
|
||||
}
|
||||
|
||||
USER_LOG_INFO("--> Step 3: Reset gimbal angles.\r\n");
|
||||
returnCode = DjiGimbalManager_Reset(mountPosition);
|
||||
returnCode = DjiGimbalManager_Reset(mountPosition, DJI_GIMBAL_RESET_MODE_PITCH_AND_YAW);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Reset gimbal failed, error code: 0x%08X", returnCode);
|
||||
}
|
||||
@ -108,7 +134,7 @@ T_DjiReturnCode DjiTest_GimbalManagerRunSample(E_DjiMountPosition mountPosition,
|
||||
for (int i = 0; i < sizeof(s_rotationActionList) / sizeof(T_DjiTestGimbalActionList); ++i) {
|
||||
if (s_rotationActionList[i].action == DJI_TEST_GIMBAL_RESET) {
|
||||
USER_LOG_INFO("Target gimbal reset.\r\n");
|
||||
returnCode = DjiGimbalManager_Reset(mountPosition);
|
||||
returnCode = DjiGimbalManager_Reset(mountPosition, DJI_GIMBAL_RESET_MODE_PITCH_AND_YAW);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Reset gimbal failed, error code: 0x%08X", returnCode);
|
||||
}
|
||||
@ -120,11 +146,27 @@ T_DjiReturnCode DjiTest_GimbalManagerRunSample(E_DjiMountPosition mountPosition,
|
||||
continue;
|
||||
}
|
||||
|
||||
USER_LOG_INFO("Target gimbal pry = (%.1f, %.1f, %.1f)",
|
||||
s_rotationActionList[i].rotation.pitch, s_rotationActionList[i].rotation.roll,
|
||||
s_rotationActionList[i].rotation.yaw);
|
||||
|
||||
rotation = s_rotationActionList[i].rotation;
|
||||
|
||||
if (DJI_AIRCRAFT_SERIES_M30 == aircraftSeries
|
||||
|| DJI_AIRCRAFT_SERIES_M3 == aircraftSeries
|
||||
|| DJI_AIRCRAFT_SERIES_M3D == aircraftSeries
|
||||
|| DJI_AIRCRAFT_SERIES_M4 == aircraftSeries
|
||||
|| DJI_AIRCRAFT_SERIES_M4D == aircraftSeries
|
||||
) {
|
||||
if (s_rotationActionList[i].rotation.rotationMode == DJI_GIMBAL_ROTATION_MODE_ABSOLUTE_ANGLE) {
|
||||
T_DjiFcSubscriptionGimbalAngles gimbalAngles = {0};
|
||||
T_DjiDataTimestamp timestamp = {0};
|
||||
returnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GIMBAL_ANGLES,
|
||||
(uint8_t *) &gimbalAngles,
|
||||
sizeof(T_DjiFcSubscriptionGimbalAngles),
|
||||
×tamp);
|
||||
rotation.yaw = gimbalAngles.z;
|
||||
}
|
||||
}
|
||||
|
||||
USER_LOG_INFO("Target gimbal pry = (%.1f, %.1f, %.1f)", rotation.pitch, rotation.roll, rotation.yaw);
|
||||
|
||||
returnCode = DjiGimbalManager_Rotate(mountPosition, rotation);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Target gimbal pry = (%.1f, %.1f, %.1f) failed, error code: 0x%08X",
|
||||
@ -136,6 +178,14 @@ T_DjiReturnCode DjiTest_GimbalManagerRunSample(E_DjiMountPosition mountPosition,
|
||||
}
|
||||
}
|
||||
|
||||
if (gimbalAnglesSubscribedFlag) {
|
||||
returnCode = DjiFcSubscription_UnSubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GIMBAL_ANGLES);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Failed to unsubscribe topic %d, 0x%08X", DJI_FC_SUBSCRIPTION_TOPIC_GIMBAL_ANGLES, returnCode);
|
||||
}
|
||||
gimbalAnglesSubscribedFlag = false;
|
||||
}
|
||||
|
||||
USER_LOG_INFO("--> Step 5: Deinit gimbal manager module");
|
||||
DjiTest_WidgetLogAppend("--> Step 5: Deinit gimbal manager module");
|
||||
returnCode = DjiGimbalManager_Deinit();
|
||||
|
||||
32210
samples/sample_c/module_sample/hms/data/hms.json
Normal file
32210
samples/sample_c/module_sample/hms/data/hms.json
Normal file
File diff suppressed because it is too large
Load Diff
@ -25,6 +25,8 @@
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include <widget_interaction_test/test_widget_interaction.h>
|
||||
#include <utils/util_misc.h>
|
||||
#include <utils/cJSON.h>
|
||||
#include <utils/util_file.h>
|
||||
#include "test_hms.h"
|
||||
#include "dji_hms.h"
|
||||
#include "dji_hms_info_table.h"
|
||||
@ -41,7 +43,11 @@
|
||||
#define MAX_HMS_ERROR_LEVEL (6)
|
||||
#define HMS_DIR_PATH_LEN_MAX (256)
|
||||
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
#define DJI_CUSTOM_HMS_CODE_INJECT_ON (0)
|
||||
#else
|
||||
#define DJI_CUSTOM_HMS_CODE_INJECT_ON (1)
|
||||
#endif
|
||||
|
||||
/* Private types -------------------------------------------------------------*/
|
||||
|
||||
@ -49,31 +55,42 @@
|
||||
static const char *oldReplaceAlarmIdStr = "%alarmid";
|
||||
static const char *oldReplaceIndexStr = "%index";
|
||||
static const char *oldReplaceComponentIndexStr = "%component_index";
|
||||
static bool isHmsManagerInit = false;
|
||||
static T_DjiHmsFileBinaryArray s_EnHmsTextConfigFileBinaryArrayList[] = {
|
||||
{hms_text_config_json_fileName, hms_text_config_json_fileSize, hms_text_config_json_fileBinaryArray},
|
||||
};
|
||||
static bool s_hmsServiceRunFlag = false;
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
static uint8_t *s_hmsJsonData = NULL;
|
||||
#endif
|
||||
static E_DjiMobileAppLanguage s_hmsLanguage = DJI_MOBILE_APP_LANGUAGE_ENGLISH;
|
||||
static bool s_isHmsConfigFileDirPathConfigured = false;
|
||||
static char s_hmsConfigFileDirPath[DJI_FILE_PATH_SIZE_MAX] = {0};
|
||||
|
||||
/* Private functions declaration ---------------------------------------------*/
|
||||
static T_DjiReturnCode DjiTest_HmsInit(void);
|
||||
static T_DjiReturnCode DjiTest_HmsDeInit(void);
|
||||
static T_DjiReturnCode DjiTest_HmsManagerInit(void);
|
||||
static T_DjiReturnCode DjiTest_HmsManagerDeInit(void);
|
||||
static T_DjiFcSubscriptionFlightStatus DjiTest_GetValueOfFlightStatus(void);
|
||||
static bool DjiTest_ReplaceStr(char *buffer, uint32_t bufferMaxLen, const char *target, const char *dest);
|
||||
static bool DjiTest_MarchErrCodeInfoTable(T_DjiHmsInfoTable hmsInfoTable);
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
static bool DjiTest_MarchErrCodeInfoTableByJson(T_DjiHmsInfoTable hmsInfoTable);
|
||||
#endif
|
||||
static T_DjiReturnCode DjiTest_HmsInfoCallback(T_DjiHmsInfoTable hmsInfoTable);
|
||||
|
||||
/* Exported functions definition ---------------------------------------------*/
|
||||
T_DjiReturnCode DjiTest_HmsRunSample(void)
|
||||
T_DjiReturnCode DjiTest_HmsManagerRunSample(E_DjiMobileAppLanguage language)
|
||||
{
|
||||
T_DjiReturnCode returnCode;
|
||||
T_DjiOsalHandler *osalHandler;
|
||||
|
||||
s_hmsLanguage = language;
|
||||
|
||||
USER_LOG_INFO("Hms Sample Start");
|
||||
DjiTest_WidgetLogAppend("Hms Sample Start");
|
||||
|
||||
USER_LOG_INFO("--> Step 1: Init hms sample");
|
||||
DjiTest_WidgetLogAppend("--> Step 1: Init hms sample");
|
||||
returnCode = DjiTest_HmsInit();
|
||||
returnCode = DjiTest_HmsManagerInit();
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Hms sample init error, error code:0x%08llX", returnCode);
|
||||
goto out;
|
||||
@ -82,7 +99,7 @@ T_DjiReturnCode DjiTest_HmsRunSample(void)
|
||||
osalHandler = DjiPlatform_GetOsalHandler();
|
||||
USER_LOG_INFO("--> Step 2: Register callback function of push HMS information");
|
||||
DjiTest_WidgetLogAppend("--> Step 2: Register callback function of push HMS information");
|
||||
returnCode = DjiHms_RegHmsInfoCallback(DjiTest_HmsInfoCallback);
|
||||
returnCode = DjiHmsManager_RegHmsInfoCallback(DjiTest_HmsInfoCallback);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Register callback function of push HMS information failed, error code:0x%08llX", returnCode);
|
||||
goto out;
|
||||
@ -93,7 +110,7 @@ T_DjiReturnCode DjiTest_HmsRunSample(void)
|
||||
out:
|
||||
USER_LOG_INFO("--> Step 3: Deinit hms sample");
|
||||
DjiTest_WidgetLogAppend("--> Step 3: Deinit hms sample");
|
||||
returnCode = DjiTest_HmsDeInit();
|
||||
returnCode = DjiTest_HmsManagerDeInit();
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Hms sample deinit error, error code:0x%08llX", returnCode);
|
||||
}
|
||||
@ -104,7 +121,7 @@ out:
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
T_DjiReturnCode DjiTest_HmsStartService(void)
|
||||
T_DjiReturnCode DjiTest_HmsCustomizationStartService(void)
|
||||
{
|
||||
T_DjiReturnCode returnCode;
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
@ -112,7 +129,7 @@ T_DjiReturnCode DjiTest_HmsStartService(void)
|
||||
char tempPath[HMS_DIR_PATH_LEN_MAX];
|
||||
#endif
|
||||
|
||||
returnCode = DjiHms_Init();
|
||||
returnCode = DjiHmsCustomization_Init();
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Hms init error, error code:0x%08llX", returnCode);
|
||||
return returnCode;
|
||||
@ -126,27 +143,36 @@ T_DjiReturnCode DjiTest_HmsStartService(void)
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
snprintf(tempPath, HMS_DIR_PATH_LEN_MAX, "%shms_text/en", curFileDirPath);
|
||||
if (s_isHmsConfigFileDirPathConfigured == true) {
|
||||
snprintf(tempPath, HMS_DIR_PATH_LEN_MAX, "%shms_text/en", s_hmsConfigFileDirPath);
|
||||
} else {
|
||||
snprintf(tempPath, HMS_DIR_PATH_LEN_MAX, "%shms_text/en", curFileDirPath);
|
||||
}
|
||||
|
||||
//set default hms text config path
|
||||
returnCode = DjiHms_RegDefaultHmsTextConfigByDirPath(tempPath);
|
||||
returnCode = DjiHmsCustomization_RegDefaultHmsTextConfigByDirPath(tempPath);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Add default hms text config error, stat = 0x%08llX", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
//set hms text config for English language
|
||||
returnCode = DjiHms_RegHmsTextConfigByDirPath(DJI_MOBILE_APP_LANGUAGE_ENGLISH,
|
||||
tempPath);
|
||||
returnCode = DjiHmsCustomization_RegHmsTextConfigByDirPath(DJI_MOBILE_APP_LANGUAGE_ENGLISH,
|
||||
tempPath);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Add hms text config error, stat = 0x%08llX", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
//set hms text config for Chinese language
|
||||
snprintf(tempPath, HMS_DIR_PATH_LEN_MAX, "%shms_text/cn", curFileDirPath);
|
||||
returnCode = DjiHms_RegHmsTextConfigByDirPath(DJI_MOBILE_APP_LANGUAGE_CHINESE,
|
||||
tempPath);
|
||||
if (s_isHmsConfigFileDirPathConfigured == true) {
|
||||
snprintf(tempPath, HMS_DIR_PATH_LEN_MAX, "%shms_text/cn", s_hmsConfigFileDirPath);
|
||||
} else {
|
||||
snprintf(tempPath, HMS_DIR_PATH_LEN_MAX, "%shms_text/cn", curFileDirPath);
|
||||
}
|
||||
|
||||
returnCode = DjiHmsCustomization_RegHmsTextConfigByDirPath(DJI_MOBILE_APP_LANGUAGE_CHINESE,
|
||||
tempPath);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Add hms text config error, stat = 0x%08llX", returnCode);
|
||||
return returnCode;
|
||||
@ -159,7 +185,7 @@ T_DjiReturnCode DjiTest_HmsStartService(void)
|
||||
};
|
||||
|
||||
//set default hms text config
|
||||
returnCode = DjiHms_RegDefaultHmsTextConfigByBinaryArray(&enHmsTextBinaryArrayConfig);
|
||||
returnCode = DjiHmsCustomization_RegDefaultHmsTextConfigByBinaryArray(&enHmsTextBinaryArrayConfig);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Add default hms text config error, stat = 0x%08llX", returnCode);
|
||||
return returnCode;
|
||||
@ -167,18 +193,34 @@ T_DjiReturnCode DjiTest_HmsStartService(void)
|
||||
#endif
|
||||
|
||||
#if DJI_CUSTOM_HMS_CODE_INJECT_ON
|
||||
DjiHms_InjectHmsErrorCode(0x1E020000, DJI_HMS_ERROR_LEVEL_FATAL);
|
||||
DjiHmsCustomization_InjectHmsErrorCode(0x1E020000, DJI_HMS_ERROR_LEVEL_FATAL);
|
||||
DjiHmsCustomization_InjectHmsErrorCode(0x1E020001, DJI_HMS_ERROR_LEVEL_CRITICAL);
|
||||
DjiHmsCustomization_InjectHmsErrorCode(0x1E020002, DJI_HMS_ERROR_LEVEL_WARN);
|
||||
DjiHmsCustomization_InjectHmsErrorCode(0x1E020003, DJI_HMS_ERROR_LEVEL_HINT);
|
||||
#endif
|
||||
|
||||
s_hmsServiceRunFlag = true;
|
||||
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
T_DjiReturnCode DjiTest_HmsCustomizationSetConfigFilePath(const char *path)
|
||||
{
|
||||
memset(s_hmsConfigFileDirPath, 0, sizeof(s_hmsConfigFileDirPath));
|
||||
memcpy(s_hmsConfigFileDirPath, path, USER_UTIL_MIN(strlen(path), sizeof(s_hmsConfigFileDirPath) - 1));
|
||||
s_isHmsConfigFileDirPathConfigured = true;
|
||||
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
/* Private functions definition-----------------------------------------------*/
|
||||
static T_DjiReturnCode DjiTest_HmsInit(void)
|
||||
static T_DjiReturnCode DjiTest_HmsManagerInit(void)
|
||||
{
|
||||
T_DjiReturnCode returnCode;
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
char curFileDirPath[HMS_DIR_PATH_LEN_MAX];
|
||||
char tempFileDirPath[HMS_DIR_PATH_LEN_MAX];
|
||||
uint32_t fileSize = 0;
|
||||
uint32_t readRealSize = 0;
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
#endif
|
||||
|
||||
returnCode = DjiFcSubscription_Init();
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
@ -190,22 +232,48 @@ static T_DjiReturnCode DjiTest_HmsInit(void)
|
||||
returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_STATUS_FLIGHT,
|
||||
DJI_DATA_SUBSCRIPTION_TOPIC_10_HZ,
|
||||
NULL);
|
||||
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("HMS sample subscribe topic flight status error, error code:0x%08llX", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
if (s_hmsServiceRunFlag == true) {
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
|
||||
returnCode = DjiUserUtil_GetCurrentFileDirPath(__FILE__, HMS_DIR_PATH_LEN_MAX, curFileDirPath);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Get file current path error, stat = 0x%08llX", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
return DjiHms_Init();
|
||||
snprintf(tempFileDirPath, HMS_DIR_PATH_LEN_MAX, "%s/data/hms.json", curFileDirPath);
|
||||
|
||||
returnCode = UtilFile_GetFileSizeByPath(tempFileDirPath, &fileSize);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Get file size by path failed, stat = 0x%08llX", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
USER_LOG_DEBUG("Hms json file size is %d", fileSize);
|
||||
|
||||
s_hmsJsonData = osalHandler->Malloc(fileSize);
|
||||
if (s_hmsJsonData == NULL) {
|
||||
USER_LOG_ERROR("Malloc failed.");
|
||||
}
|
||||
|
||||
UtilFile_GetFileDataByPath(tempFileDirPath, 0, fileSize, s_hmsJsonData, &readRealSize);
|
||||
#endif
|
||||
|
||||
isHmsManagerInit = true;
|
||||
|
||||
return DjiHmsManager_Init();
|
||||
}
|
||||
|
||||
static T_DjiReturnCode DjiTest_HmsDeInit(void)
|
||||
static T_DjiReturnCode DjiTest_HmsManagerDeInit(void)
|
||||
{
|
||||
T_DjiReturnCode returnCode;
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
#endif
|
||||
|
||||
returnCode = DjiFcSubscription_DeInit();
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
@ -214,11 +282,13 @@ static T_DjiReturnCode DjiTest_HmsDeInit(void)
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
if (s_hmsServiceRunFlag == true) {
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
osalHandler->Free(s_hmsJsonData);
|
||||
#endif
|
||||
|
||||
return DjiHms_DeInit();
|
||||
isHmsManagerInit = false;
|
||||
|
||||
return DjiHmsManager_DeInit();
|
||||
}
|
||||
|
||||
static T_DjiFcSubscriptionFlightStatus DjiTest_GetValueOfFlightStatus(void)
|
||||
@ -227,6 +297,10 @@ static T_DjiFcSubscriptionFlightStatus DjiTest_GetValueOfFlightStatus(void)
|
||||
T_DjiFcSubscriptionFlightStatus flightStatus;
|
||||
T_DjiDataTimestamp flightStatusTimestamp = {0};
|
||||
|
||||
if (isHmsManagerInit == false) {
|
||||
return DJI_FC_SUBSCRIPTION_FLIGHT_STATUS_ON_GROUND;
|
||||
}
|
||||
|
||||
returnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_STATUS_FLIGHT,
|
||||
(uint8_t *) &flightStatus,
|
||||
sizeof(T_DjiFcSubscriptionFlightStatus),
|
||||
@ -326,12 +400,71 @@ static bool DjiTest_MarchErrCodeInfoTable(T_DjiHmsInfoTable hmsInfoTable)
|
||||
return true;
|
||||
}
|
||||
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
static bool DjiTest_MarchErrCodeInfoTableByJson(T_DjiHmsInfoTable hmsInfoTable)
|
||||
{
|
||||
cJSON *hmsJsonRoot = NULL;
|
||||
cJSON *hmsErrorCode = NULL;
|
||||
cJSON *hmsLanguage = NULL;
|
||||
char hmsErrorCodeString[HMS_DIR_PATH_LEN_MAX] = {0};
|
||||
|
||||
hmsJsonRoot = cJSON_Parse((char *) s_hmsJsonData);
|
||||
if (hmsJsonRoot == NULL) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
for (int i = 0; i < hmsInfoTable.hmsInfoNum; i++) {
|
||||
if (DjiTest_GetValueOfFlightStatus() == DJI_FC_SUBSCRIPTION_FLIGHT_STATUS_IN_AIR) {
|
||||
sprintf(hmsErrorCodeString, "fpv_tip_0x%08X_in_the_sky", hmsInfoTable.hmsInfo[i].errorCode);
|
||||
} else {
|
||||
sprintf(hmsErrorCodeString, "fpv_tip_0x%08X", hmsInfoTable.hmsInfo[i].errorCode);
|
||||
}
|
||||
|
||||
hmsErrorCode = cJSON_GetObjectItem(hmsJsonRoot, hmsErrorCodeString);
|
||||
if (hmsErrorCode != NULL) {
|
||||
if (s_hmsLanguage == DJI_MOBILE_APP_LANGUAGE_CHINESE) {
|
||||
hmsLanguage = cJSON_GetObjectItem(hmsErrorCode, "zh");
|
||||
} else if (s_hmsLanguage == DJI_MOBILE_APP_LANGUAGE_ENGLISH) {
|
||||
hmsLanguage = cJSON_GetObjectItem(hmsErrorCode, "en");
|
||||
} else if (s_hmsLanguage == DJI_MOBILE_APP_LANGUAGE_JAPANESE) {
|
||||
hmsLanguage = cJSON_GetObjectItem(hmsErrorCode, "ja");
|
||||
} else if (s_hmsLanguage == DJI_MOBILE_APP_LANGUAGE_FRENCH) {
|
||||
hmsLanguage = cJSON_GetObjectItem(hmsErrorCode, "fr");
|
||||
}
|
||||
if (hmsLanguage != NULL) {
|
||||
if (hmsInfoTable.hmsInfo[i].errorLevel > MIN_HMS_ERROR_LEVEL &&
|
||||
hmsInfoTable.hmsInfo[i].errorLevel < MID_HMS_ERROR_LEVEL) {
|
||||
USER_LOG_WARN("[ErrorCode: 0x%2x]: %s", hmsInfoTable.hmsInfo[i].errorCode,
|
||||
hmsLanguage->valuestring);
|
||||
} else if (hmsInfoTable.hmsInfo[i].errorLevel >= MID_HMS_ERROR_LEVEL &&
|
||||
hmsInfoTable.hmsInfo[i].errorLevel < MAX_HMS_ERROR_LEVEL) {
|
||||
USER_LOG_ERROR("[ErrorCode: 0x%2x]: %s", hmsInfoTable.hmsInfo[i].errorCode,
|
||||
hmsLanguage->valuestring);
|
||||
}
|
||||
} else {
|
||||
USER_LOG_WARN("[ErrorCode: 0x%2x] There are no matching documents for this language for now.",
|
||||
hmsInfoTable.hmsInfo[i].errorCode);
|
||||
}
|
||||
} else {
|
||||
USER_LOG_WARN("[ErrorCode: 0x%2x] There are no matching documents in the current json for now.",
|
||||
hmsInfoTable.hmsInfo[i].errorCode);
|
||||
}
|
||||
}
|
||||
|
||||
cJSON_Delete(hmsJsonRoot);
|
||||
}
|
||||
#endif
|
||||
|
||||
static T_DjiReturnCode DjiTest_HmsInfoCallback(T_DjiHmsInfoTable hmsInfoTable)
|
||||
{
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
DjiTest_MarchErrCodeInfoTableByJson(hmsInfoTable);
|
||||
#else
|
||||
if (!DjiTest_MarchErrCodeInfoTable(hmsInfoTable)) {
|
||||
USER_LOG_ERROR("March HMS Information failed.");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (hmsInfoTable.hmsInfoNum == 0) {
|
||||
USER_LOG_INFO("All systems of drone are running well now.");
|
||||
|
||||
@ -39,8 +39,9 @@ extern "C" {
|
||||
/* Exported types ------------------------------------------------------------*/
|
||||
|
||||
/* Exported functions --------------------------------------------------------*/
|
||||
T_DjiReturnCode DjiTest_HmsRunSample(void);
|
||||
T_DjiReturnCode DjiTest_HmsStartService(void);
|
||||
T_DjiReturnCode DjiTest_HmsManagerRunSample(E_DjiMobileAppLanguage language);
|
||||
T_DjiReturnCode DjiTest_HmsCustomizationStartService(void);
|
||||
T_DjiReturnCode DjiTest_HmsCustomizationSetConfigFilePath(const char *path);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
@ -0,0 +1,155 @@
|
||||
/**
|
||||
********************************************************************
|
||||
* @file test_interest_point.c
|
||||
* @brief
|
||||
*
|
||||
* @copyright (c) 2018 DJI. All rights reserved.
|
||||
*
|
||||
* All information contained herein is, and remains, the property of DJI.
|
||||
* The intellectual and technical concepts contained herein are proprietary
|
||||
* to DJI and may be covered by U.S. and foreign patents, patents in process,
|
||||
* and protected by trade secret or copyright law. Dissemination of this
|
||||
* information, including but not limited to data and other proprietary
|
||||
* material(s) incorporated within the information, in any form, is strictly
|
||||
* prohibited without the express written consent of DJI.
|
||||
*
|
||||
* If you receive this source code without DJI’s authorization, you may not
|
||||
* further disseminate the information, and you must immediately remove the
|
||||
* source code and notify DJI of its removal. DJI reserves the right to pursue
|
||||
* legal actions against you for any loss(es) or damage(s) caused by your
|
||||
* failure to do so.
|
||||
*
|
||||
*********************************************************************
|
||||
*/
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include "test_interest_point.h"
|
||||
#include "dji_interest_point.h"
|
||||
#include "dji_logger.h"
|
||||
#include "dji_flight_controller.h"
|
||||
#include "flight_control/test_flight_control.h"
|
||||
#include "dji_aircraft_info.h"
|
||||
|
||||
/* Private constants ---------------------------------------------------------*/
|
||||
|
||||
/* Private types -------------------------------------------------------------*/
|
||||
|
||||
/* Private values -------------------------------------------------------------*/
|
||||
static T_DjiReturnCode DjiUser_InterestPointMissionStateCallback(T_DjiInterestPointMissionState missionState);
|
||||
|
||||
/* Private functions declaration ---------------------------------------------*/
|
||||
|
||||
/* Exported functions definition ---------------------------------------------*/
|
||||
T_DjiReturnCode DjiTest_InterestPointRunSample(void)
|
||||
{
|
||||
T_DjiReturnCode returnCode;
|
||||
T_DjiInterestPointSettings interestPointSettings = {0};
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
T_DjiFlightControllerRidInfo ridInfo = {0};
|
||||
T_DjiAircraftInfoBaseInfo aircraftInfoBaseInfo = {0};
|
||||
|
||||
ridInfo.latitude = 22.542812;
|
||||
ridInfo.longitude = 113.958902;
|
||||
ridInfo.altitude = 10;
|
||||
|
||||
returnCode = DjiAircraftInfo_GetBaseInfo(&aircraftInfoBaseInfo);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("get aircraft base info error");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SYSTEM_ERROR;
|
||||
}
|
||||
|
||||
returnCode = DjiFlightController_Init(ridInfo);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Flight control init failed, errno=%lld", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
returnCode = DjiInterestPoint_Init();
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Point interest init failed, errno=%lld", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
osalHandler->TaskSleepMs(1000);
|
||||
DjiFlightController_ObtainJoystickCtrlAuthority();
|
||||
|
||||
osalHandler->TaskSleepMs(1000);
|
||||
DjiFlightController_StartTakeoff();
|
||||
|
||||
osalHandler->TaskSleepMs(1000);
|
||||
DjiTest_FlightControlVelocityAndYawRateCtrl((T_DjiTestFlightControlVector3f) {0, 0, 5}, 0, 10000);
|
||||
|
||||
osalHandler->TaskSleepMs(1000);
|
||||
DjiTest_FlightControlVelocityAndYawRateCtrl((T_DjiTestFlightControlVector3f) {3, 0, 0}, 0, 5000);
|
||||
|
||||
interestPointSettings.latitude = 22.57775;
|
||||
interestPointSettings.longitude = 113.94265;
|
||||
|
||||
returnCode = DjiInterestPoint_RegMissionStateCallback(DjiUser_InterestPointMissionStateCallback);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Register mission state callback failed, errno=%lld", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
if (aircraftInfoBaseInfo.aircraftSeries == DJI_AIRCRAFT_SERIES_M400) {
|
||||
interestPointSettings.payloadCameraIndex = 1;
|
||||
USER_LOG_INFO("The sample try to use payload camera on position %d for poi, please make sure that DJI camera mounted on this position.",
|
||||
interestPointSettings.payloadCameraIndex);
|
||||
}
|
||||
|
||||
USER_LOG_INFO("Start interest point circle, circumcenter(%f, %f)", interestPointSettings.latitude, interestPointSettings.longitude);
|
||||
returnCode = DjiInterestPoint_Start(interestPointSettings);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Point interest start failed, errno=%lld", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
osalHandler->TaskSleepMs(1000);
|
||||
|
||||
USER_LOG_INFO("set speed to 15");
|
||||
DjiInterestPoint_SetSpeed(15.0f);
|
||||
|
||||
for (int i = 0; i < 60; ++i) {
|
||||
USER_LOG_INFO("Interest point mission running %d.", i);
|
||||
osalHandler->TaskSleepMs(1000);
|
||||
}
|
||||
|
||||
USER_LOG_INFO("Stop interest point circle");
|
||||
returnCode = DjiInterestPoint_Stop();
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Point interest stop failed, errno=%lld", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
USER_LOG_INFO("Force landing");
|
||||
DjiFlightController_StartForceLanding();
|
||||
|
||||
returnCode = DjiInterestPoint_DeInit();
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Point interest deinit failed, errno=%lld", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
returnCode = DjiFlightController_DeInit();
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Flight control init failed, errno=%lld", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
/* Private functions definition-----------------------------------------------*/
|
||||
static T_DjiReturnCode DjiUser_InterestPointMissionStateCallback(T_DjiInterestPointMissionState missionState)
|
||||
{
|
||||
USER_LOG_INFO("Interest point state: %s, radius: %.2f m, speed: %.2f m/s",
|
||||
missionState.state == DJI_INTEREST_POINT_MISSION_ACTION_STATE_NOT_STARTED ? "not started" :
|
||||
missionState.state == DJI_INTEREST_POINT_MISSION_ACTION_STATE_PAUSE ? "pause" :
|
||||
missionState.state == DJI_INTEREST_POINT_MISSION_ACTION_STATE_RUNNING ? "running" : "unknown",
|
||||
missionState.radius,
|
||||
missionState.curSpeed);
|
||||
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/
|
||||
@ -0,0 +1,49 @@
|
||||
/**
|
||||
********************************************************************
|
||||
* @file test_interest_point.h
|
||||
* @brief This is the header file for "test_interest_point.c", defining the structure and
|
||||
* (exported) function prototypes.
|
||||
*
|
||||
* @copyright (c) 2018 DJI. All rights reserved.
|
||||
*
|
||||
* All information contained herein is, and remains, the property of DJI.
|
||||
* The intellectual and technical concepts contained herein are proprietary
|
||||
* to DJI and may be covered by U.S. and foreign patents, patents in process,
|
||||
* and protected by trade secret or copyright law. Dissemination of this
|
||||
* information, including but not limited to data and other proprietary
|
||||
* material(s) incorporated within the information, in any form, is strictly
|
||||
* prohibited without the express written consent of DJI.
|
||||
*
|
||||
* If you receive this source code without DJI’s authorization, you may not
|
||||
* further disseminate the information, and you must immediately remove the
|
||||
* source code and notify DJI of its removal. DJI reserves the right to pursue
|
||||
* legal actions against you for any loss(es) or damage(s) caused by your
|
||||
* failure to do so.
|
||||
*
|
||||
*********************************************************************
|
||||
*/
|
||||
|
||||
/* Define to prevent recursive inclusion -------------------------------------*/
|
||||
#ifndef TEST_INTEREST_POINT_H
|
||||
#define TEST_INTEREST_POINT_H
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include "dji_typedef.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Exported constants --------------------------------------------------------*/
|
||||
|
||||
/* Exported types ------------------------------------------------------------*/
|
||||
|
||||
/* Exported functions --------------------------------------------------------*/
|
||||
T_DjiReturnCode DjiTest_InterestPointRunSample(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // TEST_INTEREST_POINT_H
|
||||
/************************ (C) COPYRIGHT DJI Innovations *******END OF FILE******/
|
||||
@ -77,14 +77,14 @@ T_DjiReturnCode DjiTest_LiveviewRunSample(E_DjiMountPosition mountPosition)
|
||||
goto out;
|
||||
}
|
||||
|
||||
USER_LOG_INFO("--> Step 2: Start h264 stream of the fpv and selected payload\r\n");
|
||||
USER_LOG_INFO("--> Step 2: Start h264 stream of the fpv and default of selected payload\r\n");
|
||||
DjiTest_WidgetLogAppend("--> Step 2: Start h264 stream of the fpv and selected payload\r\n");
|
||||
|
||||
if (aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M3E) {
|
||||
//TODO: how to use on M3E
|
||||
} else if (aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M3T) {
|
||||
//TODO: how to use on M3T
|
||||
} else {
|
||||
if (aircraftInfoBaseInfo.aircraftSeries == DJI_AIRCRAFT_SERIES_M300 ||
|
||||
aircraftInfoBaseInfo.aircraftSeries == DJI_AIRCRAFT_SERIES_M350 ||
|
||||
aircraftInfoBaseInfo.aircraftSeries == DJI_AIRCRAFT_SERIES_M30 ||
|
||||
aircraftInfoBaseInfo.aircraftSeries == DJI_AIRCRAFT_SERIES_M400) {
|
||||
|
||||
localTime = localtime(¤tTime);
|
||||
sprintf(s_fpvCameraStreamFilePath, "fpv_stream_%04d%02d%02d_%02d-%02d-%02d.h264",
|
||||
localTime->tm_year + 1900, localTime->tm_mon + 1, localTime->tm_mday,
|
||||
@ -98,8 +98,23 @@ T_DjiReturnCode DjiTest_LiveviewRunSample(E_DjiMountPosition mountPosition)
|
||||
}
|
||||
}
|
||||
|
||||
if (aircraftInfoBaseInfo.aircraftSeries == DJI_AIRCRAFT_SERIES_M400
|
||||
&& aircraftInfoBaseInfo.mountPositionType == DJI_MOUNT_POSITION_TYPE_MANIFOLD3_ONBOARD)
|
||||
{
|
||||
// XXX: On MANIFOLD3, FPV and MAIN CAMERA SOURCE_DEFAULT streams cannot be subscribed to at the same time.
|
||||
for (int i = 0; i < TEST_LIVEVIEW_STREAM_STROING_TIME_IN_SECONDS; ++i) {
|
||||
USER_LOG_INFO("Storing camera h264 stream, second: %d.", i + 1);
|
||||
osalHandler->TaskSleepMs(1000);
|
||||
}
|
||||
returnCode = DjiLiveview_StopH264Stream(DJI_LIVEVIEW_CAMERA_POSITION_FPV, DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Request to stop h264 of fpv failed, error code: 0x%08X", returnCode);
|
||||
goto out;
|
||||
}
|
||||
}
|
||||
|
||||
localTime = localtime(¤tTime);
|
||||
sprintf(s_payloadCameraStreamFilePath, "payload%d_vis_stream_%04d%02d%02d_%02d-%02d-%02d.h264",
|
||||
sprintf(s_payloadCameraStreamFilePath, "payload%d_default_stream_%04d%02d%02d_%02d-%02d-%02d.h264",
|
||||
mountPosition, localTime->tm_year + 1900, localTime->tm_mon + 1, localTime->tm_mday,
|
||||
localTime->tm_hour, localTime->tm_min, localTime->tm_sec);
|
||||
|
||||
@ -113,12 +128,17 @@ T_DjiReturnCode DjiTest_LiveviewRunSample(E_DjiMountPosition mountPosition)
|
||||
for (int i = 0; i < TEST_LIVEVIEW_STREAM_STROING_TIME_IN_SECONDS; ++i) {
|
||||
USER_LOG_INFO("Storing camera h264 stream, second: %d.", i + 1);
|
||||
#if TEST_LIVEVIEW_STREAM_REQUEST_I_FRAME_ON
|
||||
if (i % TEST_LIVEVIEW_STREAM_REQUEST_I_FRAME_TICK_IN_SECONDS == 0) {
|
||||
returnCode = DjiLiveview_RequestIntraframeFrameData((E_DjiLiveViewCameraPosition) mountPosition,
|
||||
DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Request stream I frame of payload %d failed, error code: 0x%08X", mountPosition,
|
||||
returnCode);
|
||||
if (aircraftInfoBaseInfo.mountPositionType != DJI_MOUNT_POSITION_TYPE_MANIFOLD3_ONBOARD)
|
||||
{
|
||||
if (i % TEST_LIVEVIEW_STREAM_REQUEST_I_FRAME_TICK_IN_SECONDS == 0)
|
||||
{
|
||||
returnCode = DjiLiveview_RequestIntraframeFrameData((E_DjiLiveViewCameraPosition)mountPosition,
|
||||
DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
|
||||
{
|
||||
USER_LOG_ERROR("Request stream I frame of payload %d failed, error code: 0x%08X", mountPosition,
|
||||
returnCode);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
@ -127,11 +147,11 @@ T_DjiReturnCode DjiTest_LiveviewRunSample(E_DjiMountPosition mountPosition)
|
||||
|
||||
USER_LOG_INFO("--> Step 3: Stop h264 stream of the fpv and selected payload\r\n");
|
||||
DjiTest_WidgetLogAppend("--> Step 3: Stop h264 stream of the fpv and selected payload");
|
||||
if (aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M3E) {
|
||||
|
||||
} else if (aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M3T) {
|
||||
|
||||
} else {
|
||||
if (aircraftInfoBaseInfo.aircraftSeries == DJI_AIRCRAFT_SERIES_M300 ||
|
||||
aircraftInfoBaseInfo.aircraftSeries == DJI_AIRCRAFT_SERIES_M350 ||
|
||||
aircraftInfoBaseInfo.aircraftSeries == DJI_AIRCRAFT_SERIES_M30 ||
|
||||
(aircraftInfoBaseInfo.aircraftSeries == DJI_AIRCRAFT_SERIES_M400
|
||||
&& aircraftInfoBaseInfo.mountPositionType != DJI_MOUNT_POSITION_TYPE_MANIFOLD3_ONBOARD)) {
|
||||
returnCode = DjiLiveview_StopH264Stream(DJI_LIVEVIEW_CAMERA_POSITION_FPV, DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Request to stop h264 of fpv failed, error code: 0x%08X", returnCode);
|
||||
@ -146,10 +166,12 @@ T_DjiReturnCode DjiTest_LiveviewRunSample(E_DjiMountPosition mountPosition)
|
||||
goto out;
|
||||
}
|
||||
|
||||
USER_LOG_INFO("Fpv stream is saved to file: %s", s_fpvCameraStreamFilePath);
|
||||
USER_LOG_INFO("Payload%d stream is saved to file: %s\r\n", mountPosition, s_payloadCameraStreamFilePath);
|
||||
|
||||
if (aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M3T) {
|
||||
if (DJI_AIRCRAFT_TYPE_M3T == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M3TA == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M3TD == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M4T == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M4TD == aircraftInfoBaseInfo.aircraftType
|
||||
) {
|
||||
USER_LOG_INFO("--> Start h264 stream of the fpv and selected payload\r\n");
|
||||
|
||||
localTime = localtime(¤tTime);
|
||||
|
||||
@ -180,7 +180,7 @@ REWAIT:
|
||||
}
|
||||
|
||||
sendDataCount++;
|
||||
memset(sendBuf, sendDataCount, TEST_MOP_CHANNEL_NORMAL_TRANSFOR_SEND_BUFFER);
|
||||
memset(sendBuf, 'A' - 1 + (sendDataCount % 26), TEST_MOP_CHANNEL_NORMAL_TRANSFOR_SEND_BUFFER);
|
||||
returnCode = DjiMopChannel_SendData(s_testMopChannelNormalOutHandle, sendBuf,
|
||||
TEST_MOP_CHANNEL_NORMAL_TRANSFOR_SEND_BUFFER, &realLen);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
@ -723,6 +723,8 @@ static void *DjiTest_MopChannelFileServiceRecvTask(void *arg)
|
||||
s_fileServiceContent[clientNum].uploadState = MOP_FILE_SERVICE_UPLOAD_DATA_SENDING;
|
||||
s_fileServiceContent[clientNum].uploadSeqNum = fileTransfor->seqNum;
|
||||
|
||||
USER_LOG_INFO("fileTransfor->seqNum %d", fileTransfor->seqNum);
|
||||
|
||||
uploadWriteLen = fwrite(&recvBuf[UTIL_OFFSETOF(T_DjiMopChannel_FileTransfor, data)], 1,
|
||||
fileTransfor->dataLen, uploadFile);
|
||||
if (uploadWriteLen < 0) {
|
||||
|
||||
@ -32,22 +32,36 @@
|
||||
#include "dji_platform.h"
|
||||
#include "time_sync/test_time_sync.h"
|
||||
|
||||
/* Private constants ---------------------------------------------------------*/
|
||||
#define POSITIONING_TASK_FREQ (1)
|
||||
#define POSITIONING_TASK_STACK_SIZE (1024)
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
|
||||
#define DJI_TEST_POSITIONING_EVENT_COUNT (2)
|
||||
#include "time.h"
|
||||
|
||||
#endif
|
||||
|
||||
/* Private constants ---------------------------------------------------------*/
|
||||
#define POSITIONING_TASK_FREQ (0.1)
|
||||
#define POSITIONING_TASK_STACK_SIZE (3 * 1024)
|
||||
#define TEST_RTCM_FILE_PATH_STR_MAX_SIZE (64)
|
||||
|
||||
#define DJI_TEST_POSITIONING_EVENT_COUNT (1)
|
||||
#define DJI_TEST_TIME_INTERVAL_AMONG_EVENTS_US (200000)
|
||||
|
||||
/* Private types -------------------------------------------------------------*/
|
||||
|
||||
|
||||
/* Private functions declaration ---------------------------------------------*/
|
||||
static void *DjiTest_PositioningTask(void *arg);
|
||||
static T_DjiReturnCode DjiTest_ReceiveRtkOnAircraftRtcmDataCallback(uint8_t index, const uint8_t *data,
|
||||
uint16_t dataLen);
|
||||
static T_DjiReturnCode DjiTest_ReceiveRtkBaseStationRtcmDataCallback(uint8_t index, const uint8_t *data,
|
||||
uint16_t dataLen);
|
||||
|
||||
/* Private variables ---------------------------------------------------------*/
|
||||
static T_DjiTaskHandle s_userPositioningThread;
|
||||
static int32_t s_eventIndex = 0;
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
static char s_rtkOnAircraftRtcmFilePath[TEST_RTCM_FILE_PATH_STR_MAX_SIZE];
|
||||
static char s_rtkBaseStationRtcmFilePath[TEST_RTCM_FILE_PATH_STR_MAX_SIZE];
|
||||
#endif
|
||||
|
||||
/* Exported functions definition ---------------------------------------------*/
|
||||
T_DjiReturnCode DjiTest_PositioningStartService(void)
|
||||
@ -63,12 +77,40 @@ T_DjiReturnCode DjiTest_PositioningStartService(void)
|
||||
|
||||
DjiPositioning_SetTaskIndex(0);
|
||||
|
||||
#ifndef SYSTEM_ARCH_LINUX
|
||||
if (osalHandler->TaskCreate("user_positioning_task", DjiTest_PositioningTask,
|
||||
POSITIONING_TASK_STACK_SIZE, NULL, &s_userPositioningThread) !=
|
||||
DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("user positioning task create error.");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
}
|
||||
#else
|
||||
time_t currentTime = time(NULL);
|
||||
struct tm *localTime = NULL;
|
||||
|
||||
localTime = localtime(¤tTime);
|
||||
sprintf(s_rtkOnAircraftRtcmFilePath, "rtk_on_aircraft_%04d%02d%02d_%02d-%02d-%02d.rtcm",
|
||||
localTime->tm_year + 1900, localTime->tm_mon + 1, localTime->tm_mday,
|
||||
localTime->tm_hour, localTime->tm_min, localTime->tm_sec);
|
||||
|
||||
localTime = localtime(¤tTime);
|
||||
sprintf(s_rtkBaseStationRtcmFilePath, "rtk_base_station_%04d%02d%02d_%02d-%02d-%02d.rtcm",
|
||||
localTime->tm_year + 1900, localTime->tm_mon + 1, localTime->tm_mday,
|
||||
localTime->tm_hour, localTime->tm_min, localTime->tm_sec);
|
||||
#endif
|
||||
djiStat = DjiPositioning_RegReceiveRtcmDataCallback(DJI_POSITIONING_RTCM_DATA_TYPE_RTK_BASE_STATION,
|
||||
DjiTest_ReceiveRtkBaseStationRtcmDataCallback);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Register receive rtk base station callback error.");
|
||||
return djiStat;
|
||||
}
|
||||
|
||||
djiStat = DjiPositioning_RegReceiveRtcmDataCallback(DJI_POSITIONING_RTCM_DATA_TYPE_RTK_ON_AIRCRAFT,
|
||||
DjiTest_ReceiveRtkOnAircraftRtcmDataCallback);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Register receive rtk on aircraft callback error.");
|
||||
return djiStat;
|
||||
}
|
||||
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
@ -128,22 +170,22 @@ static void *DjiTest_PositioningTask(void *arg)
|
||||
continue;
|
||||
}
|
||||
|
||||
USER_LOG_DEBUG("request position of target points success.");
|
||||
USER_LOG_DEBUG("detail position information:");
|
||||
USER_LOG_INFO("request position of target points success.");
|
||||
USER_LOG_INFO("detail position information:");
|
||||
for (i = 0; i < DJI_TEST_POSITIONING_EVENT_COUNT; ++i) {
|
||||
USER_LOG_DEBUG("position solution property: %d.", positionInfo[i].positionSolutionProperty);
|
||||
USER_LOG_DEBUG("pitchAttitudeAngle: %d\trollAttitudeAngle: %d\tyawAttitudeAngle: %d",
|
||||
USER_LOG_INFO("position solution property: %d.", positionInfo[i].positionSolutionProperty);
|
||||
USER_LOG_INFO("pitchAttitudeAngle: %d\trollAttitudeAngle: %d\tyawAttitudeAngle: %d",
|
||||
positionInfo[i].uavAttitude.pitch, positionInfo[i].uavAttitude.roll,
|
||||
positionInfo[i].uavAttitude.yaw);
|
||||
USER_LOG_DEBUG("northPositionOffset: %d\tearthPositionOffset: %d\tdownPositionOffset: %d",
|
||||
USER_LOG_INFO("northPositionOffset: %d\tearthPositionOffset: %d\tdownPositionOffset: %d",
|
||||
positionInfo[i].offsetBetweenMainAntennaAndTargetPoint.x,
|
||||
positionInfo[i].offsetBetweenMainAntennaAndTargetPoint.y,
|
||||
positionInfo[i].offsetBetweenMainAntennaAndTargetPoint.z);
|
||||
USER_LOG_DEBUG("longitude: %.8f\tlatitude: %.8f\theight: %.8f",
|
||||
USER_LOG_INFO("longitude: %.8f\tlatitude: %.8f\theight: %.8f",
|
||||
positionInfo[i].targetPointPosition.longitude,
|
||||
positionInfo[i].targetPointPosition.latitude,
|
||||
positionInfo[i].targetPointPosition.height);
|
||||
USER_LOG_DEBUG(
|
||||
USER_LOG_INFO(
|
||||
"longStandardDeviation: %.8f\tlatStandardDeviation: %.8f\thgtStandardDeviation: %.8f",
|
||||
positionInfo[i].targetPointPositionStandardDeviation.longitude,
|
||||
positionInfo[i].targetPointPositionStandardDeviation.latitude,
|
||||
@ -158,4 +200,51 @@ static void *DjiTest_PositioningTask(void *arg)
|
||||
#pragma GCC diagnostic pop
|
||||
#endif
|
||||
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
static int32_t DjiTest_SaveRtcmData(char *filePath, const uint8_t *data, uint32_t len)
|
||||
{
|
||||
FILE *fp = NULL;
|
||||
size_t size;
|
||||
|
||||
fp = fopen(filePath, "ab+");
|
||||
if (fp == NULL) {
|
||||
printf("fopen failed!\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
size = fwrite(data, 1, len, fp);
|
||||
if (size != len) {
|
||||
fclose(fp);
|
||||
return -1;
|
||||
}
|
||||
|
||||
fflush(fp);
|
||||
fclose(fp);
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
static T_DjiReturnCode DjiTest_ReceiveRtkOnAircraftRtcmDataCallback(uint8_t index, const uint8_t *data,
|
||||
uint16_t dataLen)
|
||||
{
|
||||
USER_LOG_INFO("Receive rtcm data from rtk on aircraft, index: %d, len: %d", index, dataLen);
|
||||
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
DjiTest_SaveRtcmData(s_rtkOnAircraftRtcmFilePath, data, dataLen);
|
||||
#endif
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
static T_DjiReturnCode DjiTest_ReceiveRtkBaseStationRtcmDataCallback(uint8_t index, const uint8_t *data,
|
||||
uint16_t dataLen)
|
||||
{
|
||||
USER_LOG_INFO("Receive rtcm data from rtk base station, index: %d, len: %d", index, dataLen);
|
||||
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
DjiTest_SaveRtcmData(s_rtkBaseStationRtcmFilePath, data, dataLen);
|
||||
#endif
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
|
||||
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/
|
||||
|
||||
@ -98,10 +98,11 @@ T_DjiReturnCode DjiTest_PowerManagementStartService(void)
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
if ((baseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M300_RTK ||
|
||||
baseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M350_RTK) &&
|
||||
(baseInfo.djiAdapterType == DJI_SDK_ADAPTER_TYPE_SKYPORT_V2 ||
|
||||
baseInfo.djiAdapterType == DJI_SDK_ADAPTER_TYPE_XPORT)) {
|
||||
if (baseInfo.djiAdapterType == DJI_SDK_ADAPTER_TYPE_SKYPORT_V2 ||
|
||||
baseInfo.djiAdapterType == DJI_SDK_ADAPTER_TYPE_XPORT ||
|
||||
baseInfo.djiAdapterType == DJI_SDK_ADAPTER_TYPE_SKYPORT_V3 ||
|
||||
baseInfo.djiAdapterType == DJI_SDK_ADAPTER_TYPE_EPORT_V2_RIBBON_CABLE ||
|
||||
baseInfo.aircraftType == DJI_AIRCRAFT_TYPE_FC30) {
|
||||
// apply high power
|
||||
//如果函数指针为空(没有指向函数),就返回错误代码:DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN
|
||||
if (s_applyHighPowerHandler.pinInit == NULL) {
|
||||
@ -128,11 +129,38 @@ T_DjiReturnCode DjiTest_PowerManagementStartService(void)
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
//执行高电压请求
|
||||
returnCode = DjiPowerManagement_ApplyHighPowerSync();
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("apply high power error");
|
||||
return returnCode;
|
||||
//这是3.5申请高功率的代码
|
||||
// returnCode = DjiPowerManagement_ApplyHighPowerSync();
|
||||
// if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
// USER_LOG_ERROR("apply high power error");
|
||||
// return returnCode;
|
||||
|
||||
//这是3.13.0申请高功率的代码
|
||||
if (baseInfo.djiAdapterType == DJI_SDK_ADAPTER_TYPE_SKYPORT_V3 ||
|
||||
baseInfo.djiAdapterType == DJI_SDK_ADAPTER_TYPE_EPORT_V2_RIBBON_CABLE) {
|
||||
E_DjiHighPowerVoltage voltage = E_DJI_HIGH_POWER_VOLTAGE_17V;
|
||||
|
||||
USER_LOG_INFO("Start to apply high power.");
|
||||
|
||||
returnCode = DjiPowerManagement_ApplyHighPowerSyncV2(voltage);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("apply high power error");
|
||||
return returnCode;
|
||||
}
|
||||
USER_LOG_INFO("Success to apply high power %s.",
|
||||
voltage == E_DJI_HIGH_POWER_VOLTAGE_13V6 ? "13V6" :
|
||||
voltage == E_DJI_HIGH_POWER_VOLTAGE_17V ? "17V" :
|
||||
voltage == E_DJI_HIGH_POWER_VOLTAGE_24V ? "24V" : "???");
|
||||
} else {
|
||||
USER_LOG_INFO("Start to apply high power.");
|
||||
|
||||
//执行高电压请求
|
||||
returnCode = DjiPowerManagement_ApplyHighPowerSync();
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("apply high power error");
|
||||
return returnCode;
|
||||
}
|
||||
USER_LOG_INFO("Success to apply high power.");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -0,0 +1,109 @@
|
||||
/**
|
||||
********************************************************************
|
||||
* @file test_tethered_battery.c
|
||||
* @brief
|
||||
*
|
||||
* @copyright (c) 2024 DJI. All rights reserved.
|
||||
*
|
||||
* All information contained herein is, and remains, the property of DJI.
|
||||
* The intellectual and technical concepts contained herein are proprietary
|
||||
* to DJI and may be covered by U.S. and foreign patents, patents in process,
|
||||
* and protected by trade secret or copyright law. Dissemination of this
|
||||
* information, including but not limited to data and other proprietary
|
||||
* material(s) incorporated within the information, in any form, is strictly
|
||||
* prohibited without the express written consent of DJI.
|
||||
*
|
||||
* If you receive this source code without DJI’s authorization, you may not
|
||||
* further disseminate the information, and you must immediately remove the
|
||||
* source code and notify DJI of its removal. DJI reserves the right to pursue
|
||||
* legal actions against you for any loss(es) or damage(s) caused by your
|
||||
* failure to do so.
|
||||
*
|
||||
*********************************************************************
|
||||
*/
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include "test_tethered_battery.h"
|
||||
#include "dji_logger.h"
|
||||
#include "dji_tethered_battery.h"
|
||||
#include "utils/util_misc.h"
|
||||
|
||||
/* Private constants ---------------------------------------------------------*/
|
||||
#define TETHERED_BATTERY_TASK_STACK_SIZE (2048)
|
||||
#define TETHERED_BATTERY_TETHER_LINE_MAX_LENGTH (150)
|
||||
|
||||
/* Private types -------------------------------------------------------------*/
|
||||
|
||||
/* Private values -------------------------------------------------------------*/
|
||||
static T_DjiTaskHandle s_tetheredBatteryTestThread;
|
||||
static bool s_isTetherRetrieval = false;
|
||||
|
||||
/* Private functions declaration ---------------------------------------------*/
|
||||
static void *DjiTest_TetheredBatteryTask(void *arg);
|
||||
|
||||
/* Exported functions definition ---------------------------------------------*/
|
||||
T_DjiReturnCode DjiTest_TetheredBatteryStartService(void)
|
||||
{
|
||||
T_DjiReturnCode returnCode;
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
|
||||
returnCode = DjiTetheredBattery_Init();
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Dji test tethered battery init failed, errno = 0x%08llX", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
returnCode = osalHandler->TaskCreate("user_tethered_battery_task", DjiTest_TetheredBatteryTask,
|
||||
TETHERED_BATTERY_TASK_STACK_SIZE, NULL,
|
||||
&s_tetheredBatteryTestThread);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Dji tethered battery test task create failed, errno = 0x%08llX", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
/* Private functions definition-----------------------------------------------*/
|
||||
|
||||
static void *DjiTest_TetheredBatteryTask(void *arg)
|
||||
{
|
||||
T_DjiReturnCode returnCode;
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
T_DjiTetherLineStatus tetherLineStatus = {0};
|
||||
|
||||
USER_UTIL_UNUSED(arg);
|
||||
|
||||
tetherLineStatus.usedLength = 0;
|
||||
tetherLineStatus.totalLength = TETHERED_BATTERY_TETHER_LINE_MAX_LENGTH;
|
||||
|
||||
while (1) {
|
||||
if (tetherLineStatus.usedLength >= tetherLineStatus.totalLength) {
|
||||
s_isTetherRetrieval = true;
|
||||
}
|
||||
|
||||
if (tetherLineStatus.usedLength <= 0.01f) {
|
||||
s_isTetherRetrieval = false;
|
||||
}
|
||||
|
||||
if (s_isTetherRetrieval == false) {
|
||||
tetherLineStatus.usedLength += 1.00f;
|
||||
} else {
|
||||
tetherLineStatus.usedLength -= 1.00f;
|
||||
}
|
||||
|
||||
returnCode = DjiTetheredBattery_PushTetherLineStatus(tetherLineStatus);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Test push tether line status error, stat = 0x%08llX", returnCode);
|
||||
}
|
||||
|
||||
USER_LOG_DEBUG("Push tether line status, total: %.2f, used: %.2f", tetherLineStatus.totalLength,
|
||||
tetherLineStatus.usedLength);
|
||||
|
||||
osalHandler->TaskSleepMs(100);
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/
|
||||
@ -0,0 +1,49 @@
|
||||
/**
|
||||
********************************************************************
|
||||
* @file test_tethered_battery.h
|
||||
* @brief This is the header file for "test_tethered_battery.c", defining the structure and
|
||||
* (exported) function prototypes.
|
||||
*
|
||||
* @copyright (c) 2024 DJI. All rights reserved.
|
||||
*
|
||||
* All information contained herein is, and remains, the property of DJI.
|
||||
* The intellectual and technical concepts contained herein are proprietary
|
||||
* to DJI and may be covered by U.S. and foreign patents, patents in process,
|
||||
* and protected by trade secret or copyright law. Dissemination of this
|
||||
* information, including but not limited to data and other proprietary
|
||||
* material(s) incorporated within the information, in any form, is strictly
|
||||
* prohibited without the express written consent of DJI.
|
||||
*
|
||||
* If you receive this source code without DJI’s authorization, you may not
|
||||
* further disseminate the information, and you must immediately remove the
|
||||
* source code and notify DJI of its removal. DJI reserves the right to pursue
|
||||
* legal actions against you for any loss(es) or damage(s) caused by your
|
||||
* failure to do so.
|
||||
*
|
||||
*********************************************************************
|
||||
*/
|
||||
|
||||
/* Define to prevent recursive inclusion -------------------------------------*/
|
||||
#ifndef TEST_TETHERED_BATTERY_H
|
||||
#define TEST_TETHERED_BATTERY_H
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include "dji_typedef.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Exported constants --------------------------------------------------------*/
|
||||
|
||||
/* Exported types ------------------------------------------------------------*/
|
||||
|
||||
/* Exported functions --------------------------------------------------------*/
|
||||
T_DjiReturnCode DjiTest_TetheredBatteryStartService(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // TEST_TETHERED_BATTERY_H
|
||||
/************************ (C) COPYRIGHT DJI Innovations *******END OF FILE******/
|
||||
@ -160,9 +160,16 @@ static void *DjiTest_TimeSyncTask(void *arg)
|
||||
continue;
|
||||
}
|
||||
|
||||
USER_LOG_DEBUG("current aircraft time is %04d-%02d-%02d %02d:%02d:%02d %d.",
|
||||
aircraftTime.year, aircraftTime.month, aircraftTime.day,
|
||||
aircraftTime.hour, aircraftTime.minute, aircraftTime.second, aircraftTime.microsecond);
|
||||
if ((aircraftTime.second % 30) == 0) {
|
||||
USER_LOG_INFO("current aircraft time is %04d-%02d-%02d %02d:%02d:%02d %d.",
|
||||
aircraftTime.year, aircraftTime.month, aircraftTime.day,
|
||||
aircraftTime.hour, aircraftTime.minute, aircraftTime.second, aircraftTime.microsecond);
|
||||
} else {
|
||||
USER_LOG_DEBUG("current aircraft time is %04d-%02d-%02d %02d:%02d:%02d %d.",
|
||||
aircraftTime.year, aircraftTime.month, aircraftTime.day,
|
||||
aircraftTime.hour, aircraftTime.minute, aircraftTime.second, aircraftTime.microsecond);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
2688
samples/sample_c/module_sample/utils/cJSON.c
Normal file
2688
samples/sample_c/module_sample/utils/cJSON.c
Normal file
File diff suppressed because it is too large
Load Diff
290
samples/sample_c/module_sample/utils/cJSON.h
Normal file
290
samples/sample_c/module_sample/utils/cJSON.h
Normal file
@ -0,0 +1,290 @@
|
||||
/*
|
||||
Copyright (c) 2009-2017 Dave Gamble and cJSON contributors
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#ifndef cJSON__h
|
||||
#define cJSON__h
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
#if !defined(__WINDOWS__) && (defined(WIN32) || defined(WIN64) || defined(_MSC_VER) || defined(_WIN32))
|
||||
#define __WINDOWS__
|
||||
#endif
|
||||
|
||||
#ifdef __WINDOWS__
|
||||
|
||||
/* When compiling for windows, we specify a specific calling convention to avoid issues where we are being called from a project with a different default calling convention. For windows you have 3 define options:
|
||||
|
||||
CJSON_HIDE_SYMBOLS - Define this in the case where you don't want to ever dllexport symbols
|
||||
CJSON_EXPORT_SYMBOLS - Define this on library build when you want to dllexport symbols (default)
|
||||
CJSON_IMPORT_SYMBOLS - Define this if you want to dllimport symbol
|
||||
|
||||
For *nix builds that support visibility attribute, you can define similar behavior by
|
||||
|
||||
setting default visibility to hidden by adding
|
||||
-fvisibility=hidden (for gcc)
|
||||
or
|
||||
-xldscope=hidden (for sun cc)
|
||||
to CFLAGS
|
||||
|
||||
then using the CJSON_API_VISIBILITY flag to "export" the same symbols the way CJSON_EXPORT_SYMBOLS does
|
||||
|
||||
*/
|
||||
|
||||
#define CJSON_CDECL __cdecl
|
||||
#define CJSON_STDCALL __stdcall
|
||||
|
||||
/* export symbols by default, this is necessary for copy pasting the C and header file */
|
||||
#if !defined(CJSON_HIDE_SYMBOLS) && !defined(CJSON_IMPORT_SYMBOLS) && !defined(CJSON_EXPORT_SYMBOLS)
|
||||
#define CJSON_EXPORT_SYMBOLS
|
||||
#endif
|
||||
|
||||
#if defined(CJSON_HIDE_SYMBOLS)
|
||||
#define CJSON_PUBLIC(type) type CJSON_STDCALL
|
||||
#elif defined(CJSON_EXPORT_SYMBOLS)
|
||||
#define CJSON_PUBLIC(type) __declspec(dllexport) type CJSON_STDCALL
|
||||
#elif defined(CJSON_IMPORT_SYMBOLS)
|
||||
#define CJSON_PUBLIC(type) __declspec(dllimport) type CJSON_STDCALL
|
||||
#endif
|
||||
#else /* !__WINDOWS__ */
|
||||
#define CJSON_CDECL
|
||||
#define CJSON_STDCALL
|
||||
|
||||
#if (defined(__GNUC__) || defined(__SUNPRO_CC) || defined (__SUNPRO_C)) && defined(CJSON_API_VISIBILITY)
|
||||
#define CJSON_PUBLIC(type) __attribute__((visibility("default"))) type
|
||||
#else
|
||||
#define CJSON_PUBLIC(type) type
|
||||
#endif
|
||||
#endif
|
||||
|
||||
/* project version */
|
||||
#define CJSON_VERSION_MAJOR 1
|
||||
#define CJSON_VERSION_MINOR 7
|
||||
#define CJSON_VERSION_PATCH 12
|
||||
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
|
||||
/* cJSON Types: */
|
||||
#define cJSON_Invalid (0)
|
||||
#define cJSON_False (1 << 0)
|
||||
#define cJSON_True (1 << 1)
|
||||
#define cJSON_NULL (1 << 2)
|
||||
#define cJSON_Number (1 << 3)
|
||||
#define cJSON_String (1 << 4)
|
||||
#define cJSON_Array (1 << 5)
|
||||
#define cJSON_Object (1 << 6)
|
||||
#define cJSON_Raw (1 << 7) /* raw json */
|
||||
|
||||
#define cJSON_IsReference 256
|
||||
#define cJSON_StringIsConst 512
|
||||
|
||||
/* The cJSON structure: */
|
||||
typedef struct cJSON {
|
||||
/* next/prev allow you to walk array/object chains. Alternatively, use GetArraySize/GetArrayItem/GetObjectItem */
|
||||
struct cJSON *next;
|
||||
struct cJSON *prev;
|
||||
/* An array or object item will have a child pointer pointing to a chain of the items in the array/object. */
|
||||
struct cJSON *child;
|
||||
|
||||
/* The type of the item, as above. */
|
||||
int type;
|
||||
|
||||
/* The item's string, if type==cJSON_String and type == cJSON_Raw */
|
||||
char *valuestring;
|
||||
/* writing to valueint is DEPRECATED, use cJSON_SetNumberValue instead */
|
||||
int valueint;
|
||||
/* The item's number, if type==cJSON_Number */
|
||||
double valuedouble;
|
||||
|
||||
/* The item's name string, if this item is the child of, or is in the list of subitems of an object. */
|
||||
char *string;
|
||||
} cJSON;
|
||||
|
||||
typedef struct cJSON_Hooks {
|
||||
/* malloc/free are CDECL on Windows regardless of the default calling convention of the compiler, so ensure the hooks allow passing those functions directly. */
|
||||
void *(CJSON_CDECL *malloc_fn)(size_t sz);
|
||||
void (CJSON_CDECL *free_fn)(void *ptr);
|
||||
} cJSON_Hooks;
|
||||
|
||||
typedef int cJSON_bool;
|
||||
|
||||
/* Limits how deeply nested arrays/objects can be before cJSON rejects to parse them.
|
||||
* This is to prevent stack overflows. */
|
||||
#ifndef CJSON_NESTING_LIMIT
|
||||
#define CJSON_NESTING_LIMIT 1000
|
||||
#endif
|
||||
|
||||
/* returns the version of cJSON as a string */
|
||||
CJSON_PUBLIC(const char*)cJSON_Version(void);
|
||||
|
||||
/* Supply malloc, realloc and free functions to cJSON */
|
||||
CJSON_PUBLIC(void) cJSON_InitHooks(cJSON_Hooks *hooks);
|
||||
|
||||
/* Memory Management: the caller is always responsible to free the results from all variants of cJSON_Parse (with cJSON_Delete) and cJSON_Print (with stdlib free, cJSON_Hooks.free_fn, or cJSON_free as appropriate). The exception is cJSON_PrintPreallocated, where the caller has full responsibility of the buffer. */
|
||||
/* Supply a block of JSON, and this returns a cJSON object you can interrogate. */
|
||||
CJSON_PUBLIC(cJSON *)cJSON_Parse(const char *value);
|
||||
CJSON_PUBLIC(cJSON *)cJSON_ParseByJsonData(const uint8_t *json_data, uint16_t data_len);
|
||||
/* ParseWithOpts allows you to require (and check) that the JSON is null terminated, and to retrieve the pointer to the final byte parsed. */
|
||||
/* If you supply a ptr in return_parse_end and parsing fails, then return_parse_end will contain a pointer to the error so will match cJSON_GetErrorPtr(). */
|
||||
CJSON_PUBLIC(cJSON *)cJSON_ParseWithOpts(const char *value, const char **return_parse_end,
|
||||
cJSON_bool require_null_terminated);
|
||||
|
||||
/* Render a cJSON entity to text for transfer/storage. */
|
||||
CJSON_PUBLIC(char *)cJSON_Print(const cJSON *item);
|
||||
/* Render a cJSON entity to text for transfer/storage without any formatting. */
|
||||
CJSON_PUBLIC(char *)cJSON_PrintUnformatted(const cJSON *item);
|
||||
/* Render a cJSON entity to text using a buffered strategy. prebuffer is a guess at the final size. guessing well reduces reallocation. fmt=0 gives unformatted, =1 gives formatted */
|
||||
CJSON_PUBLIC(char *)cJSON_PrintBuffered(const cJSON *item, int prebuffer, cJSON_bool fmt);
|
||||
/* Render a cJSON entity to text using a buffer already allocated in memory with given length. Returns 1 on success and 0 on failure. */
|
||||
/* NOTE: cJSON is not always 100% accurate in estimating how much memory it will use, so to be safe allocate 5 bytes more than you actually need */
|
||||
CJSON_PUBLIC(cJSON_bool) cJSON_PrintPreallocated(cJSON *item, char *buffer, const int length, const cJSON_bool format);
|
||||
/* Delete a cJSON entity and all subentities. */
|
||||
CJSON_PUBLIC(void) cJSON_Delete(cJSON *item);
|
||||
|
||||
/* Returns the number of items in an array (or object). */
|
||||
CJSON_PUBLIC(int) cJSON_GetArraySize(const cJSON *array);
|
||||
/* Retrieve item number "index" from array "array". Returns NULL if unsuccessful. */
|
||||
CJSON_PUBLIC(cJSON *)cJSON_GetArrayItem(const cJSON *array, int index);
|
||||
/* Get item "string" from object. Case insensitive. */
|
||||
CJSON_PUBLIC(cJSON *)cJSON_GetObjectItem(const cJSON *const object, const char *const string);
|
||||
CJSON_PUBLIC(cJSON *)cJSON_GetObjectItemCaseSensitive(const cJSON *const object, const char *const string);
|
||||
CJSON_PUBLIC(cJSON_bool) cJSON_HasObjectItem(const cJSON *object, const char *string);
|
||||
/* For analysing failed parses. This returns a pointer to the parse error. You'll probably need to look a few chars back to make sense of it. Defined when cJSON_Parse() returns 0. 0 when cJSON_Parse() succeeds. */
|
||||
CJSON_PUBLIC(const char *)cJSON_GetErrorPtr(void);
|
||||
|
||||
/* Check if the item is a string and return its valuestring */
|
||||
CJSON_PUBLIC(char *)cJSON_GetStringValue(cJSON *item);
|
||||
|
||||
/* These functions check the type of an item */
|
||||
CJSON_PUBLIC(cJSON_bool) cJSON_IsInvalid(const cJSON *const item);
|
||||
CJSON_PUBLIC(cJSON_bool) cJSON_IsFalse(const cJSON *const item);
|
||||
CJSON_PUBLIC(cJSON_bool) cJSON_IsTrue(const cJSON *const item);
|
||||
CJSON_PUBLIC(cJSON_bool) cJSON_IsBool(const cJSON *const item);
|
||||
CJSON_PUBLIC(cJSON_bool) cJSON_IsNull(const cJSON *const item);
|
||||
CJSON_PUBLIC(cJSON_bool) cJSON_IsNumber(const cJSON *const item);
|
||||
CJSON_PUBLIC(cJSON_bool) cJSON_IsString(const cJSON *const item);
|
||||
CJSON_PUBLIC(cJSON_bool) cJSON_IsArray(const cJSON *const item);
|
||||
CJSON_PUBLIC(cJSON_bool) cJSON_IsObject(const cJSON *const item);
|
||||
CJSON_PUBLIC(cJSON_bool) cJSON_IsRaw(const cJSON *const item);
|
||||
|
||||
/* These calls create a cJSON item of the appropriate type. */
|
||||
CJSON_PUBLIC(cJSON *)cJSON_CreateNull(void);
|
||||
CJSON_PUBLIC(cJSON *)cJSON_CreateTrue(void);
|
||||
CJSON_PUBLIC(cJSON *)cJSON_CreateFalse(void);
|
||||
CJSON_PUBLIC(cJSON *)cJSON_CreateBool(cJSON_bool boolean);
|
||||
CJSON_PUBLIC(cJSON *)cJSON_CreateNumber(double num);
|
||||
CJSON_PUBLIC(cJSON *)cJSON_CreateString(const char *string);
|
||||
/* raw json */
|
||||
CJSON_PUBLIC(cJSON *)cJSON_CreateRaw(const char *raw);
|
||||
CJSON_PUBLIC(cJSON *)cJSON_CreateArray(void);
|
||||
CJSON_PUBLIC(cJSON *)cJSON_CreateObject(void);
|
||||
|
||||
/* Create a string where valuestring references a string so
|
||||
* it will not be freed by cJSON_Delete */
|
||||
CJSON_PUBLIC(cJSON *)cJSON_CreateStringReference(const char *string);
|
||||
/* Create an object/array that only references it's elements so
|
||||
* they will not be freed by cJSON_Delete */
|
||||
CJSON_PUBLIC(cJSON *)cJSON_CreateObjectReference(const cJSON *child);
|
||||
CJSON_PUBLIC(cJSON *)cJSON_CreateArrayReference(const cJSON *child);
|
||||
|
||||
/* These utilities create an Array of count items.
|
||||
* The parameter count cannot be greater than the number of elements in the number array, otherwise array access will be out of bounds.*/
|
||||
CJSON_PUBLIC(cJSON *)cJSON_CreateIntArray(const int *numbers, int count);
|
||||
CJSON_PUBLIC(cJSON *)cJSON_CreateFloatArray(const float *numbers, int count);
|
||||
CJSON_PUBLIC(cJSON *)cJSON_CreateDoubleArray(const double *numbers, int count);
|
||||
CJSON_PUBLIC(cJSON *)cJSON_CreateStringArray(const char **strings, int count);
|
||||
|
||||
/* Append item to the specified array/object. */
|
||||
CJSON_PUBLIC(void) cJSON_AddItemToArray(cJSON *array, cJSON *item);
|
||||
CJSON_PUBLIC(void) cJSON_AddItemToObject(cJSON *object, const char *string, cJSON *item);
|
||||
/* Use this when string is definitely const (i.e. a literal, or as good as), and will definitely survive the cJSON object.
|
||||
* WARNING: When this function was used, make sure to always check that (item->type & cJSON_StringIsConst) is zero before
|
||||
* writing to `item->string` */
|
||||
CJSON_PUBLIC(void) cJSON_AddItemToObjectCS(cJSON *object, const char *string, cJSON *item);
|
||||
/* Append reference to item to the specified array/object. Use this when you want to add an existing cJSON to a new cJSON, but don't want to corrupt your existing cJSON. */
|
||||
CJSON_PUBLIC(void) cJSON_AddItemReferenceToArray(cJSON *array, cJSON *item);
|
||||
CJSON_PUBLIC(void) cJSON_AddItemReferenceToObject(cJSON *object, const char *string, cJSON *item);
|
||||
|
||||
/* Remove/Detach items from Arrays/Objects. */
|
||||
CJSON_PUBLIC(cJSON *)cJSON_DetachItemViaPointer(cJSON *parent, cJSON *const item);
|
||||
CJSON_PUBLIC(cJSON *)cJSON_DetachItemFromArray(cJSON *array, int which);
|
||||
CJSON_PUBLIC(void) cJSON_DeleteItemFromArray(cJSON *array, int which);
|
||||
CJSON_PUBLIC(cJSON *)cJSON_DetachItemFromObject(cJSON *object, const char *string);
|
||||
CJSON_PUBLIC(cJSON *)cJSON_DetachItemFromObjectCaseSensitive(cJSON *object, const char *string);
|
||||
CJSON_PUBLIC(void) cJSON_DeleteItemFromObject(cJSON *object, const char *string);
|
||||
CJSON_PUBLIC(void) cJSON_DeleteItemFromObjectCaseSensitive(cJSON *object, const char *string);
|
||||
|
||||
/* Update array items. */
|
||||
CJSON_PUBLIC(void)
|
||||
cJSON_InsertItemInArray(cJSON *array, int which, cJSON *newitem); /* Shifts pre-existing items to the right. */
|
||||
CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemViaPointer(cJSON *const parent, cJSON *const item, cJSON *replacement);
|
||||
CJSON_PUBLIC(void) cJSON_ReplaceItemInArray(cJSON *array, int which, cJSON *newitem);
|
||||
CJSON_PUBLIC(void) cJSON_ReplaceItemInObject(cJSON *object, const char *string, cJSON *newitem);
|
||||
CJSON_PUBLIC(void) cJSON_ReplaceItemInObjectCaseSensitive(cJSON *object, const char *string, cJSON *newitem);
|
||||
|
||||
/* Duplicate a cJSON item */
|
||||
CJSON_PUBLIC(cJSON *)cJSON_Duplicate(const cJSON *item, cJSON_bool recurse);
|
||||
/* Duplicate will create a new, identical cJSON item to the one you pass, in new memory that will
|
||||
* need to be released. With recurse!=0, it will duplicate any children connected to the item.
|
||||
* The item->next and ->prev pointers are always zero on return from Duplicate. */
|
||||
/* Recursively compare two cJSON items for equality. If either a or b is NULL or invalid, they will be considered unequal.
|
||||
* case_sensitive determines if object keys are treated case sensitive (1) or case insensitive (0) */
|
||||
CJSON_PUBLIC(cJSON_bool) cJSON_Compare(const cJSON *const a, const cJSON *const b, const cJSON_bool case_sensitive);
|
||||
|
||||
/* Minify a strings, remove blank characters(such as ' ', '\t', '\r', '\n') from strings.
|
||||
* The input pointer json cannot point to a read-only address area, such as a string constant,
|
||||
* but should point to a readable and writable adress area. */
|
||||
CJSON_PUBLIC(void) cJSON_Minify(char *json);
|
||||
|
||||
/* Helper functions for creating and adding items to an object at the same time.
|
||||
* They return the added item or NULL on failure. */
|
||||
CJSON_PUBLIC(cJSON*)cJSON_AddNullToObject(cJSON *const object, const char *const name);
|
||||
CJSON_PUBLIC(cJSON*)cJSON_AddTrueToObject(cJSON *const object, const char *const name);
|
||||
CJSON_PUBLIC(cJSON*)cJSON_AddFalseToObject(cJSON *const object, const char *const name);
|
||||
CJSON_PUBLIC(cJSON*)cJSON_AddBoolToObject(cJSON *const object, const char *const name, const cJSON_bool boolean);
|
||||
CJSON_PUBLIC(cJSON*)cJSON_AddNumberToObject(cJSON *const object, const char *const name, const double number);
|
||||
CJSON_PUBLIC(cJSON*)cJSON_AddStringToObject(cJSON *const object, const char *const name, const char *const string);
|
||||
CJSON_PUBLIC(cJSON*)cJSON_AddRawToObject(cJSON *const object, const char *const name, const char *const raw);
|
||||
CJSON_PUBLIC(cJSON*)cJSON_AddObjectToObject(cJSON *const object, const char *const name);
|
||||
CJSON_PUBLIC(cJSON*)cJSON_AddArrayToObject(cJSON *const object, const char *const name);
|
||||
|
||||
/* When assigning an integer value, it needs to be propagated to valuedouble too. */
|
||||
#define cJSON_SetIntValue(object, number) ((object) ? (object)->valueint = (object)->valuedouble = (number) : (number))
|
||||
/* helper for the cJSON_SetNumberValue macro */
|
||||
CJSON_PUBLIC(double) cJSON_SetNumberHelper(cJSON *object, double number);
|
||||
#define cJSON_SetNumberValue(object, number) ((object != NULL) ? cJSON_SetNumberHelper(object, (double)number) : (number))
|
||||
|
||||
/* Macro for iterating over an array or object */
|
||||
#define cJSON_ArrayForEach(element, array) for(element = (array != NULL) ? (array)->child : NULL; element != NULL; element = element->next)
|
||||
|
||||
/* malloc/free objects using the malloc/free functions that have been set with cJSON_InitHooks */
|
||||
CJSON_PUBLIC(void *)cJSON_malloc(size_t size);
|
||||
CJSON_PUBLIC(void) cJSON_free(void *object);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
359
samples/sample_c/module_sample/utils/dji_config_manager.c
Normal file
359
samples/sample_c/module_sample/utils/dji_config_manager.c
Normal file
@ -0,0 +1,359 @@
|
||||
/**
|
||||
********************************************************************
|
||||
* @file dji_config_manager.c
|
||||
* @brief
|
||||
*
|
||||
* @copyright (c) 2018 DJI. All rights reserved.
|
||||
*
|
||||
* All information contained herein is, and remains, the property of DJI.
|
||||
* The intellectual and technical concepts contained herein are proprietary
|
||||
* to DJI and may be covered by U.S. and foreign patents, patents in process,
|
||||
* and protected by trade secret or copyright law. Dissemination of this
|
||||
* information, including but not limited to data and other proprietary
|
||||
* material(s) incorporated within the information, in any form, is strictly
|
||||
* prohibited without the express written consent of DJI.
|
||||
*
|
||||
* If you receive this source code without DJI’s authorization, you may not
|
||||
* further disseminate the information, and you must immediately remove the
|
||||
* source code and notify DJI of its removal. DJI reserves the right to pursue
|
||||
* legal actions against you for any loss(es) or damage(s) caused by your
|
||||
* failure to do so.
|
||||
*
|
||||
*********************************************************************
|
||||
*/
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include <utils/util_misc.h>
|
||||
#include <dji_logger.h>
|
||||
#include <utils/util_file.h>
|
||||
#include <dji_aircraft_info.h>
|
||||
#include "dji_config_manager.h"
|
||||
#include "utils/cJSON.h"
|
||||
|
||||
/* Private constants ---------------------------------------------------------*/
|
||||
|
||||
/* Private types -------------------------------------------------------------*/
|
||||
|
||||
/* Private values -------------------------------------------------------------*/
|
||||
static T_DjiUserInfo s_configManagerUserInfo = {0};
|
||||
static T_DjiUserLinkConfig s_configManagerLinkInfo = {0};
|
||||
static bool s_configManagerIsEnable = false;
|
||||
|
||||
/* Private functions declaration ---------------------------------------------*/
|
||||
static T_DjiReturnCode DjiUserConfigManager_GetAppInfoInner(const char *path, T_DjiUserInfo *userInfo);
|
||||
static T_DjiReturnCode DjiUserConfigManager_GetLinkConfigInner(const char *path, T_DjiUserLinkConfig *linkConfig);
|
||||
|
||||
/* Exported functions definition ---------------------------------------------*/
|
||||
T_DjiReturnCode DjiUserConfigManager_LoadConfiguration(const char *path)
|
||||
{
|
||||
T_DjiReturnCode returnCode;
|
||||
|
||||
if (path == NULL) {
|
||||
perror("Config file path is null.\n");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER;
|
||||
}
|
||||
|
||||
printf("Load configuration start, config file path is: %s\r\n", path);
|
||||
|
||||
returnCode = DjiUserConfigManager_GetAppInfoInner(path, &s_configManagerUserInfo);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
perror("Get app info failed.\n");
|
||||
}
|
||||
|
||||
returnCode = DjiUserConfigManager_GetLinkConfigInner(path, &s_configManagerLinkInfo);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
perror("Get link info failed.\n");
|
||||
}
|
||||
|
||||
printf("\r\nLoad configuration successfully.\r\n");
|
||||
|
||||
s_configManagerIsEnable = true;
|
||||
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
void DjiUserConfigManager_GetAppInfo(T_DjiUserInfo *userInfo)
|
||||
{
|
||||
memcpy(userInfo, &s_configManagerUserInfo, sizeof(T_DjiUserInfo));
|
||||
}
|
||||
|
||||
void DjiUserConfigManager_GetLinkConfig(T_DjiUserLinkConfig *linkConfig)
|
||||
{
|
||||
memcpy(linkConfig, &s_configManagerLinkInfo, sizeof(T_DjiUserLinkConfig));
|
||||
}
|
||||
|
||||
bool DjiUserConfigManager_IsEnable(void)
|
||||
{
|
||||
return s_configManagerIsEnable;
|
||||
}
|
||||
|
||||
/* Private functions definition-----------------------------------------------*/
|
||||
static T_DjiReturnCode DjiUserConfigManager_GetAppInfoInner(const char *path, T_DjiUserInfo *userInfo)
|
||||
{
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
T_DjiReturnCode returnCode;
|
||||
uint32_t fileSize = 0;
|
||||
uint32_t readRealSize = 0;
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
uint8_t *jsonData = NULL;
|
||||
cJSON *jsonRoot = NULL;
|
||||
cJSON *jsonItem = NULL;
|
||||
cJSON *jsonValue = NULL;
|
||||
|
||||
returnCode = UtilFile_GetFileSizeByPath(path, &fileSize);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Get file size by path failed, stat = 0x%08llX", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
USER_LOG_DEBUG("Get config json file size is %d", fileSize);
|
||||
|
||||
jsonData = osalHandler->Malloc(fileSize + 1);
|
||||
if (jsonData == NULL) {
|
||||
USER_LOG_ERROR("Malloc failed.");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
memset(jsonData, 0, fileSize);
|
||||
|
||||
UtilFile_GetFileDataByPath(path, 0, fileSize, jsonData, &readRealSize);
|
||||
|
||||
jsonData[readRealSize] = '\0';
|
||||
|
||||
jsonRoot = cJSON_Parse((char *) jsonData);
|
||||
if (jsonRoot == NULL) {
|
||||
returnCode = DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
goto jsonDataFree;
|
||||
}
|
||||
|
||||
jsonItem = cJSON_GetObjectItem(jsonRoot, "dji_sdk_app_info");
|
||||
if (jsonItem != NULL) {
|
||||
jsonValue = cJSON_GetObjectItem(jsonItem, "user_app_name");
|
||||
if (jsonValue != NULL) {
|
||||
strcpy(userInfo->appName, jsonValue->valuestring);
|
||||
}
|
||||
|
||||
jsonValue = cJSON_GetObjectItem(jsonItem, "user_app_id");
|
||||
if (jsonValue != NULL) {
|
||||
strcpy(userInfo->appId, jsonValue->valuestring);
|
||||
}
|
||||
|
||||
jsonValue = cJSON_GetObjectItem(jsonItem, "user_app_key");
|
||||
if (jsonValue != NULL) {
|
||||
strcpy(userInfo->appKey, jsonValue->valuestring);
|
||||
}
|
||||
|
||||
jsonValue = cJSON_GetObjectItem(jsonItem, "user_app_license");
|
||||
if (jsonValue != NULL) {
|
||||
strcpy(userInfo->appLicense, jsonValue->valuestring);
|
||||
}
|
||||
|
||||
jsonValue = cJSON_GetObjectItem(jsonItem, "user_develop_account");
|
||||
if (jsonValue != NULL) {
|
||||
strcpy(userInfo->developerAccount, jsonValue->valuestring);
|
||||
}
|
||||
|
||||
jsonValue = cJSON_GetObjectItem(jsonItem, "user_baud_rate");
|
||||
if (jsonValue != NULL) {
|
||||
strcpy(userInfo->baudRate, jsonValue->valuestring);
|
||||
}
|
||||
}
|
||||
|
||||
if (strlen(userInfo->appName) >= sizeof(userInfo->appName) ||
|
||||
strlen(userInfo->appId) > sizeof(userInfo->appId) ||
|
||||
strlen(userInfo->appKey) > sizeof(userInfo->appKey) ||
|
||||
strlen(userInfo->appLicense) > sizeof(userInfo->appLicense) ||
|
||||
strlen(userInfo->developerAccount) >= sizeof(userInfo->developerAccount) ||
|
||||
strlen(userInfo->baudRate) > sizeof(userInfo->baudRate)) {
|
||||
USER_LOG_ERROR("Length of user information string is beyond limit. Please check.");
|
||||
returnCode = DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER;
|
||||
goto jsonDataFree;
|
||||
}
|
||||
|
||||
if (!strcmp(userInfo->appName, "your_app_name") ||
|
||||
!strcmp(userInfo->appId, "your_app_id") ||
|
||||
!strcmp(userInfo->appKey, "your_app_key") ||
|
||||
!strcmp(userInfo->appLicense, "your_app_license") ||
|
||||
!strcmp(userInfo->developerAccount, "your_developer_account") ||
|
||||
!strcmp(userInfo->baudRate, "your_baud_rate")) {
|
||||
USER_LOG_ERROR(
|
||||
"Please fill in correct user information to 'samples/sample_c++/platform/linux/manifold2/application/dji_sdk_config.json' file.");
|
||||
returnCode = DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER;
|
||||
goto jsonDataFree;
|
||||
}
|
||||
|
||||
jsonDataFree:
|
||||
osalHandler->Free(jsonData);
|
||||
#endif
|
||||
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
static T_DjiReturnCode DjiUserConfigManager_GetLinkConfigInner(const char *path, T_DjiUserLinkConfig *linkConfig)
|
||||
{
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
T_DjiReturnCode returnCode;
|
||||
uint32_t fileSize = 0;
|
||||
uint32_t readRealSize = 0;
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
uint8_t *jsonData = NULL;
|
||||
cJSON *jsonRoot = NULL;
|
||||
cJSON *jsonItem = NULL;
|
||||
cJSON *jsonValue = NULL;
|
||||
cJSON *jsonConfig = NULL;
|
||||
int32_t configValue;
|
||||
|
||||
returnCode = UtilFile_GetFileSizeByPath(path, &fileSize);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Get file size by path failed, stat = 0x%08llX", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
USER_LOG_DEBUG("Get config json file size is %d", fileSize);
|
||||
|
||||
jsonData = osalHandler->Malloc(fileSize + 1);
|
||||
if (jsonData == NULL) {
|
||||
USER_LOG_ERROR("Malloc failed.");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
memset(jsonData, 0, fileSize);
|
||||
|
||||
UtilFile_GetFileDataByPath(path, 0, fileSize, jsonData, &readRealSize);
|
||||
|
||||
jsonData[readRealSize] = '\0';
|
||||
|
||||
jsonRoot = cJSON_Parse((char *) jsonData);
|
||||
if (jsonRoot == NULL) {
|
||||
goto jsonDataFree;
|
||||
}
|
||||
|
||||
jsonItem = cJSON_GetObjectItem(jsonRoot, "dji_sdk_link_config");
|
||||
if (jsonItem != NULL) {
|
||||
jsonValue = cJSON_GetObjectItem(jsonItem, "link_select");
|
||||
if (jsonValue != NULL) {
|
||||
printf("\r\nSelect link type: %s\r\n", jsonValue->valuestring);
|
||||
if (strcmp(jsonValue->valuestring, "use_only_uart") == 0) {
|
||||
linkConfig->type = DJI_USER_LINK_CONFIG_USE_ONLY_UART;
|
||||
} else if (strcmp(jsonValue->valuestring, "use_uart_and_network_device") == 0) {
|
||||
linkConfig->type = DJI_USER_LINK_CONFIG_USE_UART_AND_NETWORK_DEVICE;
|
||||
} else if (strcmp(jsonValue->valuestring, "use_uart_and_usb_bulk_device") == 0) {
|
||||
linkConfig->type = DJI_USER_LINK_CONFIG_USE_UART_AND_USB_BULK_DEVICE;
|
||||
} else if (strcmp(jsonValue->valuestring, "use_only_usb_bulk_device") == 0) {
|
||||
linkConfig->type = DJI_USER_LINK_CONFIG_USE_ONLY_USB_BULK_DEVICE;
|
||||
} else if (strcmp(jsonValue->valuestring, "use_only_network_device") == 0) {
|
||||
linkConfig->type = DJI_USER_LINK_CONFIG_USE_ONLY_NETWORK_DEVICE;
|
||||
}
|
||||
}
|
||||
|
||||
jsonValue = cJSON_GetObjectItem(jsonItem, "uart_config");
|
||||
if (jsonValue != NULL) {
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "uart1_device_name");
|
||||
printf("\r\nConfig uart1 device name: %s\r\n", jsonConfig->valuestring);
|
||||
strcpy(linkConfig->uartConfig.uart1DeviceName, jsonConfig->valuestring);
|
||||
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "uart2_device_name");
|
||||
printf("Config uart2 device name: %s\r\n", jsonConfig->valuestring);
|
||||
strcpy(linkConfig->uartConfig.uart2DeviceName, jsonConfig->valuestring);
|
||||
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "uart2_device_enable");
|
||||
printf("Config uart2 device enable: %s\r\n", jsonConfig->valuestring);
|
||||
if (strcmp(jsonConfig->valuestring, "true") == 0) {
|
||||
linkConfig->uartConfig.uart2DeviceEnable = true;
|
||||
} else {
|
||||
linkConfig->uartConfig.uart2DeviceEnable = false;
|
||||
}
|
||||
}
|
||||
|
||||
jsonValue = cJSON_GetObjectItem(jsonItem, "network_config");
|
||||
if (jsonValue != NULL) {
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "network_device_name");
|
||||
printf("\r\nConfig network device name: %s\r\n", jsonConfig->valuestring);
|
||||
strcpy(linkConfig->networkConfig.networkDeviceName, jsonConfig->valuestring);
|
||||
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "network_usb_adapter_vid");
|
||||
printf("Config network usb adapter vid: %s\r\n", jsonConfig->valuestring);
|
||||
sscanf(jsonConfig->valuestring, "%X", &configValue);
|
||||
linkConfig->networkConfig.networkUsbAdapterVid = configValue;
|
||||
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "network_usb_adapter_pid");
|
||||
printf("Config network usb adapter vid: %s\r\n", jsonConfig->valuestring);
|
||||
sscanf(jsonConfig->valuestring, "%X", &configValue);
|
||||
linkConfig->networkConfig.networkUsbAdapterPid = configValue;
|
||||
}
|
||||
|
||||
jsonValue = cJSON_GetObjectItem(jsonItem, "usb_bulk_config");
|
||||
if (jsonValue != NULL) {
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "usb_device_vid");
|
||||
printf("\r\nConfig usb device vid: %s\r\n", jsonConfig->valuestring);
|
||||
sscanf(jsonConfig->valuestring, "%X", &configValue);
|
||||
linkConfig->usbBulkConfig.usbDeviceVid = configValue;
|
||||
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "usb_device_pid");
|
||||
printf("Config usb device pid: %s\r\n", jsonConfig->valuestring);
|
||||
sscanf(jsonConfig->valuestring, "%X", &configValue);
|
||||
linkConfig->usbBulkConfig.usbDevicePid = configValue;
|
||||
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "usb_bulk1_device_name");
|
||||
printf("Config usb bulk1 device name: %s\r\n", jsonConfig->valuestring);
|
||||
strcpy(linkConfig->usbBulkConfig.usbBulk1DeviceName, jsonConfig->valuestring);
|
||||
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "usb_bulk1_interface_num");
|
||||
printf("Config usb bulk1 interface num: %s\r\n", jsonConfig->valuestring);
|
||||
sscanf(jsonConfig->valuestring, "%X", &configValue);
|
||||
linkConfig->usbBulkConfig.usbBulk1InterfaceNum = configValue;
|
||||
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "usb_bulk1_endpoint_in");
|
||||
printf("Config usb bulk1 endpoint in: %s\r\n", jsonConfig->valuestring);
|
||||
sscanf(jsonConfig->valuestring, "%X", &configValue);
|
||||
linkConfig->usbBulkConfig.usbBulk1EndpointIn = configValue;
|
||||
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "usb_bulk1_endpoint_out");
|
||||
printf("Config usb bulk1 endpoint out: %s\r\n", jsonConfig->valuestring);
|
||||
sscanf(jsonConfig->valuestring, "%X", &configValue);
|
||||
linkConfig->usbBulkConfig.usbBulk1EndpointOut = configValue;
|
||||
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "usb_bulk2_device_name");
|
||||
printf("Config usb bulk2 device name: %s\r\n", jsonConfig->valuestring);
|
||||
strcpy(linkConfig->usbBulkConfig.usbBulk2DeviceName, jsonConfig->valuestring);
|
||||
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "usb_bulk2_interface_num");
|
||||
printf("Config usb bulk2 interface num: %s\r\n", jsonConfig->valuestring);
|
||||
sscanf(jsonConfig->valuestring, "%X", &configValue);
|
||||
linkConfig->usbBulkConfig.usbBulk2InterfaceNum = configValue;
|
||||
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "usb_bulk2_endpoint_in");
|
||||
printf("Config usb bulk2 endpoint in: %s\r\n", jsonConfig->valuestring);
|
||||
linkConfig->usbBulkConfig.usbBulk2EndpointIn = configValue;
|
||||
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "usb_bulk2_endpoint_out");
|
||||
printf("Config usb bulk2 endpoint out: %s\r\n", jsonConfig->valuestring);
|
||||
linkConfig->usbBulkConfig.usbBulk2EndpointOut = configValue;
|
||||
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "usb_bulk3_device_name");
|
||||
printf("Config usb bulk2 device name: %s\r\n", jsonConfig->valuestring);
|
||||
strcpy(linkConfig->usbBulkConfig.usbBulk3DeviceName, jsonConfig->valuestring);
|
||||
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "usb_bulk3_interface_num");
|
||||
printf("Config usb bulk2 interface num: %s\r\n", jsonConfig->valuestring);
|
||||
sscanf(jsonConfig->valuestring, "%X", &configValue);
|
||||
linkConfig->usbBulkConfig.usbBulk3InterfaceNum = configValue;
|
||||
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "usb_bulk3_endpoint_in");
|
||||
printf("Config usb bulk2 endpoint in: %s\r\n", jsonConfig->valuestring);
|
||||
linkConfig->usbBulkConfig.usbBulk3EndpointIn = configValue;
|
||||
|
||||
jsonConfig = cJSON_GetObjectItem(jsonValue, "usb_bulk3_endpoint_out");
|
||||
printf("Config usb bulk2 endpoint out: %s\r\n", jsonConfig->valuestring);
|
||||
linkConfig->usbBulkConfig.usbBulk3EndpointOut = configValue;
|
||||
}
|
||||
}
|
||||
|
||||
jsonDataFree:
|
||||
osalHandler->Free(jsonData);
|
||||
#endif
|
||||
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/
|
||||
95
samples/sample_c/module_sample/utils/dji_config_manager.h
Normal file
95
samples/sample_c/module_sample/utils/dji_config_manager.h
Normal file
@ -0,0 +1,95 @@
|
||||
/**
|
||||
********************************************************************
|
||||
* @file dji_config_manager.h
|
||||
* @brief This is the header file for "dji_config_manager.c", defining the structure and
|
||||
* (exported) function prototypes.
|
||||
*
|
||||
* @copyright (c) 2018 DJI. All rights reserved.
|
||||
*
|
||||
* All information contained herein is, and remains, the property of DJI.
|
||||
* The intellectual and technical concepts contained herein are proprietary
|
||||
* to DJI and may be covered by U.S. and foreign patents, patents in process,
|
||||
* and protected by trade secret or copyright law. Dissemination of this
|
||||
* information, including but not limited to data and other proprietary
|
||||
* material(s) incorporated within the information, in any form, is strictly
|
||||
* prohibited without the express written consent of DJI.
|
||||
*
|
||||
* If you receive this source code without DJI’s authorization, you may not
|
||||
* further disseminate the information, and you must immediately remove the
|
||||
* source code and notify DJI of its removal. DJI reserves the right to pursue
|
||||
* legal actions against you for any loss(es) or damage(s) caused by your
|
||||
* failure to do so.
|
||||
*
|
||||
*********************************************************************
|
||||
*/
|
||||
|
||||
/* Define to prevent recursive inclusion -------------------------------------*/
|
||||
#ifndef DJI_CONFIG_MANAGER_H
|
||||
#define DJI_CONFIG_MANAGER_H
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include "dji_platform.h"
|
||||
#include "dji_core.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Exported constants --------------------------------------------------------*/
|
||||
#define USER_DEVICE_NAME_STR_MAX_SIZE (64)
|
||||
|
||||
/* Exported types ------------------------------------------------------------*/
|
||||
typedef enum {
|
||||
DJI_USER_LINK_CONFIG_USE_ONLY_UART,
|
||||
DJI_USER_LINK_CONFIG_USE_UART_AND_NETWORK_DEVICE,
|
||||
DJI_USER_LINK_CONFIG_USE_UART_AND_USB_BULK_DEVICE,
|
||||
DJI_USER_LINK_CONFIG_USE_ONLY_USB_BULK_DEVICE,
|
||||
DJI_USER_LINK_CONFIG_USE_ONLY_NETWORK_DEVICE,
|
||||
} E_DjiUserLinkConfigType;
|
||||
|
||||
typedef struct {
|
||||
E_DjiUserLinkConfigType type;
|
||||
struct {
|
||||
char uart1DeviceName[USER_DEVICE_NAME_STR_MAX_SIZE];
|
||||
bool uart2DeviceEnable;
|
||||
char uart2DeviceName[USER_DEVICE_NAME_STR_MAX_SIZE];
|
||||
} uartConfig;
|
||||
struct {
|
||||
char networkDeviceName[USER_DEVICE_NAME_STR_MAX_SIZE];
|
||||
// M300/M350 RTK payload port no need
|
||||
uint16_t networkUsbAdapterVid;
|
||||
uint16_t networkUsbAdapterPid;
|
||||
} networkConfig;
|
||||
struct {
|
||||
uint16_t usbDeviceVid;
|
||||
uint16_t usbDevicePid;
|
||||
|
||||
char usbBulk1DeviceName[USER_DEVICE_NAME_STR_MAX_SIZE];
|
||||
uint8_t usbBulk1InterfaceNum;
|
||||
uint8_t usbBulk1EndpointIn;
|
||||
uint8_t usbBulk1EndpointOut;
|
||||
|
||||
char usbBulk2DeviceName[USER_DEVICE_NAME_STR_MAX_SIZE];
|
||||
uint8_t usbBulk2InterfaceNum;
|
||||
uint8_t usbBulk2EndpointIn;
|
||||
uint8_t usbBulk2EndpointOut;
|
||||
|
||||
char usbBulk3DeviceName[USER_DEVICE_NAME_STR_MAX_SIZE];
|
||||
uint8_t usbBulk3InterfaceNum;
|
||||
uint8_t usbBulk3EndpointIn;
|
||||
uint8_t usbBulk3EndpointOut;
|
||||
} usbBulkConfig;
|
||||
} T_DjiUserLinkConfig;
|
||||
|
||||
/* Exported functions --------------------------------------------------------*/
|
||||
T_DjiReturnCode DjiUserConfigManager_LoadConfiguration(const char *path);
|
||||
void DjiUserConfigManager_GetAppInfo(T_DjiUserInfo *userInfo);
|
||||
void DjiUserConfigManager_GetLinkConfig(T_DjiUserLinkConfig *linkConfig);
|
||||
bool DjiUserConfigManager_IsEnable(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // DJI_CONFIG_MANAGER_H
|
||||
/************************ (C) COPYRIGHT DJI Innovations *******END OF FILE******/
|
||||
@ -89,8 +89,8 @@ T_DjiReturnCode UtilFile_GetFileSizeByPath(const char *filePath, uint32_t *fileS
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
T_DjiReturnCode UtilFile_GetFileDataByPath(const char *filePath, uint32_t offset, uint16_t len,
|
||||
uint8_t *data, uint16_t *realLen)
|
||||
T_DjiReturnCode UtilFile_GetFileDataByPath(const char *filePath, uint32_t offset, uint32_t len,
|
||||
uint8_t *data, uint32_t *realLen)
|
||||
{
|
||||
FILE *pF;
|
||||
T_DjiReturnCode psdkStat;
|
||||
|
||||
@ -52,8 +52,8 @@ typedef struct {
|
||||
/* Exported functions --------------------------------------------------------*/
|
||||
T_DjiReturnCode UtilFile_GetCreateTime(const char *filePath, T_UtilFileCreateTime *createTime);
|
||||
T_DjiReturnCode UtilFile_GetFileSizeByPath(const char *filePath, uint32_t *fileSize);
|
||||
T_DjiReturnCode UtilFile_GetFileDataByPath(const char *filePath, uint32_t offset, uint16_t len,
|
||||
uint8_t *data, uint16_t *realLen);
|
||||
T_DjiReturnCode UtilFile_GetFileDataByPath(const char *filePath, uint32_t offset, uint32_t len,
|
||||
uint8_t *data, uint32_t *realLen);
|
||||
T_DjiReturnCode DjiFile_Delete(const char *filePath);
|
||||
|
||||
T_DjiReturnCode UtilFile_GetFileSize(FILE *file, uint32_t *fileSize);
|
||||
|
||||
218
samples/sample_c/module_sample/utils/util_link_list.c
Normal file
218
samples/sample_c/module_sample/utils/util_link_list.c
Normal file
@ -0,0 +1,218 @@
|
||||
/**
|
||||
********************************************************************
|
||||
* @file util_link_list.c
|
||||
* @brief
|
||||
*
|
||||
* @copyright (c) 2021 DJI. All rights reserved.
|
||||
*
|
||||
* All information contained herein is, and remains, the property of DJI.
|
||||
* The intellectual and technical concepts contained herein are proprietary
|
||||
* to DJI and may be covered by U.S. and foreign patents, patents in process,
|
||||
* and protected by trade secret or copyright law. Dissemination of this
|
||||
* information, including but not limited to data and other proprietary
|
||||
* material(s) incorporated within the information, in any form, is strictly
|
||||
* prohibited without the express written consent of DJI.
|
||||
*
|
||||
* If you receive this source code without DJI’s authorization, you may not
|
||||
* further disseminate the information, and you must immediately remove the
|
||||
* source code and notify DJI of its removal. DJI reserves the right to pursue
|
||||
* legal actions against you for any loss(es) or damage(s) caused by your
|
||||
* failure to do so.
|
||||
*
|
||||
*********************************************************************
|
||||
*/
|
||||
|
||||
#define UTIL_LINK_LIST_C
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include "util_link_list.h"
|
||||
|
||||
/* Private constants ---------------------------------------------------------*/
|
||||
|
||||
/* Private types -------------------------------------------------------------*/
|
||||
|
||||
/* Private functions declaration ---------------------------------------------*/
|
||||
|
||||
/* Private values ------------------------------------------------------------*/
|
||||
|
||||
/* Exported functions definition ---------------------------------------------*/
|
||||
void DjiUserUtil_ListNodeDeleteDataOnly( T_UtilListNode *node )
|
||||
{
|
||||
if ( NULL == node) {
|
||||
UTIL_REPORT_ERROR( "null pointer" );
|
||||
return;
|
||||
}
|
||||
|
||||
if ( NULL != node->data ) {
|
||||
UTIL_OSAL_MEMRY_FREE( node->data );
|
||||
node->data = NULL;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void DjiUserUtil_ListNodeDeleteNodeSelf( T_UtilListNode *node )
|
||||
{
|
||||
if ( NULL == node) {
|
||||
UTIL_REPORT_ERROR( "null pointer" );
|
||||
return;
|
||||
}
|
||||
|
||||
UTIL_OSAL_MEMRY_FREE( node );
|
||||
return;
|
||||
}
|
||||
|
||||
void DjiUserUtil_InitListNode( T_UtilListNode *node, void *data )
|
||||
{
|
||||
if ( NULL == node ) {
|
||||
UTIL_REPORT_ERROR( "null pointer" );
|
||||
return;
|
||||
}
|
||||
|
||||
node->prev = NULL;
|
||||
node->next = NULL;
|
||||
node->data = data;
|
||||
return;
|
||||
}
|
||||
|
||||
T_UtilListNode *DjiUserUtil_NewListNode( void *data )
|
||||
{
|
||||
T_UtilListNode *node = NULL;
|
||||
|
||||
node = (T_UtilListNode *)UTIL_OSAL_MEMRY_ALLOC( sizeof(T_UtilListNode) );
|
||||
if ( NULL == node ) {
|
||||
UTIL_REPORT_ERROR( "null pointer" );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
DjiUserUtil_InitListNode( node, data );
|
||||
return node;
|
||||
}
|
||||
|
||||
void DjiUserUtil_LinkListDestory( T_UtilLinkList *linkList )
|
||||
{
|
||||
T_UtilListNode *node = NULL;
|
||||
T_UtilListNode *next = NULL;
|
||||
|
||||
if ( NULL == linkList ) {
|
||||
UTIL_REPORT_ERROR( "null pointer" );
|
||||
return;
|
||||
}
|
||||
|
||||
for ( node = linkList->first; NULL != node ; node = next ) {
|
||||
next = node->next;
|
||||
DjiUserUtil_ListNodeDeleteDataOnly( node );
|
||||
DjiUserUtil_ListNodeDeleteNodeSelf( node );
|
||||
}
|
||||
}
|
||||
void DjiUserUtil_LinkListAddNodeFirst( T_UtilLinkList *linkList, T_UtilListNode *node )
|
||||
{
|
||||
if ( ( NULL == linkList ) || ( NULL == node ) ) {
|
||||
UTIL_REPORT_ERROR( "null pointer" );
|
||||
return;
|
||||
}
|
||||
|
||||
if ( UTIL_LINKLIST_IS_EMPTY( *linkList ) ) {
|
||||
node->prev = NULL;
|
||||
node->next = NULL;
|
||||
linkList->first = node;
|
||||
linkList->last = node;
|
||||
} else {
|
||||
node->prev = NULL;
|
||||
node->next = linkList->first;
|
||||
linkList->first = node;
|
||||
node->next->prev = node;
|
||||
}
|
||||
linkList->count ++;
|
||||
return;
|
||||
}
|
||||
|
||||
void DjiUserUtil_LinkListAddNodeLast( T_UtilLinkList *linkList, T_UtilListNode *node )
|
||||
{
|
||||
if ( ( NULL == linkList ) || ( NULL == node ) ) {
|
||||
UTIL_REPORT_ERROR( "null pointer" );
|
||||
return;
|
||||
}
|
||||
|
||||
if ( UTIL_LINKLIST_IS_EMPTY( *linkList ) ) {
|
||||
node->prev = NULL;
|
||||
node->next = NULL;
|
||||
linkList->first = node;
|
||||
linkList->last = node;
|
||||
} else {
|
||||
node->next = NULL;
|
||||
node->prev = linkList->last;
|
||||
linkList->last = node;
|
||||
node->prev->next = node;
|
||||
}
|
||||
linkList->count ++;
|
||||
return;
|
||||
}
|
||||
|
||||
void DjiUserUtil_InitLinkList( T_UtilLinkList *linkList )
|
||||
{
|
||||
if ( NULL == linkList ) {
|
||||
UTIL_REPORT_ERROR( "null pointer" );
|
||||
return;
|
||||
}
|
||||
|
||||
linkList->first = NULL;
|
||||
linkList->last = NULL;
|
||||
linkList->count = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
T_UtilLinkList *DjiUserUtil_NewLinkList( void )
|
||||
{
|
||||
T_UtilLinkList *linkList = NULL;
|
||||
|
||||
linkList = (T_UtilLinkList *)UTIL_OSAL_MEMRY_ALLOC( sizeof( T_UtilLinkList ) );
|
||||
if ( NULL == linkList ) {
|
||||
UTIL_REPORT_ERROR( "null pointer" );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
DjiUserUtil_InitLinkList( linkList );
|
||||
return linkList;
|
||||
}
|
||||
|
||||
void DjiUserUtil_LinkListRemoveNodeOnly( T_UtilLinkList *linkList, T_UtilListNode *node )
|
||||
{
|
||||
if ( ( NULL == linkList ) || ( NULL == node ) ) {
|
||||
UTIL_REPORT_ERROR( "null pointer" );
|
||||
return;
|
||||
}
|
||||
|
||||
if ( node == linkList->first ) {
|
||||
linkList->first = linkList->first->next;
|
||||
}
|
||||
|
||||
if ( node == linkList->last ) {
|
||||
linkList->last = linkList->last->prev;
|
||||
}
|
||||
|
||||
if ( NULL != node->next ) {
|
||||
node->next->prev = node->prev;
|
||||
}
|
||||
|
||||
if ( NULL != node->prev ) {
|
||||
node->prev->next = node->next;
|
||||
}
|
||||
|
||||
if ( 0 != linkList->count ) {
|
||||
linkList->count --;
|
||||
}
|
||||
|
||||
DjiUserUtil_ListNodeDeleteNodeSelf( node );
|
||||
return;
|
||||
}
|
||||
/* Private functions definition-----------------------------------------------*/
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/
|
||||
94
samples/sample_c/module_sample/utils/util_link_list.h
Normal file
94
samples/sample_c/module_sample/utils/util_link_list.h
Normal file
@ -0,0 +1,94 @@
|
||||
/**
|
||||
********************************************************************
|
||||
* @file link_list.h
|
||||
* @brief This is the header file for "link_list.c", defining the structure and
|
||||
* (exported) function prototypes.
|
||||
*
|
||||
* @copyright (c) 2021 DJI. All rights reserved.
|
||||
*
|
||||
* All information contained herein is, and remains, the property of DJI.
|
||||
* The intellectual and technical concepts contained herein are proprietary
|
||||
* to DJI and may be covered by U.S. and foreign patents, patents in process,
|
||||
* and protected by trade secret or copyright law. Dissemination of this
|
||||
* information, including but not limited to data and other proprietary
|
||||
* material(s) incorporated within the information, in any form, is strictly
|
||||
* prohibited without the express written consent of DJI.
|
||||
*
|
||||
* If you receive this source code without DJI’s authorization, you may not
|
||||
* further disseminate the information, and you must immediately remove the
|
||||
* source code and notify DJI of its removal. DJI reserves the right to pursue
|
||||
* legal actions against you for any loss(es) or damage(s) caused by your
|
||||
* failure to do so.
|
||||
*
|
||||
*********************************************************************
|
||||
*/
|
||||
|
||||
/* Define to prevent recursive inclusion -------------------------------------*/
|
||||
#ifndef UTIL_LINK_LIST_H
|
||||
#define UTIL_LINK_LIST_H
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include "dji_platform.h"
|
||||
|
||||
#ifdef UTIL_LINK_LIST_C
|
||||
#define UTIL_LINK_LIST_EXT
|
||||
#else
|
||||
#define UTIL_LINK_LIST_EXT extern
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Exported constants --------------------------------------------------------*/
|
||||
#define UTIL_REPORT_ERROR( exp... )
|
||||
|
||||
#define UTIL_OSAL_MEMRY_ALLOC(size) ( { T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler(); osalHandler->Malloc( size ); } )
|
||||
#define UTIL_OSAL_MEMRY_FREE(ptr) ( { T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler(); osalHandler->Free( ptr ); } )
|
||||
|
||||
#define UTIL_OSAL_SEM_FREE_MAY_RETURN(sem) ( { T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler(); if( DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS != osalHandler->SemaphoreDestroy( sem ) ) { return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER; } } )
|
||||
#define UTIL_OSAL_SEM_INIT_MAY_RETURN(sem) ( { T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler(); if( DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS != osalHandler->SemaphoreCreate( 0, sem ) ) { return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER; } } )
|
||||
#define UTIL_OSAL_SEM_POST_MAY_RETURN(sem) ( { T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler(); if( DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS != osalHandler->SemaphorePost( sem ) ) { return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER; } } )
|
||||
#define UTIL_OSAL_SEM_WAIT_MAY_RETURN(sem) ( { T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler(); if( DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS != osalHandler->SemaphoreWait( sem ) ) { return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER; } } )
|
||||
#define UTIL_OSAL_SEM_WAIT_FOR(sem,ms) ( { T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler(); if( DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS != osalHandler->SemaphoreTimedWait( sem, ms ) ) { return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER; } } )
|
||||
|
||||
#define UTIL_OSAL_MUTEX_FREE_MAY_RETURN(mutex) ( { T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler(); if( DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS != osalHandler->MutexDestroy( mutex ) ) { return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER; } } )
|
||||
#define UTIL_OSAL_MUTEX_INIT_MAY_RETURN(mutex) ( { T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler(); if( DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS != osalHandler->MutexCreate( mutex ) ) { return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER; } } )
|
||||
#define UTIL_OSAL_MUTEX_LOCK_MAY_RETURN(mutex) ( { T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler(); if( DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS != osalHandler->MutexLock( mutex ) ) { return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER; } } )
|
||||
#define UTIL_OSAL_MUTEX_UNLOCK_MAY_RETURN(mutex) ( { T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler(); if( DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS != osalHandler->MutexUnlock( mutex ) ) { return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER; } } )
|
||||
|
||||
#define UTIL_LINKLIST_IS_EMPTY(l) ( ((l).first == NULL) ? true : false )
|
||||
|
||||
/* Exported types ------------------------------------------------------------*/
|
||||
typedef struct tagT_UtilListNode {
|
||||
struct tagT_UtilListNode *next;
|
||||
struct tagT_UtilListNode *prev;
|
||||
void *data;
|
||||
} T_UtilListNode;
|
||||
|
||||
typedef struct tagT_UtilLinkList {
|
||||
T_UtilListNode *first;
|
||||
T_UtilListNode *last;
|
||||
uint32_t count;
|
||||
} T_UtilLinkList;
|
||||
|
||||
/* Exported functions --------------------------------------------------------*/
|
||||
UTIL_LINK_LIST_EXT void DjiUserUtil_ListNodeDeleteDataOnly( T_UtilListNode *node );
|
||||
UTIL_LINK_LIST_EXT void DjiUserUtil_ListNodeDeleteNodeSelf( T_UtilListNode *node );
|
||||
UTIL_LINK_LIST_EXT void DjiUserUtil_InitListNode( T_UtilListNode *node, void *data );
|
||||
UTIL_LINK_LIST_EXT T_UtilListNode *DjiUserUtil_NewListNode( void *data );
|
||||
|
||||
UTIL_LINK_LIST_EXT void DjiUserUtil_LinkListDestory( T_UtilLinkList *linkList );
|
||||
UTIL_LINK_LIST_EXT void DjiUserUtil_LinkListAddNodeFirst( T_UtilLinkList *linkList, T_UtilListNode *node );
|
||||
UTIL_LINK_LIST_EXT void DjiUserUtil_LinkListAddNodeLast( T_UtilLinkList *linkList, T_UtilListNode *node );
|
||||
UTIL_LINK_LIST_EXT void DjiUserUtil_InitLinkList( T_UtilLinkList *linkList );
|
||||
UTIL_LINK_LIST_EXT T_UtilLinkList *DjiUserUtil_NewLinkList( void );
|
||||
|
||||
UTIL_LINK_LIST_EXT void DjiUserUtil_LinkListRemoveNodeOnly( T_UtilLinkList *linkList, T_UtilListNode *node );
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // UTIL_LINK_LIST_H
|
||||
/************************ (C) COPYRIGHT DJI Innovations *******END OF FILE******/
|
||||
@ -37,21 +37,26 @@
|
||||
/* Private types -------------------------------------------------------------*/
|
||||
|
||||
/* Private values -------------------------------------------------------------*/
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
static T_DjiWaypointV3MissionState s_lastWaypointV3MissionState = {0};
|
||||
#endif
|
||||
|
||||
/* Private functions declaration ---------------------------------------------*/
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
static T_DjiReturnCode DjiTest_WaypointV3MissionStateCallback(T_DjiWaypointV3MissionState missionState);
|
||||
static T_DjiReturnCode DjiTest_WaypointV3ActionStateCallback(T_DjiWaypointV3ActionState actionState);
|
||||
#endif
|
||||
static T_DjiReturnCode DjiTest_WaypointV3WaitEndFlightStatus(T_DjiFcSubscriptionFlightStatus status);
|
||||
|
||||
/* Exported functions definition ---------------------------------------------*/
|
||||
T_DjiReturnCode DjiTest_WaypointV3RunSample(void)
|
||||
{
|
||||
T_DjiReturnCode returnCode;
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
T_DjiFcSubscriptionFlightStatus flightStatus = 0;
|
||||
T_DjiDataTimestamp flightStatusTimestamp = {0};
|
||||
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
FILE *kmzFile = NULL;
|
||||
uint32_t kmzFileSize = 0;
|
||||
uint8_t *kmzFileBuf;
|
||||
@ -87,7 +92,7 @@ T_DjiReturnCode DjiTest_WaypointV3RunSample(void)
|
||||
/*! Attention: suggest use the exported kmz file by DJI pilot. If use this test file, you need set the longitude as
|
||||
* 113.94255, latitude as 22.57765 on DJI Assistant 2 simulator */
|
||||
snprintf(tempPath, DJI_TEST_WAYPOINT_V3_KMZ_FILE_PATH_LEN_MAX, "%s/waypoint_file/waypoint_v3_test_file.kmz",
|
||||
curFileDirPath);
|
||||
curFileDirPath);
|
||||
|
||||
kmzFile = fopen(tempPath, "r");
|
||||
if (kmzFile == NULL) {
|
||||
@ -136,8 +141,6 @@ T_DjiReturnCode DjiTest_WaypointV3RunSample(void)
|
||||
goto close_file;
|
||||
}
|
||||
|
||||
osalHandler->TaskSleepMs(2000);
|
||||
|
||||
close_file:
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
returnCode = fclose(kmzFile);
|
||||
@ -155,21 +158,49 @@ close_file:
|
||||
goto out;
|
||||
}
|
||||
|
||||
do {
|
||||
osalHandler->TaskSleepMs(2000);
|
||||
USER_LOG_INFO("The aircraft is on the ground and motors are stoped...");
|
||||
returnCode = DjiTest_WaypointV3WaitEndFlightStatus(DJI_FC_SUBSCRIPTION_FLIGHT_STATUS_STOPED);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Wait end flight status error.");
|
||||
goto out;
|
||||
}
|
||||
|
||||
returnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_STATUS_FLIGHT,
|
||||
(uint8_t *) &flightStatus,
|
||||
sizeof(T_DjiFcSubscriptionFlightStatus),
|
||||
&flightStatusTimestamp);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Get value of topic flight status failed, error code:0x%08llX", returnCode);
|
||||
}
|
||||
USER_LOG_INFO("The aircraft is on the ground and motors are rotating...");
|
||||
returnCode = DjiTest_WaypointV3WaitEndFlightStatus(DJI_FC_SUBSCRIPTION_FLIGHT_STATUS_ON_GROUND);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Wait end flight status error.");
|
||||
goto out;
|
||||
}
|
||||
|
||||
USER_LOG_INFO("flight status: %d", flightStatus);
|
||||
} while(flightStatus == DJI_FC_SUBSCRIPTION_FLIGHT_STATUS_IN_AIR);
|
||||
USER_LOG_INFO("The aircraft is in the air...");
|
||||
returnCode = DjiTest_WaypointV3WaitEndFlightStatus(DJI_FC_SUBSCRIPTION_FLIGHT_STATUS_IN_AIR);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Wait end flight status error.");
|
||||
goto out;
|
||||
}
|
||||
|
||||
USER_LOG_INFO("The aircraft is on the ground now.");
|
||||
USER_LOG_INFO("The aircraft is on the ground and motors are rotating...");
|
||||
returnCode = DjiTest_WaypointV3WaitEndFlightStatus(DJI_FC_SUBSCRIPTION_FLIGHT_STATUS_ON_GROUND);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Wait end flight status error.");
|
||||
goto out;
|
||||
}
|
||||
|
||||
returnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_STATUS_FLIGHT,
|
||||
(uint8_t *) &flightStatus,
|
||||
sizeof(T_DjiFcSubscriptionFlightStatus),
|
||||
&flightStatusTimestamp);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Get value of topic flight status failed, error code:0x%08llX", returnCode);
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (flightStatus != DJI_FC_SUBSCRIPTION_FLIGHT_STATUS_STOPED) {
|
||||
USER_LOG_ERROR("Aircraft's flight status error, motors are not stoped.");
|
||||
goto out;
|
||||
}
|
||||
|
||||
USER_LOG_INFO("The aircraft is on the ground now, and motor are stoped.");
|
||||
|
||||
out:
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
@ -182,6 +213,7 @@ out:
|
||||
}
|
||||
|
||||
/* Private functions definition-----------------------------------------------*/
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
static T_DjiReturnCode DjiTest_WaypointV3MissionStateCallback(T_DjiWaypointV3MissionState missionState)
|
||||
{
|
||||
if (s_lastWaypointV3MissionState.state == missionState.state
|
||||
@ -208,5 +240,27 @@ static T_DjiReturnCode DjiTest_WaypointV3ActionStateCallback(T_DjiWaypointV3Acti
|
||||
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
#endif
|
||||
|
||||
static T_DjiReturnCode DjiTest_WaypointV3WaitEndFlightStatus(T_DjiFcSubscriptionFlightStatus status) {
|
||||
T_DjiReturnCode returnCode;
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
T_DjiDataTimestamp flightStatusTimestamp = {0};
|
||||
T_DjiFcSubscriptionFlightStatus flightStatus = 0;
|
||||
|
||||
do {
|
||||
osalHandler->TaskSleepMs(20);
|
||||
|
||||
returnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_STATUS_FLIGHT,
|
||||
(uint8_t *)&flightStatus,
|
||||
sizeof(T_DjiFcSubscriptionFlightStatus),
|
||||
&flightStatusTimestamp);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Get value of topic flight status failed, error code:0x%08llX", returnCode);
|
||||
}
|
||||
} while(flightStatus == status);
|
||||
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/
|
||||
|
||||
@ -171,6 +171,7 @@ T_DjiReturnCode DjiTest_WidgetStartService(void)
|
||||
}
|
||||
|
||||
//set default ui config path
|
||||
USER_LOG_INFO("widget file: %s", tempPath);
|
||||
djiStat = DjiWidget_RegDefaultUiConfigByDirPath(tempPath);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Add default widget ui config error, stat = 0x%08llX", djiStat);
|
||||
|
||||
@ -62,7 +62,7 @@
|
||||
#define WIDGET_SPEAKER_AUDIO_OPUS_DECODE_BITRATE_8KBPS (8000)
|
||||
|
||||
/* The speaker initialization parameters */
|
||||
#define WIDGET_SPEAKER_DEFAULT_VOLUME (30)
|
||||
#define WIDGET_SPEAKER_DEFAULT_VOLUME (60)
|
||||
#define EKHO_INSTALLED (1)
|
||||
|
||||
/* Private types -------------------------------------------------------------*/
|
||||
@ -71,10 +71,13 @@
|
||||
static T_DjiWidgetSpeakerHandler s_speakerHandler = {0};
|
||||
static T_DjiMutexHandle s_speakerMutex = {0};
|
||||
static T_DjiWidgetSpeakerState s_speakerState = {0};
|
||||
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
static T_DjiTaskHandle s_widgetSpeakerTestThread;
|
||||
static FILE *s_ttsFile = NULL;
|
||||
#endif
|
||||
|
||||
static FILE *s_audioFile = NULL;
|
||||
static FILE *s_ttsFile = NULL;
|
||||
static bool s_isDecodeFinished = true;
|
||||
static uint16_t s_decodeBitrate = 0;
|
||||
|
||||
@ -321,8 +324,16 @@ static T_DjiReturnCode DjiTest_PlayTtsData(void)
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SYSTEM_ERROR;
|
||||
}
|
||||
|
||||
if (aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M3E ||
|
||||
aircraftInfoBaseInfo.aircraftType == DJI_AIRCRAFT_TYPE_M3T) {
|
||||
if (DJI_AIRCRAFT_TYPE_M3E == aircraftInfoBaseInfo.aircraftType || DJI_AIRCRAFT_TYPE_M3T == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M3D == aircraftInfoBaseInfo.aircraftType || DJI_AIRCRAFT_TYPE_M3TD == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M3TA == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M4T == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M4TD == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M4D == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M4E == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M4TD == aircraftInfoBaseInfo.aircraftType
|
||||
|| DJI_AIRCRAFT_TYPE_M4D == aircraftInfoBaseInfo.aircraftType
|
||||
) {
|
||||
return DjiTest_PlayAudioData();
|
||||
} else {
|
||||
txtFile = fopen(WIDGET_SPEAKER_TTS_FILE_NAME, "r");
|
||||
@ -384,6 +395,7 @@ static T_DjiReturnCode DjiTest_CheckFileMd5Sum(const char *path, uint8_t *buf, u
|
||||
file = fopen(path, "rb");
|
||||
if (file == NULL) {
|
||||
USER_LOG_ERROR("Open tts file error.");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SYSTEM_ERROR;
|
||||
}
|
||||
|
||||
while (1) {
|
||||
@ -509,7 +521,9 @@ static T_DjiReturnCode SetPlayMode(E_DjiWidgetSpeakerPlayMode playMode)
|
||||
|
||||
static T_DjiReturnCode StartPlay(void)
|
||||
{
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
uint32_t pid;
|
||||
#endif
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
@ -530,7 +544,9 @@ static T_DjiReturnCode StopPlay(void)
|
||||
{
|
||||
T_DjiReturnCode returnCode;
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
uint32_t pid;
|
||||
#endif
|
||||
|
||||
returnCode = osalHandler->MutexLock(s_speakerMutex);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
@ -561,9 +577,11 @@ static T_DjiReturnCode SetVolume(uint8_t volume)
|
||||
{
|
||||
T_DjiReturnCode returnCode;
|
||||
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
|
||||
char cmdStr[128];
|
||||
int32_t ret = 0;
|
||||
#ifdef PLATFORM_ARCH_x86_64
|
||||
float realVolume;
|
||||
int32_t ret = 0;
|
||||
char cmdStr[128];
|
||||
#endif
|
||||
|
||||
returnCode = osalHandler->MutexLock(s_speakerMutex);
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
@ -571,7 +589,6 @@ static T_DjiReturnCode SetVolume(uint8_t volume)
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
realVolume = 1.5f * (float) volume;
|
||||
s_speakerState.volume = volume;
|
||||
|
||||
USER_LOG_INFO("Set widget speaker volume: %d", volume);
|
||||
@ -581,11 +598,12 @@ static T_DjiReturnCode SetVolume(uint8_t volume)
|
||||
ret = system(cmdStr);
|
||||
if (ret == DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
memset(cmdStr, 0, sizeof(cmdStr));
|
||||
realVolume = 1.5f * (float) volume;
|
||||
snprintf(cmdStr, sizeof(cmdStr), "pactl set-sink-volume %s %d%%", WIDGET_SPEAKER_USB_AUDIO_DEVICE_NAME,
|
||||
(int32_t) realVolume);
|
||||
|
||||
returnCode = DjiUserUtil_RunSystemCmd(cmdStr);
|
||||
if (returnCode < 0) {
|
||||
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Set widget speaker volume error: %d", ret);
|
||||
}
|
||||
} else {
|
||||
@ -607,8 +625,10 @@ static T_DjiReturnCode SetVolume(uint8_t volume)
|
||||
static T_DjiReturnCode ReceiveTtsData(E_DjiWidgetTransmitDataEvent event,
|
||||
uint32_t offset, uint8_t *buf, uint16_t size)
|
||||
{
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
uint16_t writeLen;
|
||||
T_DjiReturnCode returnCode;
|
||||
#endif
|
||||
|
||||
if (event == DJI_WIDGET_TRANSMIT_DATA_EVENT_START) {
|
||||
USER_LOG_INFO("Create tts file.");
|
||||
@ -659,8 +679,11 @@ static T_DjiReturnCode ReceiveTtsData(E_DjiWidgetTransmitDataEvent event,
|
||||
static T_DjiReturnCode ReceiveAudioData(E_DjiWidgetTransmitDataEvent event,
|
||||
uint32_t offset, uint8_t *buf, uint16_t size)
|
||||
{
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
uint16_t writeLen;
|
||||
T_DjiReturnCode returnCode;
|
||||
#endif
|
||||
|
||||
T_DjiWidgetTransDataContent transDataContent = {0};
|
||||
|
||||
if (event == DJI_WIDGET_TRANSMIT_DATA_EVENT_START) {
|
||||
|
||||
@ -3,6 +3,470 @@
|
||||
"major": 1,
|
||||
"minor": 0
|
||||
},
|
||||
"ar_config": {
|
||||
"circleStyleList": [
|
||||
{
|
||||
"face": {
|
||||
"bottom": {
|
||||
"backColor": -256,
|
||||
"faceColor": -65536
|
||||
},
|
||||
"distAlpha": {
|
||||
"maxAlpha": 0.4,
|
||||
"maxDist": 100,
|
||||
"minAlpha": 0,
|
||||
"minDist": 50
|
||||
},
|
||||
"side": {
|
||||
"backColor": -256,
|
||||
"faceColor": -65536
|
||||
},
|
||||
"top": {
|
||||
"backColor": -256,
|
||||
"faceColor": -65536
|
||||
}
|
||||
},
|
||||
"stroke": {
|
||||
"bottom": {
|
||||
"distAlpha": {
|
||||
"maxAlpha": 0.6,
|
||||
"maxDist": 100,
|
||||
"minAlpha": 1,
|
||||
"minDist": 50
|
||||
},
|
||||
"strokeInfo": {
|
||||
"color": -65536,
|
||||
"dash": true,
|
||||
"width": {
|
||||
"maxStrokeWidth": 20,
|
||||
"maxWidthDist": 20,
|
||||
"minStrokeWidth": 2.5,
|
||||
"minWidthDist": 100
|
||||
}
|
||||
}
|
||||
},
|
||||
"side": {
|
||||
"distAlpha": {
|
||||
"maxAlpha": 0.6,
|
||||
"maxDist": 100,
|
||||
"minAlpha": 1,
|
||||
"minDist": 50
|
||||
},
|
||||
"strokeInfo": {
|
||||
"color": -65536,
|
||||
"dash": true,
|
||||
"width": {
|
||||
"maxStrokeWidth": 20,
|
||||
"maxWidthDist": 20,
|
||||
"minStrokeWidth": 2.5,
|
||||
"minWidthDist": 100
|
||||
}
|
||||
}
|
||||
},
|
||||
"top": {
|
||||
"distAlpha": {
|
||||
"maxAlpha": 0.6,
|
||||
"maxDist": 100,
|
||||
"minAlpha": 1,
|
||||
"minDist": 50
|
||||
},
|
||||
"strokeInfo": {
|
||||
"color": -65536,
|
||||
"dash": true,
|
||||
"width": {
|
||||
"maxStrokeWidth": 20,
|
||||
"maxWidthDist": 20,
|
||||
"minStrokeWidth": 2.5,
|
||||
"minWidthDist": 100
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
"styleId": 10
|
||||
},
|
||||
{
|
||||
"face": {
|
||||
"bottom": {
|
||||
"backColor": -256,
|
||||
"faceColor": -65536
|
||||
},
|
||||
"distAlpha": {
|
||||
"maxAlpha": 0.4,
|
||||
"maxDist": 100,
|
||||
"minAlpha": 0,
|
||||
"minDist": 50
|
||||
},
|
||||
"side": {
|
||||
"backColor": -256,
|
||||
"faceColor": -65536
|
||||
},
|
||||
"top": {
|
||||
"backColor": -256,
|
||||
"faceColor": -65536
|
||||
}
|
||||
},
|
||||
"stroke": {
|
||||
"bottom": {
|
||||
"distAlpha": {
|
||||
"maxAlpha": 0.6,
|
||||
"maxDist": 100,
|
||||
"minAlpha": 1,
|
||||
"minDist": 50
|
||||
},
|
||||
"strokeInfo": {
|
||||
"color": -65536,
|
||||
"dash": true,
|
||||
"width": {
|
||||
"maxStrokeWidth": 20,
|
||||
"maxWidthDist": 20,
|
||||
"minStrokeWidth": 2.5,
|
||||
"minWidthDist": 100
|
||||
}
|
||||
}
|
||||
},
|
||||
"side": {
|
||||
"distAlpha": {
|
||||
"maxAlpha": 0.6,
|
||||
"maxDist": 100,
|
||||
"minAlpha": 1,
|
||||
"minDist": 50
|
||||
},
|
||||
"strokeInfo": {
|
||||
"color": -65536,
|
||||
"dash": true,
|
||||
"width": {
|
||||
"maxStrokeWidth": 20,
|
||||
"maxWidthDist": 20,
|
||||
"minStrokeWidth": 2.5,
|
||||
"minWidthDist": 100
|
||||
}
|
||||
}
|
||||
},
|
||||
"top": {
|
||||
"distAlpha": {
|
||||
"maxAlpha": 0.6,
|
||||
"maxDist": 100,
|
||||
"minAlpha": 1,
|
||||
"minDist": 50
|
||||
},
|
||||
"strokeInfo": {
|
||||
"color": -65536,
|
||||
"dash": true,
|
||||
"width": {
|
||||
"maxStrokeWidth": 20,
|
||||
"maxWidthDist": 20,
|
||||
"minStrokeWidth": 2.5,
|
||||
"minWidthDist": 100
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
"styleId": 11
|
||||
}
|
||||
],
|
||||
"commonPointStyleList": [
|
||||
{
|
||||
"alwaysInEdge": false,
|
||||
"arTextAttribute": {
|
||||
"color": -65536
|
||||
},
|
||||
"isIgnoreBorder": true,
|
||||
"styleId": 10
|
||||
},
|
||||
{
|
||||
"alwaysInEdge": false,
|
||||
"arTextAttribute": {
|
||||
"color": -65536
|
||||
},
|
||||
"isIgnoreBorder": true,
|
||||
"styleId": 11
|
||||
}
|
||||
],
|
||||
"lineStyleList": [
|
||||
{
|
||||
"color": -256,
|
||||
"stokeInfo": {
|
||||
"color": -65536,
|
||||
"dash": false,
|
||||
"width": {
|
||||
"maxStrokeWidth": 2,
|
||||
"maxWidthDist": 2,
|
||||
"minStrokeWidth": 2,
|
||||
"minWidthDist": 2
|
||||
}
|
||||
},
|
||||
"styleId": 10,
|
||||
"useDepth": false
|
||||
},
|
||||
{
|
||||
"color": -256,
|
||||
"stokeInfo": {
|
||||
"color": -65536,
|
||||
"dash": false,
|
||||
"width": {
|
||||
"maxStrokeWidth": 2,
|
||||
"maxWidthDist": 2,
|
||||
"minStrokeWidth": 2,
|
||||
"minWidthDist": 2
|
||||
}
|
||||
},
|
||||
"styleId": 11,
|
||||
"useDepth": false
|
||||
}
|
||||
],
|
||||
"pointStyleList": [
|
||||
{
|
||||
"keyName": 0,
|
||||
"pointConfig": {
|
||||
"arrowConfig": {
|
||||
"customIcon": false,
|
||||
"defaultDeg": 0,
|
||||
"img": "",
|
||||
"imgHeight": 0,
|
||||
"imgWidth": 0,
|
||||
"showMode": 0,
|
||||
"translate": 0,
|
||||
"type": 0
|
||||
},
|
||||
"needFont": true,
|
||||
"needOrthographic": true,
|
||||
"needPerspective": false,
|
||||
"oConfig": {
|
||||
"displayHeight": 20.0,
|
||||
"displayWidth": 20.0,
|
||||
"img": "<svg xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\" fill=\"none\"\n version=\"1.1\" width=\"8\" height=\"8\" viewBox=\"0 0 8 8\">\n <g>\n <ellipse id=\"point_normal_bg\"\n cx=\"4\" cy=\"4\" rx=\"2\" ry=\"2\"\n fill=\"#FFFFFF\" fill-opacity=\"1\" />\n </g>\n\n</svg>",
|
||||
"imgHeight": 20.0,
|
||||
"imgWidth": 20.0
|
||||
},
|
||||
"pConfig": {
|
||||
"maxSVG": "",
|
||||
"maxSize": 0,
|
||||
"minSVG": "",
|
||||
"minSize": 0,
|
||||
"realSize": 0,
|
||||
"strokeWidthRadio": 0
|
||||
},
|
||||
"textConfig": {
|
||||
"backgroundColor": 1427379220,
|
||||
"cornerRadius": 5,
|
||||
"paddingB": 5,
|
||||
"paddingL": 10,
|
||||
"paddingR": 10,
|
||||
"paddingT": 5,
|
||||
"shadow": false,
|
||||
"textGravity": 1,
|
||||
"textSize": 20
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"keyName": 1,
|
||||
"pointConfig": {
|
||||
"arrowConfig": {
|
||||
"customIcon": false,
|
||||
"defaultDeg": 0,
|
||||
"img": "",
|
||||
"imgHeight": 0,
|
||||
"imgWidth": 0,
|
||||
"showMode": 0,
|
||||
"translate": 0,
|
||||
"type": 0
|
||||
},
|
||||
"needFont": true,
|
||||
"needOrthographic": true,
|
||||
"needPerspective": false,
|
||||
"oConfig": {
|
||||
"displayHeight": 20.0,
|
||||
"displayWidth": 20.0,
|
||||
"img": "<svg xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\" fill=\"none\"\n version=\"1.1\" width=\"8\" height=\"8\" viewBox=\"0 0 8 8\">\n <g>\n <ellipse id=\"point_normal_bg\"\n cx=\"4\" cy=\"4\" rx=\"2\" ry=\"2\"\n fill=\"#FFFFFF\" fill-opacity=\"1\" />\n </g>\n\n</svg>",
|
||||
"imgHeight": 20.0,
|
||||
"imgWidth": 20.0
|
||||
},
|
||||
"pConfig": {
|
||||
"maxSVG": "",
|
||||
"maxSize": 0,
|
||||
"minSVG": "",
|
||||
"minSize": 0,
|
||||
"realSize": 0,
|
||||
"strokeWidthRadio": 0
|
||||
},
|
||||
"textConfig": {
|
||||
"backgroundColor": 1427379220,
|
||||
"cornerRadius": 5,
|
||||
"paddingB": 5,
|
||||
"paddingL": 10,
|
||||
"paddingR": 10,
|
||||
"paddingT": 5,
|
||||
"shadow": false,
|
||||
"textGravity": 1,
|
||||
"textSize": 20
|
||||
}
|
||||
}
|
||||
}
|
||||
],
|
||||
"polygonStyleList": [
|
||||
{
|
||||
"face": {
|
||||
"bottom": {
|
||||
"backColor": -256,
|
||||
"faceColor": -65536
|
||||
},
|
||||
"distAlpha": {
|
||||
"maxAlpha": 0.4,
|
||||
"maxDist": 100,
|
||||
"minAlpha": 0,
|
||||
"minDist": 50
|
||||
},
|
||||
"side": {
|
||||
"backColor": -256,
|
||||
"faceColor": -65536
|
||||
},
|
||||
"top": {
|
||||
"backColor": -256,
|
||||
"faceColor": -65536
|
||||
}
|
||||
},
|
||||
"is3DMesh": false,
|
||||
"stroke": {
|
||||
"bottom": {
|
||||
"distAlpha": {
|
||||
"maxAlpha": 0.6,
|
||||
"maxDist": 100,
|
||||
"minAlpha": 1,
|
||||
"minDist": 50
|
||||
},
|
||||
"strokeInfo": {
|
||||
"color": -65536,
|
||||
"dash": true,
|
||||
"width": {
|
||||
"maxStrokeWidth": 20,
|
||||
"maxWidthDist": 20,
|
||||
"minStrokeWidth": 2.5,
|
||||
"minWidthDist": 100
|
||||
}
|
||||
}
|
||||
},
|
||||
"side": {
|
||||
"distAlpha": {
|
||||
"maxAlpha": 0.6,
|
||||
"maxDist": 100,
|
||||
"minAlpha": 1,
|
||||
"minDist": 50
|
||||
},
|
||||
"strokeInfo": {
|
||||
"color": -65536,
|
||||
"dash": true,
|
||||
"width": {
|
||||
"maxStrokeWidth": 20,
|
||||
"maxWidthDist": 20,
|
||||
"minStrokeWidth": 2.5,
|
||||
"minWidthDist": 100
|
||||
}
|
||||
}
|
||||
},
|
||||
"top": {
|
||||
"distAlpha": {
|
||||
"maxAlpha": 0.6,
|
||||
"maxDist": 100,
|
||||
"minAlpha": 1,
|
||||
"minDist": 50
|
||||
},
|
||||
"strokeInfo": {
|
||||
"color": -65536,
|
||||
"dash": true,
|
||||
"width": {
|
||||
"maxStrokeWidth": 20,
|
||||
"maxWidthDist": 20,
|
||||
"minStrokeWidth": 2.5,
|
||||
"minWidthDist": 100
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
"styleId": 10
|
||||
},
|
||||
{
|
||||
"face": {
|
||||
"bottom": {
|
||||
"backColor": -256,
|
||||
"faceColor": -65536
|
||||
},
|
||||
"distAlpha": {
|
||||
"maxAlpha": 0.4,
|
||||
"maxDist": 100,
|
||||
"minAlpha": 0,
|
||||
"minDist": 50
|
||||
},
|
||||
"side": {
|
||||
"backColor": -256,
|
||||
"faceColor": -65536
|
||||
},
|
||||
"top": {
|
||||
"backColor": -256,
|
||||
"faceColor": -65536
|
||||
}
|
||||
},
|
||||
"is3DMesh": false,
|
||||
"stroke": {
|
||||
"bottom": {
|
||||
"distAlpha": {
|
||||
"maxAlpha": 0.6,
|
||||
"maxDist": 100,
|
||||
"minAlpha": 1,
|
||||
"minDist": 50
|
||||
},
|
||||
"strokeInfo": {
|
||||
"color": -65536,
|
||||
"dash": true,
|
||||
"width": {
|
||||
"maxStrokeWidth": 20,
|
||||
"maxWidthDist": 20,
|
||||
"minStrokeWidth": 2.5,
|
||||
"minWidthDist": 100
|
||||
}
|
||||
}
|
||||
},
|
||||
"side": {
|
||||
"distAlpha": {
|
||||
"maxAlpha": 0.6,
|
||||
"maxDist": 100,
|
||||
"minAlpha": 1,
|
||||
"minDist": 50
|
||||
},
|
||||
"strokeInfo": {
|
||||
"color": -65536,
|
||||
"dash": true,
|
||||
"width": {
|
||||
"maxStrokeWidth": 20,
|
||||
"maxWidthDist": 20,
|
||||
"minStrokeWidth": 2.5,
|
||||
"minWidthDist": 100
|
||||
}
|
||||
}
|
||||
},
|
||||
"top": {
|
||||
"distAlpha": {
|
||||
"maxAlpha": 0.6,
|
||||
"maxDist": 100,
|
||||
"minAlpha": 1,
|
||||
"minDist": 50
|
||||
},
|
||||
"strokeInfo": {
|
||||
"color": -65536,
|
||||
"dash": true,
|
||||
"width": {
|
||||
"maxStrokeWidth": 20,
|
||||
"maxWidthDist": 20,
|
||||
"minStrokeWidth": 2.5,
|
||||
"minWidthDist": 100
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
"styleId": 11
|
||||
}
|
||||
]
|
||||
},
|
||||
"main_interface": {
|
||||
"floating_window": {
|
||||
"is_enable": true
|
||||
|
||||
@ -73,7 +73,7 @@ typedef enum {
|
||||
E_DJI_SAMPLE_INDEX_FLIGHT_CONTROL_TAKE_OFF_VELOCITY_CTRL_LANDING = 6,
|
||||
E_DJI_SAMPLE_INDEX_FLIGHT_CONTROL_ARREST_FLYING = 7,
|
||||
E_DJI_SAMPLE_INDEX_FLIGHT_CONTROL_SET_GET_PARAM = 8,
|
||||
E_DJI_SAMPLE_INDEX_HMS = 9,
|
||||
E_DJI_SAMPLE_INDEX_HMS_MANAGER = 9,
|
||||
E_DJI_SAMPLE_INDEX_GIMBAL_MANAGER_FREE_MODE = 10,
|
||||
E_DJI_SAMPLE_INDEX_GIMBAL_MANAGER_YAW_FOLLOW_MODE = 11,
|
||||
E_DJI_SAMPLE_INDEX_LIVEVIEW = 12,
|
||||
@ -87,12 +87,11 @@ typedef enum {
|
||||
E_DJI_SAMPLE_INDEX_CAMMGR_TAP_ZOOM = 20,
|
||||
E_DJI_SAMPLE_INDEX_CAMMGR_OPTICAL_ZOOM = 21,
|
||||
E_DJI_SAMPLE_INDEX_CAMMGR_SINGLE_PHOTO = 22,
|
||||
E_DJI_SAMPLE_INDEX_CAMMGR_AEB_PHOTO = 23,
|
||||
E_DJI_SAMPLE_INDEX_CAMMGR_BURST_PHOTO = 24,
|
||||
E_DJI_SAMPLE_INDEX_CAMMGR_INTERVAL_PHOTO = 25,
|
||||
E_DJI_SAMPLE_INDEX_CAMMGR_RECORDER_VIDEO = 26,
|
||||
E_DJI_SAMPLE_INDEX_CAMMGR_MEDIA_DOWNLOAD = 27,
|
||||
E_DJI_SAMPLE_INDEX_CAMMGR_THERMOMETRY = 28,
|
||||
E_DJI_SAMPLE_INDEX_CAMMGR_BURST_PHOTO = 23,
|
||||
E_DJI_SAMPLE_INDEX_CAMMGR_INTERVAL_PHOTO = 24,
|
||||
E_DJI_SAMPLE_INDEX_CAMMGR_RECORDER_VIDEO = 25,
|
||||
E_DJI_SAMPLE_INDEX_CAMMGR_MEDIA_DOWNLOAD = 26,
|
||||
E_DJI_SAMPLE_INDEX_CAMMGR_THERMOMETRY = 27,
|
||||
E_DJI_SAMPLE_INDEX_UNKNOWN = 0xFF,
|
||||
} E_DjiExtensionPortSampleIndex;
|
||||
|
||||
@ -102,7 +101,7 @@ typedef enum {
|
||||
E_DJI_HMS_ERROR_CODE_INDEX3,
|
||||
E_DJI_HMS_ERROR_CODE_INDEX4,
|
||||
E_DJI_HMS_ERROR_CODE_INDEX5,
|
||||
}E_DjiExtensionPortHmsErrorCodeIndex;
|
||||
} E_DjiExtensionPortHmsErrorCodeIndex;
|
||||
|
||||
typedef enum {
|
||||
E_DJI_HMS_ERROR_LEVEL_INDEX1 = 0,
|
||||
@ -110,7 +109,7 @@ typedef enum {
|
||||
E_DJI_HMS_ERROR_LEVEL_INDEX3,
|
||||
E_DJI_HMS_ERROR_LEVEL_INDEX4,
|
||||
E_DJI_HMS_ERROR_LEVEL_INDEX5,
|
||||
}E_DjiExtensionPortHmsErrorLevelIndex;
|
||||
} E_DjiExtensionPortHmsErrorLevelIndex;
|
||||
|
||||
typedef struct {
|
||||
bool valid;
|
||||
@ -212,10 +211,11 @@ T_DjiReturnCode DjiTest_WidgetInteractionStartService(void)
|
||||
return djiStat;
|
||||
}
|
||||
|
||||
#ifdef SYSTEM_ARCH_LINUX_DISABLEED
|
||||
#ifdef SYSTEM_ARCH_LINUX_DISABLE
|
||||
//Step 2 : Set UI Config (Linux environment)
|
||||
char curFileDirPath[WIDGET_DIR_PATH_LEN_MAX];
|
||||
char tempPath[WIDGET_DIR_PATH_LEN_MAX];
|
||||
|
||||
djiStat = DjiUserUtil_GetCurrentFileDirPath(__FILE__, WIDGET_DIR_PATH_LEN_MAX, curFileDirPath);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Get file current path error, stat = 0x%08llX", djiStat);
|
||||
@ -228,7 +228,9 @@ T_DjiReturnCode DjiTest_WidgetInteractionStartService(void)
|
||||
snprintf(tempPath, WIDGET_DIR_PATH_LEN_MAX, "%swidget_file/en_big_screen", curFileDirPath);
|
||||
}
|
||||
|
||||
USER_LOG_ERROR("Dji test widget set path");
|
||||
//set default ui config path
|
||||
USER_LOG_INFO("widget file: %s", tempPath);
|
||||
djiStat = DjiWidget_RegDefaultUiConfigByDirPath(tempPath);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("Add default widget ui config error, stat = 0x%08llX", djiStat);
|
||||
@ -418,7 +420,7 @@ static void *DjiTest_WidgetInteractionTask(void *arg)
|
||||
default:
|
||||
break;
|
||||
}
|
||||
DjiHms_InjectHmsErrorCode(errorCode, errorLevel);
|
||||
DjiHmsCustomization_InjectHmsErrorCode(errorCode, errorLevel);
|
||||
osalHandler->TaskSleepMs(500);
|
||||
s_isInjectErrcode = false;
|
||||
s_isEliminateErrcode = false;
|
||||
@ -444,7 +446,7 @@ static void *DjiTest_WidgetInteractionTask(void *arg)
|
||||
default:
|
||||
break;
|
||||
}
|
||||
DjiHms_EliminateHmsErrorCode(errorCode);
|
||||
DjiHmsCustomization_EliminateHmsErrorCode(errorCode);
|
||||
osalHandler->TaskSleepMs(500);
|
||||
s_isInjectErrcode = false;
|
||||
s_isEliminateErrcode = false;
|
||||
@ -462,7 +464,10 @@ static void *DjiTest_WidgetInteractionTask(void *arg)
|
||||
USER_LOG_INFO("--------------------------------------------------------------------------------------------->");
|
||||
DjiTest_WidgetLogAppend("-> Sample Start");
|
||||
|
||||
if (s_aircraftInfoBaseInfo.mountPosition == DJI_MOUNT_POSITION_EXTENSION_PORT) {
|
||||
if (s_aircraftInfoBaseInfo.mountPositionType == DJI_MOUNT_POSITION_TYPE_EXTENSION_PORT ||
|
||||
s_aircraftInfoBaseInfo.mountPositionType == DJI_MOUNT_POSITION_TYPE_EXTENSION_LITE_PORT ||
|
||||
s_aircraftInfoBaseInfo.mountPositionType == DJI_MOUNT_POSITION_TYPE_MANIFOLD3_ONBOARD ||
|
||||
s_aircraftInfoBaseInfo.mountPositionType == DJI_MOUNT_POSITION_TYPE_EXTENSION_PORT_V2) {
|
||||
switch (s_extensionPortSampleIndex) {
|
||||
case E_DJI_SAMPLE_INDEX_WAYPOINT_V2:
|
||||
if (s_isallowRunFlightControlSample == true) {
|
||||
@ -530,8 +535,8 @@ static void *DjiTest_WidgetInteractionTask(void *arg)
|
||||
case E_DJI_SAMPLE_INDEX_GIMBAL_MANAGER_YAW_FOLLOW_MODE:
|
||||
DjiTest_GimbalManagerRunSample(s_mountPosition, DJI_GIMBAL_MODE_YAW_FOLLOW);
|
||||
break;
|
||||
case E_DJI_SAMPLE_INDEX_HMS:
|
||||
DjiTest_HmsRunSample();
|
||||
case E_DJI_SAMPLE_INDEX_HMS_MANAGER:
|
||||
DjiTest_HmsManagerRunSample(DJI_MOBILE_APP_LANGUAGE_ENGLISH);
|
||||
break;
|
||||
case E_DJI_SAMPLE_INDEX_LIVEVIEW:
|
||||
#ifdef SYSTEM_ARCH_LINUX
|
||||
@ -582,10 +587,6 @@ static void *DjiTest_WidgetInteractionTask(void *arg)
|
||||
DjiTest_CameraManagerRunSample(s_mountPosition,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SHOOT_SINGLE_PHOTO);
|
||||
break;
|
||||
case E_DJI_SAMPLE_INDEX_CAMMGR_AEB_PHOTO:
|
||||
DjiTest_CameraManagerRunSample(s_mountPosition,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SHOOT_AEB_PHOTO);
|
||||
break;
|
||||
case E_DJI_SAMPLE_INDEX_CAMMGR_BURST_PHOTO:
|
||||
DjiTest_CameraManagerRunSample(s_mountPosition,
|
||||
E_DJI_TEST_CAMERA_MANAGER_SAMPLE_SELECT_SHOOT_BURST_PHOTO);
|
||||
|
||||
@ -205,22 +205,19 @@
|
||||
"item_name": "23_test_cammgr-single_photo"
|
||||
},
|
||||
{
|
||||
"item_name": "24_test_cammgr-AEB_photo"
|
||||
"item_name": "24_test_cammgr-burst_photo"
|
||||
},
|
||||
{
|
||||
"item_name": "25_test_cammgr-burst_photo"
|
||||
"item_name": "25_test_cammgr-interval_photo"
|
||||
},
|
||||
{
|
||||
"item_name": "26_test_cammgr-interval_photo"
|
||||
"item_name": "26_test_cammgr-recorder_video"
|
||||
},
|
||||
{
|
||||
"item_name": "27_test_cammgr-recorder_video"
|
||||
"item_name": "27_test_cammgr-media_download"
|
||||
},
|
||||
{
|
||||
"item_name": "28_test_cammgr-media_download"
|
||||
},
|
||||
{
|
||||
"item_name": "29_test_cammgr-thermometry"
|
||||
"item_name": "28_test_cammgr-thermometry"
|
||||
}
|
||||
]
|
||||
},
|
||||
|
||||
@ -205,22 +205,19 @@
|
||||
"item_name": "23_test_cammgr-single_photo"
|
||||
},
|
||||
{
|
||||
"item_name": "24_test_cammgr-AEB_photo"
|
||||
"item_name": "24_test_cammgr-burst_photo"
|
||||
},
|
||||
{
|
||||
"item_name": "25_test_cammgr-burst_photo"
|
||||
"item_name": "25_test_cammgr-interval_photo"
|
||||
},
|
||||
{
|
||||
"item_name": "26_test_cammgr-interval_photo"
|
||||
"item_name": "26_test_cammgr-recorder_video"
|
||||
},
|
||||
{
|
||||
"item_name": "27_test_cammgr-recorder_video"
|
||||
"item_name": "27_test_cammgr-media_download"
|
||||
},
|
||||
{
|
||||
"item_name": "28_test_cammgr-media_download"
|
||||
},
|
||||
{
|
||||
"item_name": "29_test_cammgr-thermometry"
|
||||
"item_name": "28_test_cammgr-thermometry"
|
||||
}
|
||||
]
|
||||
},
|
||||
|
||||
@ -6,9 +6,9 @@
|
||||
|
||||
/* Contents of file widget_config.json */
|
||||
#define widget_config_json_fileName "widget_config.json"
|
||||
#define widget_config_json_fileSize 7255
|
||||
#define widget_config_json_fileSize sizeof(widget_config_json_fileBinaryArray)
|
||||
|
||||
static const uint8_t widget_config_json_fileBinaryArray[7255] = {
|
||||
static const uint8_t widget_config_json_fileBinaryArray[] = {
|
||||
0x7B, 0x0A, 0x20, 0x20, 0x22, 0x76, 0x65, 0x72, 0x73, 0x69, 0x6F, 0x6E, 0x22, 0x3A, 0x20, 0x7B,
|
||||
0x0A, 0x20, 0x20, 0x20, 0x20, 0x22, 0x6D, 0x61, 0x6A, 0x6F, 0x72, 0x22, 0x3A, 0x20, 0x31, 0x2C,
|
||||
0x0A, 0x20, 0x20, 0x20, 0x20, 0x22, 0x6D, 0x69, 0x6E, 0x6F, 0x72, 0x22, 0x3A, 0x20, 0x30, 0x0A,
|
||||
@ -353,116 +353,111 @@ static const uint8_t widget_config_json_fileBinaryArray[7255] = {
|
||||
0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x69,
|
||||
0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x32, 0x34, 0x5F, 0x74,
|
||||
0x65, 0x73, 0x74, 0x5F, 0x63, 0x61, 0x6D, 0x6D, 0x67, 0x72, 0x2D, 0x41, 0x45, 0x42, 0x5F, 0x70,
|
||||
0x68, 0x6F, 0x74, 0x6F, 0x22, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x69, 0x74, 0x65, 0x6D,
|
||||
0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x32, 0x35, 0x5F, 0x74, 0x65, 0x73, 0x74,
|
||||
0x5F, 0x63, 0x61, 0x6D, 0x6D, 0x67, 0x72, 0x2D, 0x62, 0x75, 0x72, 0x73, 0x74, 0x5F, 0x70, 0x68,
|
||||
0x6F, 0x74, 0x6F, 0x22, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D,
|
||||
0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x69, 0x74, 0x65, 0x6D, 0x5F,
|
||||
0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x32, 0x36, 0x5F, 0x74, 0x65, 0x73, 0x74, 0x5F,
|
||||
0x63, 0x61, 0x6D, 0x6D, 0x67, 0x72, 0x2D, 0x69, 0x6E, 0x74, 0x65, 0x72, 0x76, 0x61, 0x6C, 0x5F,
|
||||
0x70, 0x68, 0x6F, 0x74, 0x6F, 0x22, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x69, 0x74, 0x65,
|
||||
0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x32, 0x37, 0x5F, 0x74, 0x65, 0x73,
|
||||
0x74, 0x5F, 0x63, 0x61, 0x6D, 0x6D, 0x67, 0x72, 0x2D, 0x72, 0x65, 0x63, 0x6F, 0x72, 0x64, 0x65,
|
||||
0x72, 0x5F, 0x76, 0x69, 0x64, 0x65, 0x6F, 0x22, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x69,
|
||||
0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x32, 0x38, 0x5F, 0x74,
|
||||
0x65, 0x73, 0x74, 0x5F, 0x63, 0x61, 0x6D, 0x6D, 0x67, 0x72, 0x2D, 0x6D, 0x65, 0x64, 0x69, 0x61,
|
||||
0x5F, 0x64, 0x6F, 0x77, 0x6E, 0x6C, 0x6F, 0x61, 0x64, 0x22, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x22, 0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x32, 0x39,
|
||||
0x5F, 0x74, 0x65, 0x73, 0x74, 0x5F, 0x63, 0x61, 0x6D, 0x6D, 0x67, 0x72, 0x2D, 0x74, 0x68, 0x65,
|
||||
0x72, 0x6D, 0x6F, 0x6D, 0x65, 0x74, 0x72, 0x79, 0x22, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x7D, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x5D, 0x0A,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7B,
|
||||
0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67, 0x65, 0x74,
|
||||
0x5F, 0x69, 0x6E, 0x64, 0x65, 0x78, 0x22, 0x3A, 0x20, 0x31, 0x30, 0x2C, 0x0A, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67, 0x65, 0x74, 0x5F, 0x74, 0x79, 0x70,
|
||||
0x65, 0x22, 0x3A, 0x20, 0x22, 0x62, 0x75, 0x74, 0x74, 0x6F, 0x6E, 0x22, 0x2C, 0x0A, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67, 0x65, 0x74, 0x5F, 0x6E, 0x61,
|
||||
0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x52, 0x75, 0x6E, 0x22, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67, 0x65, 0x74, 0x5F, 0x69, 0x6E, 0x64, 0x65,
|
||||
0x78, 0x22, 0x3A, 0x20, 0x31, 0x31, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x22, 0x77, 0x69, 0x64, 0x67, 0x65, 0x74, 0x5F, 0x74, 0x79, 0x70, 0x65, 0x22, 0x3A, 0x20, 0x22,
|
||||
0x6C, 0x69, 0x73, 0x74, 0x22, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22,
|
||||
0x77, 0x69, 0x64, 0x67, 0x65, 0x74, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x50,
|
||||
0x6C, 0x65, 0x61, 0x73, 0x65, 0x20, 0x53, 0x65, 0x6C, 0x65, 0x63, 0x74, 0x20, 0x45, 0x72, 0x72,
|
||||
0x63, 0x6F, 0x64, 0x65, 0x22, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22,
|
||||
0x6C, 0x69, 0x73, 0x74, 0x5F, 0x69, 0x74, 0x65, 0x6D, 0x22, 0x3A, 0x20, 0x5B, 0x0A, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65,
|
||||
0x22, 0x3A, 0x20, 0x22, 0x30, 0x78, 0x31, 0x45, 0x30, 0x32, 0x30, 0x30, 0x30, 0x30, 0x22, 0x0A,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22,
|
||||
0x3A, 0x20, 0x22, 0x30, 0x78, 0x31, 0x45, 0x30, 0x32, 0x30, 0x30, 0x30, 0x31, 0x22, 0x0A, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x22, 0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A,
|
||||
0x20, 0x22, 0x30, 0x78, 0x31, 0x45, 0x30, 0x32, 0x30, 0x30, 0x30, 0x32, 0x22, 0x0A, 0x20, 0x20,
|
||||
0x65, 0x73, 0x74, 0x5F, 0x63, 0x61, 0x6D, 0x6D, 0x67, 0x72, 0x2D, 0x62, 0x75, 0x72, 0x73, 0x74,
|
||||
0x5F, 0x70, 0x68, 0x6F, 0x74, 0x6F, 0x22, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7B,
|
||||
0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x69, 0x74,
|
||||
0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x32, 0x35, 0x5F, 0x74, 0x65,
|
||||
0x73, 0x74, 0x5F, 0x63, 0x61, 0x6D, 0x6D, 0x67, 0x72, 0x2D, 0x69, 0x6E, 0x74, 0x65, 0x72, 0x76,
|
||||
0x61, 0x6C, 0x5F, 0x70, 0x68, 0x6F, 0x74, 0x6F, 0x22, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22,
|
||||
0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x32, 0x36, 0x5F,
|
||||
0x74, 0x65, 0x73, 0x74, 0x5F, 0x63, 0x61, 0x6D, 0x6D, 0x67, 0x72, 0x2D, 0x72, 0x65, 0x63, 0x6F,
|
||||
0x72, 0x64, 0x65, 0x72, 0x5F, 0x76, 0x69, 0x64, 0x65, 0x6F, 0x22, 0x0A, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x22, 0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x32,
|
||||
0x37, 0x5F, 0x74, 0x65, 0x73, 0x74, 0x5F, 0x63, 0x61, 0x6D, 0x6D, 0x67, 0x72, 0x2D, 0x6D, 0x65,
|
||||
0x64, 0x69, 0x61, 0x5F, 0x64, 0x6F, 0x77, 0x6E, 0x6C, 0x6F, 0x61, 0x64, 0x22, 0x0A, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x22, 0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20,
|
||||
0x22, 0x30, 0x78, 0x31, 0x45, 0x30, 0x32, 0x30, 0x30, 0x30, 0x33, 0x22, 0x0A, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x22, 0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22,
|
||||
0x30, 0x78, 0x31, 0x45, 0x30, 0x32, 0x30, 0x30, 0x30, 0x34, 0x22, 0x0A, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x5D, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67,
|
||||
0x65, 0x74, 0x5F, 0x69, 0x6E, 0x64, 0x65, 0x78, 0x22, 0x3A, 0x20, 0x31, 0x32, 0x2C, 0x0A, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67, 0x65, 0x74, 0x5F, 0x74,
|
||||
0x79, 0x70, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x6C, 0x69, 0x73, 0x74, 0x22, 0x2C, 0x0A, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67, 0x65, 0x74, 0x5F, 0x6E, 0x61,
|
||||
0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x50, 0x6C, 0x65, 0x61, 0x73, 0x65, 0x20, 0x53, 0x65, 0x6C,
|
||||
0x65, 0x63, 0x74, 0x20, 0x45, 0x72, 0x72, 0x4C, 0x65, 0x76, 0x65, 0x6C, 0x22, 0x2C, 0x0A, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x6C, 0x69, 0x73, 0x74, 0x5F, 0x69, 0x74, 0x65,
|
||||
0x6D, 0x22, 0x3A, 0x20, 0x5B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x69,
|
||||
0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x45, 0x72, 0x72, 0x6F,
|
||||
0x72, 0x20, 0x6C, 0x65, 0x76, 0x65, 0x6C, 0x30, 0x22, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22,
|
||||
0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x45, 0x72, 0x72,
|
||||
0x6F, 0x72, 0x20, 0x6C, 0x65, 0x76, 0x65, 0x6C, 0x31, 0x22, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x22, 0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x45, 0x72,
|
||||
0x72, 0x6F, 0x72, 0x20, 0x6C, 0x65, 0x76, 0x65, 0x6C, 0x32, 0x22, 0x0A, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x22, 0x32, 0x38, 0x5F, 0x74, 0x65, 0x73, 0x74, 0x5F, 0x63, 0x61, 0x6D, 0x6D, 0x67, 0x72, 0x2D,
|
||||
0x74, 0x68, 0x65, 0x72, 0x6D, 0x6F, 0x6D, 0x65, 0x74, 0x72, 0x79, 0x22, 0x0A, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x5D, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64,
|
||||
0x67, 0x65, 0x74, 0x5F, 0x69, 0x6E, 0x64, 0x65, 0x78, 0x22, 0x3A, 0x20, 0x31, 0x30, 0x2C, 0x0A,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67, 0x65, 0x74, 0x5F,
|
||||
0x74, 0x79, 0x70, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x62, 0x75, 0x74, 0x74, 0x6F, 0x6E, 0x22, 0x2C,
|
||||
0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67, 0x65, 0x74,
|
||||
0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x52, 0x75, 0x6E, 0x22, 0x0A, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67, 0x65, 0x74, 0x5F, 0x69,
|
||||
0x6E, 0x64, 0x65, 0x78, 0x22, 0x3A, 0x20, 0x31, 0x31, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67, 0x65, 0x74, 0x5F, 0x74, 0x79, 0x70, 0x65, 0x22,
|
||||
0x3A, 0x20, 0x22, 0x6C, 0x69, 0x73, 0x74, 0x22, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67, 0x65, 0x74, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A,
|
||||
0x20, 0x22, 0x50, 0x6C, 0x65, 0x61, 0x73, 0x65, 0x20, 0x53, 0x65, 0x6C, 0x65, 0x63, 0x74, 0x20,
|
||||
0x45, 0x72, 0x72, 0x63, 0x6F, 0x64, 0x65, 0x22, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x22, 0x6C, 0x69, 0x73, 0x74, 0x5F, 0x69, 0x74, 0x65, 0x6D, 0x22, 0x3A, 0x20, 0x5B,
|
||||
0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E,
|
||||
0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x30, 0x78, 0x31, 0x45, 0x30, 0x32, 0x30, 0x30, 0x30,
|
||||
0x30, 0x22, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61,
|
||||
0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x30, 0x78, 0x31, 0x45, 0x30, 0x32, 0x30, 0x30, 0x30, 0x31,
|
||||
0x22, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D,
|
||||
0x65, 0x22, 0x3A, 0x20, 0x22, 0x30, 0x78, 0x31, 0x45, 0x30, 0x32, 0x30, 0x30, 0x30, 0x32, 0x22,
|
||||
0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65,
|
||||
0x22, 0x3A, 0x20, 0x22, 0x30, 0x78, 0x31, 0x45, 0x30, 0x32, 0x30, 0x30, 0x30, 0x33, 0x22, 0x0A,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22,
|
||||
0x3A, 0x20, 0x22, 0x30, 0x78, 0x31, 0x45, 0x30, 0x32, 0x30, 0x30, 0x30, 0x34, 0x22, 0x0A, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x5D, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77,
|
||||
0x69, 0x64, 0x67, 0x65, 0x74, 0x5F, 0x69, 0x6E, 0x64, 0x65, 0x78, 0x22, 0x3A, 0x20, 0x31, 0x32,
|
||||
0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67, 0x65,
|
||||
0x74, 0x5F, 0x74, 0x79, 0x70, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x6C, 0x69, 0x73, 0x74, 0x22, 0x2C,
|
||||
0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67, 0x65, 0x74,
|
||||
0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x50, 0x6C, 0x65, 0x61, 0x73, 0x65, 0x20,
|
||||
0x53, 0x65, 0x6C, 0x65, 0x63, 0x74, 0x20, 0x45, 0x72, 0x72, 0x4C, 0x65, 0x76, 0x65, 0x6C, 0x22,
|
||||
0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x6C, 0x69, 0x73, 0x74, 0x5F,
|
||||
0x69, 0x74, 0x65, 0x6D, 0x22, 0x3A, 0x20, 0x5B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x22, 0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x45,
|
||||
0x72, 0x72, 0x6F, 0x72, 0x20, 0x6C, 0x65, 0x76, 0x65, 0x6C, 0x33, 0x22, 0x0A, 0x20, 0x20, 0x20,
|
||||
0x72, 0x72, 0x6F, 0x72, 0x20, 0x6C, 0x65, 0x76, 0x65, 0x6C, 0x30, 0x22, 0x0A, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x22, 0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22,
|
||||
0x45, 0x72, 0x72, 0x6F, 0x72, 0x20, 0x6C, 0x65, 0x76, 0x65, 0x6C, 0x34, 0x22, 0x0A, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x5D, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69,
|
||||
0x64, 0x67, 0x65, 0x74, 0x5F, 0x69, 0x6E, 0x64, 0x65, 0x78, 0x22, 0x3A, 0x20, 0x31, 0x33, 0x2C,
|
||||
0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67, 0x65, 0x74,
|
||||
0x5F, 0x74, 0x79, 0x70, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x62, 0x75, 0x74, 0x74, 0x6F, 0x6E, 0x22,
|
||||
0x45, 0x72, 0x72, 0x6F, 0x72, 0x20, 0x6C, 0x65, 0x76, 0x65, 0x6C, 0x31, 0x22, 0x0A, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x22, 0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20,
|
||||
0x22, 0x45, 0x72, 0x72, 0x6F, 0x72, 0x20, 0x6C, 0x65, 0x76, 0x65, 0x6C, 0x32, 0x22, 0x0A, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x22, 0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A,
|
||||
0x20, 0x22, 0x45, 0x72, 0x72, 0x6F, 0x72, 0x20, 0x6C, 0x65, 0x76, 0x65, 0x6C, 0x33, 0x22, 0x0A,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x69, 0x74, 0x65, 0x6D, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22,
|
||||
0x3A, 0x20, 0x22, 0x45, 0x72, 0x72, 0x6F, 0x72, 0x20, 0x6C, 0x65, 0x76, 0x65, 0x6C, 0x34, 0x22,
|
||||
0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x0A, 0x20, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x5D, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x22, 0x77, 0x69, 0x64, 0x67, 0x65, 0x74, 0x5F, 0x69, 0x6E, 0x64, 0x65, 0x78, 0x22, 0x3A, 0x20,
|
||||
0x31, 0x33, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64,
|
||||
0x67, 0x65, 0x74, 0x5F, 0x74, 0x79, 0x70, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x62, 0x75, 0x74, 0x74,
|
||||
0x6F, 0x6E, 0x22, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69,
|
||||
0x64, 0x67, 0x65, 0x74, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x49, 0x6E, 0x6A,
|
||||
0x65, 0x63, 0x74, 0x22, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77,
|
||||
0x69, 0x64, 0x67, 0x65, 0x74, 0x5F, 0x69, 0x6E, 0x64, 0x65, 0x78, 0x22, 0x3A, 0x20, 0x31, 0x34,
|
||||
0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67, 0x65,
|
||||
0x74, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x49, 0x6E, 0x6A, 0x65, 0x63, 0x74,
|
||||
0x22, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20,
|
||||
0x20, 0x7B, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67,
|
||||
0x65, 0x74, 0x5F, 0x69, 0x6E, 0x64, 0x65, 0x78, 0x22, 0x3A, 0x20, 0x31, 0x34, 0x2C, 0x0A, 0x20,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67, 0x65, 0x74, 0x5F, 0x74,
|
||||
0x79, 0x70, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x62, 0x75, 0x74, 0x74, 0x6F, 0x6E, 0x22, 0x2C, 0x0A,
|
||||
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67, 0x65, 0x74, 0x5F,
|
||||
0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x45, 0x6C, 0x69, 0x6D, 0x69, 0x6E, 0x61, 0x74,
|
||||
0x65, 0x22, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x5D,
|
||||
0x0A, 0x20, 0x20, 0x7D, 0x0A, 0x7D, 0x0A
|
||||
0x74, 0x5F, 0x74, 0x79, 0x70, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x62, 0x75, 0x74, 0x74, 0x6F, 0x6E,
|
||||
0x22, 0x2C, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x22, 0x77, 0x69, 0x64, 0x67,
|
||||
0x65, 0x74, 0x5F, 0x6E, 0x61, 0x6D, 0x65, 0x22, 0x3A, 0x20, 0x22, 0x45, 0x6C, 0x69, 0x6D, 0x69,
|
||||
0x6E, 0x61, 0x74, 0x65, 0x22, 0x0A, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x7D, 0x0A, 0x20, 0x20,
|
||||
0x20, 0x20, 0x5D, 0x0A, 0x20, 0x20, 0x7D, 0x0A, 0x7D,
|
||||
};
|
||||
|
||||
#endif /* __widget_config_json_h_included */
|
||||
|
||||
@ -99,6 +99,7 @@ T_DjiReturnCode DjiTest_XPortStartService(void)
|
||||
//设置俯仰轴关节角(PITCH_JOINT_ANGLE)限位
|
||||
limitAngle.upperLimit = 300;
|
||||
limitAngle.lowerLimit = -1200;
|
||||
USER_LOG_INFO("Set joint angle limit of pitch axis, upperLimit %d, lowerLimit %d", limitAngle.upperLimit, limitAngle.lowerLimit);
|
||||
djiStat = DjiXPort_SetLimitAngleSync(DJI_XPORT_LIMIT_ANGLE_CATEGORY_PITCH_JOINT_ANGLE, limitAngle);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("set pitch joint angle limit angle for XPort error: 0x%08llX.", djiStat);
|
||||
@ -108,6 +109,7 @@ T_DjiReturnCode DjiTest_XPortStartService(void)
|
||||
//设置俯仰轴欧拉角(PITCH_EULER_ANGLE)限位
|
||||
limitAngle.upperLimit = 300;
|
||||
limitAngle.lowerLimit = -1200;
|
||||
USER_LOG_INFO("Set euler angle limit of pitch axis, upperLimit %d, lowerLimit %d", limitAngle.upperLimit, limitAngle.lowerLimit);
|
||||
djiStat = DjiXPort_SetLimitAngleSync(DJI_XPORT_LIMIT_ANGLE_CATEGORY_PITCH_EULER_ANGLE, limitAngle);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("set pitch euler angle limit angle for XPort error: 0x%08llX.", djiStat);
|
||||
@ -117,6 +119,7 @@ T_DjiReturnCode DjiTest_XPortStartService(void)
|
||||
//设置扩展俯仰轴欧拉角(PITCH_EULER_ANGLE_EXTENSION)限位
|
||||
limitAngle.upperLimit = 300;
|
||||
limitAngle.lowerLimit = -1000;
|
||||
USER_LOG_INFO("Set extended euler angle limit of pitch axis, upperLimit %d, lowerLimit %d", limitAngle.upperLimit, limitAngle.lowerLimit);
|
||||
djiStat = DjiXPort_SetLimitAngleSync(DJI_XPORT_LIMIT_ANGLE_CATEGORY_PITCH_EULER_ANGLE_EXTENSION, limitAngle);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("set pitch extension euler angle limit angle for XPort error: 0x%08llX.", djiStat);
|
||||
@ -140,11 +143,13 @@ T_DjiReturnCode DjiTest_XPortStartService(void)
|
||||
limitAngle.upperLimit = 1500;
|
||||
limitAngle.lowerLimit = -1500;
|
||||
} else {
|
||||
USER_LOG_WARN("payload mount position is unknown.");
|
||||
return DJI_ERROR_SYSTEM_MODULE_CODE_SYSTEM_ERROR;
|
||||
USER_LOG_WARN("unknown mounposition.");
|
||||
limitAngle.upperLimit = 300;
|
||||
limitAngle.lowerLimit = -1500;
|
||||
}
|
||||
|
||||
//设置航向轴关节角(YAW_JOINT_ANGLE)限位2、
|
||||
USER_LOG_INFO("Set joint angle limit of yaw axis, upperLimit %d, lowerLimit %d", limitAngle.upperLimit, limitAngle.lowerLimit);
|
||||
djiStat = DjiXPort_SetLimitAngleSync(DJI_XPORT_LIMIT_ANGLE_CATEGORY_YAW_JOINT_ANGLE, limitAngle);
|
||||
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
USER_LOG_ERROR("set yaw joint angle limit angle for XPort error: 0x%08llX.", djiStat);
|
||||
|
||||
Reference in New Issue
Block a user