NEW: sync the code of DJI Payload-SDK version 2.2.1 released on January 20 2021.
Signed-off-by: DJI-Martin <DJI-Martin@dji.com>
This commit is contained in:
DJI-Martin
349
sample/api_sample/xport/test_xport.c
Normal file
349
sample/api_sample/xport/test_xport.c
Normal file
@ -0,0 +1,349 @@
|
||||
/**
|
||||
********************************************************************
|
||||
* @file test_xport.c
|
||||
* @version V2.0.0
|
||||
* @date 2019/10/16
|
||||
* @brief
|
||||
*
|
||||
* @copyright (c) 2018-2019 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 <psdk_gimbal.h>
|
||||
#include "test_xport.h"
|
||||
#include "psdk_logger.h"
|
||||
#include "psdk_platform.h"
|
||||
#include "utils/util_misc.h"
|
||||
#include "psdk_aircraft_info.h"
|
||||
#include "camera_emu/test_payload_cam_emu.h"
|
||||
|
||||
/* Private constants ---------------------------------------------------------*/
|
||||
#define XPORT_TASK_FREQ (10)
|
||||
#define XPORT_TASK_STACK_SIZE (2048)
|
||||
|
||||
/* Private types -------------------------------------------------------------*/
|
||||
|
||||
|
||||
/* Private functions declaration ---------------------------------------------*/
|
||||
static void *UserXPort_Task(void *arg);
|
||||
static T_PsdkReturnCode ReceiveXPortSystemState(T_PsdkGimbalSystemState systemState);
|
||||
static T_PsdkReturnCode ReceiveXPortAttitudeInformation(T_PsdkGimbalAttitudeInformation attitudeInformation);
|
||||
|
||||
/* Private variables ---------------------------------------------------------*/
|
||||
static T_PsdkTaskHandle s_userXPortThread;
|
||||
static T_PsdkMutexHandle s_userXPortMutex;
|
||||
static T_PsdkGimbalSystemState s_userXPortSystemState = {0};
|
||||
static bool s_isUserXPortInited = false;
|
||||
static bool s_isUserXPortSystemStateVaild = false;
|
||||
|
||||
/* Exported functions definition ---------------------------------------------*/
|
||||
T_PsdkReturnCode PsdkTest_XPortInit(void)
|
||||
{
|
||||
T_PsdkReturnCode psdkStat;
|
||||
T_PsdkXPortLimitAngle limitAngle = {0};
|
||||
T_PsdkAircraftInfoBaseInfo aircraftInfoBaseInfo = {0};
|
||||
|
||||
psdkStat = PsdkXPort_Init();
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("XPort init error: 0x%08llX.", psdkStat);
|
||||
return psdkStat;
|
||||
}
|
||||
|
||||
s_isUserXPortInited = true;
|
||||
|
||||
psdkStat = PsdkOsal_MutexCreate(&s_userXPortMutex);
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("user XPort mutex create error: 0x%08llX.", psdkStat);
|
||||
return psdkStat;
|
||||
}
|
||||
|
||||
psdkStat = PsdkXPort_RegReceiveSystemStateCallback(ReceiveXPortSystemState);
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("register receive XPort system state callback function error: 0x%08llX.", psdkStat);
|
||||
return psdkStat;
|
||||
}
|
||||
|
||||
psdkStat = PsdkXPort_RegReceiveAttitudeInformationCallback(ReceiveXPortAttitudeInformation);
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("register receive XPort attitude information callback function error: 0x%08llX.",
|
||||
psdkStat);
|
||||
return psdkStat;
|
||||
}
|
||||
|
||||
limitAngle.upperLimit = 300;
|
||||
limitAngle.lowerLimit = -1000;
|
||||
psdkStat = PsdkXPort_SetLimitAngleSync(PSDK_XPORT_LIMIT_ANGLE_CATEGORY_PITCH_JOINT_ANGLE, limitAngle);
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("set pitch joint angle limit angle for XPort error: 0x%08llX.", psdkStat);
|
||||
return psdkStat;
|
||||
}
|
||||
|
||||
limitAngle.upperLimit = 300;
|
||||
limitAngle.lowerLimit = -800;
|
||||
psdkStat = PsdkXPort_SetLimitAngleSync(PSDK_XPORT_LIMIT_ANGLE_CATEGORY_PITCH_EULER_ANGLE, limitAngle);
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("set pitch euler angle limit angle for XPort error: 0x%08llX.", psdkStat);
|
||||
return psdkStat;
|
||||
}
|
||||
|
||||
limitAngle.upperLimit = 300;
|
||||
limitAngle.lowerLimit = -1000;
|
||||
psdkStat = PsdkXPort_SetLimitAngleSync(PSDK_XPORT_LIMIT_ANGLE_CATEGORY_PITCH_EULER_ANGLE_EXTENSION, limitAngle);
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("set pitch extension euler angle limit angle for XPort error: 0x%08llX.", psdkStat);
|
||||
return psdkStat;
|
||||
}
|
||||
|
||||
psdkStat = PsdkAircraftInfo_GetBaseInfo(&aircraftInfoBaseInfo);
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("get aircraft base information error: 0x%08llX.", psdkStat);
|
||||
return psdkStat;
|
||||
}
|
||||
|
||||
if (aircraftInfoBaseInfo.payloadMountPosition == PSDK_AIRCRAFT_INFO_PAYLOAD_MOUNT_POSITION_NO1) {
|
||||
limitAngle.upperLimit = 300;
|
||||
limitAngle.lowerLimit = -1500;
|
||||
} else if (aircraftInfoBaseInfo.payloadMountPosition == PSDK_AIRCRAFT_INFO_PAYLOAD_MOUNT_POSITION_NO2) {
|
||||
limitAngle.upperLimit = 1500;
|
||||
limitAngle.lowerLimit = -300;
|
||||
} else if (aircraftInfoBaseInfo.payloadMountPosition == PSDK_AIRCRAFT_INFO_PAYLOAD_MOUNT_POSITION_NO3) {
|
||||
limitAngle.upperLimit = 1500;
|
||||
limitAngle.lowerLimit = -1500;
|
||||
} else {
|
||||
PsdkLogger_UserLogWarn("payload mount position is unknown.");
|
||||
return PSDK_ERROR_SYSTEM_MODULE_CODE_SYSTEM_ERROR;
|
||||
}
|
||||
|
||||
psdkStat = PsdkXPort_SetLimitAngleSync(PSDK_XPORT_LIMIT_ANGLE_CATEGORY_YAW_JOINT_ANGLE, limitAngle);
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("set yaw joint angle limit angle for XPort error: 0x%08llX.", psdkStat);
|
||||
return psdkStat;
|
||||
}
|
||||
|
||||
psdkStat = PsdkXPort_SetGimbalModeSync(PSDK_GIMBAL_MODE_FREE);
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("set XPort gimbal mode error: 0x%08llX.", psdkStat);
|
||||
return psdkStat;
|
||||
}
|
||||
|
||||
psdkStat = PsdkXPort_ResetSync(PSDK_GIMBAL_RESET_MODE_PITCH_AND_YAW);
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("reset XPort gimbal error: 0x%08llX.", psdkStat);
|
||||
return psdkStat;
|
||||
}
|
||||
|
||||
if (PsdkOsal_TaskCreate(&s_userXPortThread, UserXPort_Task, "user_xport_task", XPORT_TASK_STACK_SIZE, NULL) !=
|
||||
PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("user XPort task create error.");
|
||||
return PSDK_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
|
||||
}
|
||||
|
||||
return PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
T_PsdkReturnCode PsdkTest_XPortDeInit(void)
|
||||
{
|
||||
T_PsdkReturnCode psdkStat;
|
||||
|
||||
psdkStat = PsdkOsal_TaskDestroy(s_userXPortThread);
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("Destroy test xport thread error: 0x%08llX.", psdkStat);
|
||||
return psdkStat;
|
||||
}
|
||||
|
||||
psdkStat = PsdkOsal_MutexDestroy(s_userXPortMutex);
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("Destroy test xport mutex error: 0x%08llX.", psdkStat);
|
||||
return psdkStat;
|
||||
}
|
||||
|
||||
psdkStat = PsdkXPort_DeInit();
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("XPort de-init error: 0x%08llX.", psdkStat);
|
||||
return psdkStat;
|
||||
}
|
||||
|
||||
s_isUserXPortInited = false;
|
||||
|
||||
return PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
T_PsdkReturnCode PsdkTest_XPortGetSystemState(T_PsdkGimbalSystemState *systemState)
|
||||
{
|
||||
T_PsdkReturnCode returnCode;
|
||||
|
||||
if (s_isUserXPortInited != true || s_isUserXPortSystemStateVaild != true) {
|
||||
PsdkLogger_UserLogWarn("user XPort has not inited.");
|
||||
return PSDK_ERROR_SYSTEM_MODULE_CODE_NONSUPPORT_IN_CURRENT_STATE;
|
||||
}
|
||||
|
||||
returnCode = PsdkOsal_MutexLock(s_userXPortMutex);
|
||||
if (returnCode != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("user XPort mutex lock error: 0x%08llX.", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
memcpy(systemState, &s_userXPortSystemState, sizeof(T_PsdkGimbalSystemState));
|
||||
|
||||
returnCode = PsdkOsal_MutexUnlock(s_userXPortMutex);
|
||||
if (returnCode != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("user XPort mutex unlock error: 0x%08llX.", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
return PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
/* Private functions definition-----------------------------------------------*/
|
||||
#ifndef __CC_ARM
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wmissing-noreturn"
|
||||
#pragma GCC diagnostic ignored "-Wreturn-type"
|
||||
#endif
|
||||
|
||||
static void *UserXPort_Task(void *arg)
|
||||
{
|
||||
T_PsdkReturnCode psdkStat;
|
||||
T_PsdkXPortLimitAngle limitAngle = {0};
|
||||
float opticalZoomFactor = 1.0f;
|
||||
float digitalZoomFactor = 1.0f;
|
||||
int step = 0;
|
||||
|
||||
USER_UTIL_UNUSED(arg);
|
||||
|
||||
while (1) {
|
||||
PsdkOsal_TaskSleepMs(1000 / XPORT_TASK_FREQ);
|
||||
step++;
|
||||
|
||||
if (USER_UTIL_IS_WORK_TURN(step, 1, XPORT_TASK_FREQ)) {
|
||||
psdkStat = PsdkXPort_GetLimitAngleSync(PSDK_XPORT_LIMIT_ANGLE_CATEGORY_ROLL_JOINT_ANGLE, &limitAngle);
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("get roll joint angle limit angle from XPort error: 0x%08llX.", psdkStat);
|
||||
continue;
|
||||
}
|
||||
|
||||
PsdkLogger_UserLogDebug("roll joint angle limit angle of XPort: upper limit %d, lower limit %d.",
|
||||
limitAngle.upperLimit, limitAngle.lowerLimit);
|
||||
|
||||
psdkStat = PsdkXPort_GetLimitAngleSync(PSDK_XPORT_LIMIT_ANGLE_CATEGORY_PITCH_JOINT_ANGLE, &limitAngle);
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("get pitch joint angle limit angle from XPort error: 0x%08llX.", psdkStat);
|
||||
continue;
|
||||
}
|
||||
|
||||
PsdkLogger_UserLogDebug("pitch joint angle limit angle of XPort: upper limit %d, lower limit %d.",
|
||||
limitAngle.upperLimit, limitAngle.lowerLimit);
|
||||
|
||||
psdkStat = PsdkXPort_GetLimitAngleSync(PSDK_XPORT_LIMIT_ANGLE_CATEGORY_PITCH_EULER_ANGLE, &limitAngle);
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("get pitch euler angle limit angle from XPort error: 0x%08llX.", psdkStat);
|
||||
continue;
|
||||
}
|
||||
|
||||
PsdkLogger_UserLogDebug("pitch euler angle limit angle of XPort: upper limit %d, lower limit %d.",
|
||||
limitAngle.upperLimit, limitAngle.lowerLimit);
|
||||
|
||||
psdkStat = PsdkXPort_GetLimitAngleSync(PSDK_XPORT_LIMIT_ANGLE_CATEGORY_PITCH_EULER_ANGLE_EXTENSION,
|
||||
&limitAngle);
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("get pitch extension euler angle limit angle from XPort error: 0x%08llX.",
|
||||
psdkStat);
|
||||
continue;
|
||||
}
|
||||
|
||||
PsdkLogger_UserLogDebug("pitch extension euler angle limit angle of XPort: upper limit %d, lower limit %d.",
|
||||
limitAngle.upperLimit, limitAngle.lowerLimit);
|
||||
|
||||
psdkStat = PsdkXPort_GetLimitAngleSync(PSDK_XPORT_LIMIT_ANGLE_CATEGORY_YAW_JOINT_ANGLE, &limitAngle);
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("get yaw joint angle limit angle from XPort error: 0x%08llX.", psdkStat);
|
||||
continue;
|
||||
}
|
||||
|
||||
PsdkLogger_UserLogDebug("yaw joint angle limit angle of XPort: upper limit %d, lower limit %d.",
|
||||
limitAngle.upperLimit, limitAngle.lowerLimit);
|
||||
}
|
||||
|
||||
if (USER_UTIL_IS_WORK_TURN(step, 10, XPORT_TASK_FREQ)) {
|
||||
if (PsdkTest_CameraIsInited()) {
|
||||
psdkStat = PsdkTest_CameraGetOpticalZoomFactor(&opticalZoomFactor);
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("get camera optical zoom factor error: %d.", psdkStat);
|
||||
continue;
|
||||
}
|
||||
|
||||
psdkStat = PsdkTest_CameraGetDigitalZoomFactor(&digitalZoomFactor);
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("get camera digital zoom factor error: %d.", psdkStat);
|
||||
continue;
|
||||
}
|
||||
|
||||
psdkStat = PsdkXPort_SetSpeedConversionFactor(1 / (opticalZoomFactor * digitalZoomFactor));
|
||||
if (psdkStat != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("set speed conversion factor error: %d.", psdkStat);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#ifndef __CC_ARM
|
||||
#pragma GCC diagnostic pop
|
||||
#endif
|
||||
|
||||
static T_PsdkReturnCode ReceiveXPortSystemState(T_PsdkGimbalSystemState systemState)
|
||||
{
|
||||
T_PsdkReturnCode returnCode;
|
||||
|
||||
returnCode = PsdkOsal_MutexLock(s_userXPortMutex);
|
||||
if (returnCode != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("user XPort mutex lock error: 0x%08llX.", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
s_isUserXPortSystemStateVaild = true;
|
||||
memcpy(&s_userXPortSystemState, &systemState, sizeof(T_PsdkGimbalSystemState));
|
||||
|
||||
returnCode = PsdkOsal_MutexUnlock(s_userXPortMutex);
|
||||
if (returnCode != PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
|
||||
PsdkLogger_UserLogError("user XPort mutex unlock error: 0x%08llX.", returnCode);
|
||||
return returnCode;
|
||||
}
|
||||
|
||||
PsdkLogger_UserLogDebug("receive XPort system state: mounted upward flag %d, gimbal mode %d.",
|
||||
systemState.mountedUpward, systemState.gimbalMode);
|
||||
|
||||
PsdkLogger_UserLogDebug("XPort fine tune: %d %d %d.", systemState.fineTuneAngle.pitch,
|
||||
systemState.fineTuneAngle.roll, systemState.fineTuneAngle.yaw);
|
||||
|
||||
return PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
static T_PsdkReturnCode ReceiveXPortAttitudeInformation(T_PsdkGimbalAttitudeInformation attitudeInformation)
|
||||
{
|
||||
PsdkLogger_UserLogDebug("receive XPort attitude information:");
|
||||
PsdkLogger_UserLogDebug("XPort attitude: pitch %d, roll %d, yaw %d.", attitudeInformation.attitude.pitch,
|
||||
attitudeInformation.attitude.roll, attitudeInformation.attitude.yaw);
|
||||
|
||||
return PSDK_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
|
||||
}
|
||||
|
||||
/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/
|
||||
Reference in New Issue
Block a user