tangchao0503/Payload-SDK
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

xport

Browse Source 
1、解决问题：飞机急停，俯仰角异常；
2、获取云台的姿态角，并显示到遥控器上；
This commit is contained in:
tangchao0503
2023-08-14 14:35:32 +08:00
parent 6688d23e13
commit bf32d053a6
1 changed files with 37 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
samples/sample_c/module_sample/xport/test_payload_xport.c
View File

@ -40,9 +40,9 @@
/* Private functions declaration ---------------------------------------------*/
static void *UserXPort_Task(void *arg);
static T_DjiReturnCode ReceiveXPortSystemState(T_DjiGimbalSystemState systemState);
static T_DjiReturnCode ReceiveXPortAttitudeInformation(T_DjiGimbalAttitudeInformation attitudeInformation);
static void *UserXPort_Task(void *arg);//用户自定义xport任务
static T_DjiReturnCode ReceiveXPortSystemState(T_DjiGimbalSystemState systemState);//通过构造回调函数接受xport的状态信息
static T_DjiReturnCode ReceiveXPortAttitudeInformation(T_DjiGimbalAttitudeInformation attitudeInformation);//构造回调函数获取X-Port 的姿态，在回调函数中打印X-Port 的姿态信息。
/* Private variables ---------------------------------------------------------*/
static T_DjiTaskHandle s_userXPortThread;
@ -51,6 +51,10 @@ static T_DjiGimbalSystemState s_userXPortSystemState = {0};
static bool s_isUserXPortInited = false;
static bool s_isUserXPortSystemStateVaild = false;
extern char * s_strPitch;
extern char * s_strRoll;
extern char * s_strYaw;
/* Exported functions definition ---------------------------------------------*/
T_DjiReturnCode DjiTest_XPortStartService(void)
{
@ -59,6 +63,7 @@ T_DjiReturnCode DjiTest_XPortStartService(void)
T_DjiAircraftInfoBaseInfo aircraftInfoBaseInfo = {0};
T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
//X-Port 功能模块初始化
djiStat = DjiXPort_Init();
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("XPort init error: 0x%08llX.", djiStat);
@ -73,12 +78,14 @@ T_DjiReturnCode DjiTest_XPortStartService(void)
return djiStat;
}
//通过注册回调函数接收X-Port 的状态信息。注册后，回调函数将会被自动调用，调用频率为
djiStat = DjiXPort_RegReceiveSystemStateCallback(ReceiveXPortSystemState);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("register receive XPort system state callback function error: 0x%08llX.", djiStat);
return djiStat;
}
//通过注册回调函数获取X-Port 的姿态信息.注册后，回调函数将会被自动调用，调用频率为
djiStat = DjiXPort_RegReceiveAttitudeInformationCallback(ReceiveXPortAttitudeInformation);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("register receive XPort attitude information callback function error: 0x%08llX.",
@ -86,22 +93,28 @@ T_DjiReturnCode DjiTest_XPortStartService(void)
return djiStat;
}
//配置xport信息；开始------------------------------------------------------------------------------------------------------
//下述代码中X-Port 的角度单位为0.1度
//设置俯仰轴关节角（PITCH_JOINT_ANGLE）限位
limitAngle.upperLimit = 300;
limitAngle.lowerLimit = -1000;
limitAngle.lowerLimit = -1200;
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);
return djiStat;
}
//设置俯仰轴欧拉角（PITCH_EULER_ANGLE）限位
limitAngle.upperLimit = 300;
limitAngle.lowerLimit = -800;
limitAngle.lowerLimit = -1200;
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);
return djiStat;
}
//设置扩展俯仰轴欧拉角（PITCH_EULER_ANGLE_EXTENSION）限位
limitAngle.upperLimit = 300;
limitAngle.lowerLimit = -1000;
djiStat = DjiXPort_SetLimitAngleSync(DJI_XPORT_LIMIT_ANGLE_CATEGORY_PITCH_EULER_ANGLE_EXTENSION, limitAngle);
@ -110,6 +123,7 @@ T_DjiReturnCode DjiTest_XPortStartService(void)
return djiStat;
}
//设置航向轴关节角（YAW_JOINT_ANGLE）限位1、获取飞机信息 → 判断云台在飞机上的挂载点
djiStat = DjiAircraftInfo_GetBaseInfo(&aircraftInfoBaseInfo);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("get aircraft base information error: 0x%08llX.", djiStat);
@ -130,24 +144,28 @@ T_DjiReturnCode DjiTest_XPortStartService(void)
return DJI_ERROR_SYSTEM_MODULE_CODE_SYSTEM_ERROR;
}
//设置航向轴关节角（YAW_JOINT_ANGLE）限位2、
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);
return djiStat;
}
djiStat = DjiXPort_SetGimbalModeSync(DJI_GIMBAL_MODE_FREE);
//设置X-Port 工作模式
djiStat = DjiXPort_SetGimbalModeSync(DJI_GIMBAL_MODE_YAW_FOLLOW);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("set XPort gimbal mode error: 0x%08llX.", djiStat);
return djiStat;
}
//使用X-Port 复位功能
djiStat = DjiXPort_ResetSync(DJI_GIMBAL_RESET_MODE_PITCH_AND_YAW);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("reset XPort gimbal error: 0x%08llX.", djiStat);
return djiStat;
}
//用户自定义xport功能
if (osalHandler->TaskCreate("user_xport_task", UserXPort_Task, XPORT_TASK_STACK_SIZE, NULL, &s_userXPortThread) !=
DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
USER_LOG_ERROR("user XPort task create error.");
@ -219,6 +237,7 @@ T_DjiReturnCode DjiTest_XPortGetSystemState(T_DjiGimbalSystemState *systemState)
#pragma GCC diagnostic ignored "-Wreturn-type"
#endif
//用户自定义xport功能函数实现
static void *UserXPort_Task(void *arg)
{
T_DjiReturnCode djiStat;
@ -234,6 +253,7 @@ static void *UserXPort_Task(void *arg)
osalHandler->TaskSleepMs(1000 / XPORT_TASK_FREQ);
step++;
//XPORT_TASK_FREQ（10）的整数才执行if后面的代码：获取xport限位角并打印出来
if (USER_UTIL_IS_WORK_TURN(step, 1, XPORT_TASK_FREQ)) {
djiStat = DjiXPort_GetLimitAngleSync(DJI_XPORT_LIMIT_ANGLE_CATEGORY_ROLL_JOINT_ANGLE, &limitAngle);
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
@ -283,6 +303,7 @@ static void *UserXPort_Task(void *arg)
limitAngle.upperLimit, limitAngle.lowerLimit);
}
//XPORT_TASK_FREQ（1）的整数才执行if后面的代码：设置xport速度转换系数
if (USER_UTIL_IS_WORK_TURN(step, 10, XPORT_TASK_FREQ)) {
if (DjiTest_CameraIsInited()) {
djiStat = DjiTest_CameraGetOpticalZoomFactor(&opticalZoomFactor);
@ -340,12 +361,22 @@ static T_DjiReturnCode ReceiveXPortSystemState(T_DjiGimbalSystemState systemStat
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
char * pitchTmp[10];
char * rollTmp[10];
char * yawTmp[10];
static T_DjiReturnCode ReceiveXPortAttitudeInformation(T_DjiGimbalAttitudeInformation attitudeInformation)
{
USER_LOG_DEBUG("receive XPort attitude information:");
USER_LOG_DEBUG("XPort attitude: pitch %d, roll %d, yaw %d.", attitudeInformation.attitude.pitch,
attitudeInformation.attitude.roll, attitudeInformation.attitude.yaw);
sprintf(pitchTmp, "%d", attitudeInformation.attitude.pitch);
sprintf(rollTmp, "%d", attitudeInformation.attitude.roll);
sprintf(yawTmp, "%d", attitudeInformation.attitude.yaw);
s_strPitch = pitchTmp;
s_strRoll = rollTmp;
s_strYaw = yawTmp;
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}