当前位置：首页 > news >正文

4.DJI-PSDK云台x-port控制：

news 来源：原创 2025/9/17 18:24:15

DJI-PSDK云台x-port控制：

X-Port 功能控制，即控制 X-Port 云台，头文件为 dji_xport.h
使用PSDK 的“云台控制”功能，开发者需要先设计负载设备的云台并开发出控制云台的程序，将云台的控制函数注册到PSDK 指定的接口后，用户通过使用DJI Pilot、基于MSDK 开发的移动端App 及遥控器即可控制基于PSDK 开发的具有云台功能的负载设备，同时获得负载设备的相关信息，如姿态等。
参考文章：

https://developer.dji.com/doc/payload-sdk-tutorial/cn/function-set/advanced-function/x-port-function.html
https://developer.dji.com/doc/payload-sdk-tutorial/cn/function-set/basic-function/gimbal-function.html
https://developer.dji.com/doc/payload-sdk-api-reference/cn/practice/xport.html#10

1.基础概念：

1.1:云台关节和云台关节角：

云台的关节如图1.云台关节所示，云台关节是云台上带动负载设备转动的结构件：云台电机，云台关节角即云台电机转动的角度。本教程使用机体坐标系描述云台的关节角。
在这里插入图片描述

1.2:云台姿态和云台姿态角：

云台的姿态如图2.云台姿态所示，根据用户的控制指令，云台能够调整姿态；云台姿态角即使用大地坐标系（NED，北东地坐标系）描述云台上负载设备的角度，该角度也称为欧拉角。
在这里插入图片描述

2. xport云台模式：

云台模式决定了云台跟随无人机运动时的转动方式：

自由模式：当无人机的姿态改变时，云台将不会转动。
FPV 模式：当无人机的姿态发生改变时，云台会转动航向轴与横滚轴，确保负载设备当前的视场角不会发生改变。
YAW 跟随模式：在该模式下，云台的航向轴会跟随无人机的航向轴转动。

typedef enum {DJI_GIMBAL_MODE_FREE = 0, /*!< Free mode, fix gimbal attitude in the ground coordinate, ignoring movement of aircraft. */DJI_GIMBAL_MODE_FPV = 1, /*!< FPV (First Person View) mode, only control roll and yaw angle of gimbal in the ground coordinate to follow aircraft. */DJI_GIMBAL_MODE_YAW_FOLLOW = 2, /*!< Yaw follow mode, only control yaw angle of gimbal in the ground coordinate to follow aircraft. */
} E_DjiGimbalMode;

//设置云台飞行模式，自由模式/FPV模式/yaw模式djiStat = DjiXPort_SetGimbalModeSync(DJI_GIMBAL_MODE_FREE);if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("set XPort gimbal mode error: 0x%08llX.", djiStat);return djiStat;}

3. 云台限位角及控制权限说明：

在这里插入图片描述

4.云台复位：

云台复位接口参数结构体：

typedef enum {/*! 仅重置云台的偏航轴。将偏航轴的角度重置为飞行器的偏航角与云台的微调偏航角之和。 */DJI_GIMBAL_RESET_MODE_YAW = 1,/*! 重置云台的偏航轴和俯仰轴。将偏航轴的角度重置为飞行器的偏航角与云台的微调偏航角之和，并将俯仰轴的角度重置为微调角度。 */DJI_GIMBAL_RESET_MODE_PITCH_AND_YAW = 3,/*! 仅重置云台的俯仰轴。 */DJI_GIMBAL_RESET_MODE_PITCH_ONLY = 4,/*! 仅重置云台的滚转轴。 */DJI_GIMBAL_RESET_MODE_ROLL_ONLY = 5,/*! 仅重置云台的偏航轴。 */DJI_GIMBAL_RESET_MODE_YAW_ONLY = 6,/*! 重置云台的偏航轴和俯仰轴。将偏航轴的角度重置为飞行器的偏航角与云台的微调偏航角之和，并且如果云台向下，则将俯仰轴的角度重置为-90度与微调角度之和；如果向上，则重置为90度与微调角度之和。 */DJI_GIMBAL_RESET_MODE_PITCH_DOWNWARD_UPWARD_AND_YAW = 11,/*! 重置云台的俯仰轴。如果云台向下，则将俯仰轴的角度重置为-90度与微调角度之和；如果向上，则重置为90度与微调角度之和。 */DJI_GIMBAL_RESET_MODE_PITCH_DOWNWARD_UPWARD = 12,
} E_DjiGimbalResetMode;

使用：

//以阻塞的方式控制X-Port 复位(大地坐标系)。// djiStat = DjiXPort_ResetSync(DJI_GIMBAL_RESET_MODE_PITCH_AND_YAW);djiStat = DjiXPort_ResetSync(DJI_GIMBAL_RESET_MODE_PITCH_DOWNWARD_UPWARD);// DJI_GIMBAL_RESET_MODE_PITCH_DOWNWARD_UPWARD_AND_YAWif (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("reset XPort gimbal error: 0x%08llX.", djiStat);return djiStat;}

5.云台旋转控制：

接口说明：

T_DjiReturnCode DjiXPort_RotateSync(E_DjiGimbalRotationMode rotationMode,T_DjiGimbalRotationProperty rotationProperty, T_DjiAttitude3d rotationValue);

参数:
rotationMode：X-Port 的控制方式
rotationProperty：X-Port 旋转命令的属性
rotationValue：云台转动命令的值，当X-Port 的转动模式为 PSDK_GIMBAL_ROTATION_MODE_RELATIVE_ANGLE 或 PSDK_GIMBAL_ROTATION_MODE_ABSOLUTE_ANGLE 时，单位为0.1°；当X-Port 的转动模式为PSDK_GIMBAL_ROTATION_MODE_SPEED 时，单位为0.1°/s
返回值
根据程序执行的情况输出对应的返回值，详情请参见：DjiErrorCode

typedef enum {DJI_GIMBAL_ROTATION_MODE_RELATIVE_ANGLE = 0, /*!< 相对角度旋转模式，表示基于当前角度旋转云台指定的角度。 */DJI_GIMBAL_ROTATION_MODE_ABSOLUTE_ANGLE = 1, /*!< 绝对角度旋转模式，表示将云台旋转到地面坐标系中的指定角度。 */DJI_GIMBAL_ROTATION_MODE_SPEED = 2, /*!< 速度旋转模式，指定云台在地面坐标系中的旋转速度。 */
} E_DjiGimbalRotationMode;

相对角度旋转模式：在此模式下，云台将根据其当前的角度进行旋转。即，如果当前偏航角为30度，用户指定旋转10度，云台将转到40度。这种模式适用于希望基于当前状态进行增量调整的场景。
绝对角度旋转模式：此模式下，云台将旋转到地面坐标系中指定的绝对角度。例如，用户设定云台的偏航角为90度，无论其当前角度如何，云台都会调整到90度位置。这种模式非常适合需要精确定位的应用。
速度旋转模式：在此模式中，用户可以指定云台的旋转速度。与具体的角度无关，此模式下云台会以设定的速度进行旋转，这对于需要快速调整拍摄角度的场景非常有用，比如：遥控手柄

typedef struct {struct {bool pitch; /*!< 指定是否忽略俯仰轴的旋转命令，true 表示忽略。 */bool roll;  /*!< 指定是否忽略滚转轴的旋转命令，true 表示忽略。 */bool yaw;   /*!< 指定是否忽略偏航轴的旋转命令，true 表示忽略。 */} rotationValueInvalidFlag; /*!< 指定是否忽略某些轴的旋转命令，包括俯仰、滚转和偏航。 */union {struct {uint16_t actionTime; /*!< 在目标角度之间移动的动作时间，单位：0.01秒。 */} relativeAngleRotation; /*!< 相对角度旋转命令的属性。 */struct {uint16_t actionTime; /*!< 在目标角度之间移动的动作时间，单位：0.01秒。 *//*! 联动角度有效标志，指定在绝对角度控制时* T_DjiGimbalRotationProperty::absoluteAngleRotation::jointAngle 是否有效。 */bool jointAngleValid;/*! 云台的联动角度，单位：0.1度。如果* T_DjiGimbalRotationProperty::absoluteAngleRotation::jointAngleValid 为 false，则此处指定的联动角度无效，请忽略联动角度。如果联动角度有效，用户应确保* 云台的目标联动角度与指定值大致相同。 */T_DjiAttitude3d jointAngle;} absoluteAngleRotation; /*!< 绝对角度旋转命令的属性。 */};
} T_DjiGimbalRotationProperty;

rotationValueInvalidFlag: 每个布尔值表明是否忽略该轴的旋转命令：
relativeAngleRotation：含相对角度旋转命令的属性：actionTime: 表示在目标角度之间移动的时间，单位为 0.01 秒。例如，如果 actionTime 为 100，则表示移动时间为 1 秒。
absoluteAngleRotation：包含绝对角度旋转命令的属性：actionTime: 同样表示在目标角度之间移动的时间，单位为 0.01 秒。
jointAngleValid: 指定联动角度（即多个轴的联合运动）是否有效。如果为 true，则联动角度有效；如果为 false，则忽略该角度。
jointAngle: 表示云台的联动角度，单位为 0.1 度。如果 jointAngleValid 为 false，则此角度无效。用户在使用时应确保目标联动角度与指定值大致相同，以避免不必要的误差。

typedef struct {int32_t pitch; /*!< 指定俯仰角的 int32 值。 */int32_t roll;  /*!< 指定滚转角的 int32 值。 */int32_t yaw;   /*!< 指定偏航角的 int32 值。 */
} T_DjiAttitude3d;

T_DjiAttitude3d 结构体是用于表示三维空间中云台姿态的一个数据结构，其中包含俯仰、滚转和偏航三个角度的值。该结构体可以用于需要精确控制云台方向和姿态的应用场景，如无人机的相机稳定器、航拍设备等。通过设置这三个角度，用户可以实现对云台的准确定位和调整。

以阻塞的方式转动X-Port。

说明:
该接口执行的最大时间可能会大于600ms；在PSDK_GIMBAL_ROTATION_MODE_RELATIVE_ANGLE
或PSDK_GIMBAL_ROTATION_MODE_ABSOLUTE_ANGLE 模式下，偏航轴的转动范围为[-1800,
1800]。X-Port 转动角度的符号不决定X-Port 旋转的方向； X-Port 可到达的角度主要受X-Port
转动角度的限制和限位缓冲区的影响； X-Port 安装在无人机顶部时，可转动的角度范围与安装在无人机下置云台架上的范围可能会有较大的差异；
当X-Port 超出限位角角度时，X-Port 将会返回错误值，或停留在限位缓冲区内； X-Port
转动的速度和转动区间受最大转动速度和最大加速度的限制；X-Port 转动的最大加速度受云台转动平滑度的限制；X-Port
转动的最大速度受X-Port
默认最大速度（90°/s）和最大速度百分比的限制详情请参见：SetControllerSmoothFactor() 和
SetControllerMaxSpeedPercentage() ； PSDK 仅支持开发者控制X-Port 的俯仰轴和偏航轴；
为防止中断X-Port 的旋转，X-Port 在控制参数自动调整、同轴度检测过程、平衡检测和重置的过程中不会响应用户发送的旋转命令；
X-Port 的有效控制时间为500ms，若X-Port 在500ms 内未收到转动命令，X-Port 将无法转动。

6. 案例：实现云台画圈方案：

示例代码参考如下：
payload_xport.h:

/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef TEST_XPORT_H
#define TEST_XPORT_H/* Includes ------------------------------------------------------------------*/
#include "dji_typedef.h"
#include "dji_xport.h"#ifdef __cplusplus
extern "C" {
#endif/* Exported constants --------------------------------------------------------*//* Exported types ------------------------------------------------------------*//* Exported functions --------------------------------------------------------*/
T_DjiReturnCode DjiTest_XPortStartService(void);
T_DjiReturnCode DjiTest_XPortDeInit(void);
T_DjiReturnCode DjiTest_XPortGetSystemState(T_DjiGimbalSystemState *systemState);T_DjiReturnCode DjiXPort_ResetGimbal(void);
#ifdef __cplusplus
}
#endif#endif // TEST_XPORT_H/************************ (C) COPYRIGHT DJI Innovations *******END OF FILE******/

payload_xport.c:

/* Includes ------------------------------------------------------------------*/
#include <dji_gimbal.h>
#include "payload_xport.h"
#include "dji_logger.h"
#include "dji_platform.h"
#include "utils/util_misc.h"
#include "dji_aircraft_info.h"
#include "psdk_gimbal.h"
#include "subscription.h"
// #include "camera_emu/test_payload_cam_emu_base.h"
#include <math.h> // 加入 math.h#ifndef M_PI
#define M_PI 3.14159265358979323846 // 定义 M_PI
#define RADIUS 300  // 定义圆的半径
#define CIRCLE_POINTS 10 // 圆的分割点数量
#endif/* Private constants ---------------------------------------------------------*/
#define XPORT_TASK_FREQ             (10)      //设置xport任务频率，每s执行10次
#define XPORT_TASK_STACK_SIZE       (2048)    //为任务分配2048字节的栈空间extern dji_f32_t height; // 声明在其他文件中定义的变量
extern bool s_isXortOn;/* Private types -------------------------------------------------------------*//* Private functions declaration ---------------------------------------------*/
static void *UserXPort_Task(void *arg);   //任务函数，在循环中执行xport的相关操作
static T_DjiReturnCode ReceiveXPortSystemState(T_DjiGimbalSystemState systemState); //回调函数，用于接收xport系统状态
static T_DjiReturnCode ReceiveXPortAttitudeInformation(T_DjiGimbalAttitudeInformation attitudeInformation); //回调函数，接收xport姿态信息/* Private variables ---------------------------------------------------------*/
static T_DjiTaskHandle s_userXPortThread;
static T_DjiMutexHandle s_userXPortMutex;
static T_DjiGimbalSystemState s_userXPortSystemState = {0};
static bool s_isUserXPortInited = false;
static bool s_isUserXPortSystemStateVaild = false;/* Exported functions definition ---------------------------------------------*/
//初始化和启动xport服务
T_DjiReturnCode DjiTest_XPortStartService(void)
{USER_LOG_ERROR("set serial number 22222222222222");T_DjiReturnCode djiStat;T_DjiXPortLimitAngle limitAngle = {0};T_DjiAircraftInfoBaseInfo aircraftInfoBaseInfo = {0};T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();//初始化xport模块djiStat = DjiXPort_Init();if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("XPort init error: 0x%08llX.", djiStat);return djiStat;}s_isUserXPortInited = true;//创建互斥锁djiStat = osalHandler->MutexCreate(&s_userXPortMutex);if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("user XPort mutex create error: 0x%08llX.", djiStat);return djiStat;}//注册回调函数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;}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.",djiStat);return djiStat;}//设置云台俯仰轴关节角角度限制limitAngle.upperLimit = 300;limitAngle.lowerLimit = -1000;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;}//设置云台俯仰轴欧拉角角度限制（在大地坐标系下）limitAngle.upperLimit = 300;limitAngle.lowerLimit = -900;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;}//扩展俯仰轴欧拉角限制（在大地坐标系下），使用该功能时，请开启扩展俯仰轴角度限制功能limitAngle.upperLimit = 300;limitAngle.lowerLimit = -1200;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);return djiStat;}//获取飞行器的基本信息，以此决定角度限制的具体值djiStat = DjiAircraftInfo_GetBaseInfo(&aircraftInfoBaseInfo);if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("get aircraft base information error: 0x%08llX.", djiStat);return djiStat;}if (aircraftInfoBaseInfo.mountPosition == DJI_MOUNT_POSITION_PAYLOAD_PORT_NO1) {limitAngle.upperLimit = 1500;limitAngle.lowerLimit = -1500;} else if (aircraftInfoBaseInfo.mountPosition == DJI_MOUNT_POSITION_PAYLOAD_PORT_NO2) {limitAngle.upperLimit = 1500;limitAngle.lowerLimit = -1500;} else if (aircraftInfoBaseInfo.mountPosition == DJI_MOUNT_POSITION_PAYLOAD_PORT_NO3) {limitAngle.upperLimit = 1500;limitAngle.lowerLimit = -1500;} else {USER_LOG_WARN("payload mount position is unknown.");return DJI_ERROR_SYSTEM_MODULE_CODE_SYSTEM_ERROR;}//偏航轴关节角限制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;}//设置云台飞行模式，自由模式/FPV模式/yaw模式djiStat = DjiXPort_SetGimbalModeSync(DJI_GIMBAL_MODE_FREE);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);djiStat = DjiXPort_ResetSync(DJI_GIMBAL_RESET_MODE_PITCH_DOWNWARD_UPWARD);// DJI_GIMBAL_RESET_MODE_PITCH_DOWNWARD_UPWARD_AND_YAWif (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("reset XPort gimbal error: 0x%08llX.", djiStat);return djiStat;}USER_LOG_ERROR("set serial number 33333333333333");//开启用户任务函数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.");return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;}return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
//销毁和反初始化
T_DjiReturnCode DjiTest_XPortDeInit(void)
{T_DjiReturnCode djiStat;T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();djiStat = osalHandler->TaskDestroy(s_userXPortThread);if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("Destroy test xport thread error: 0x%08llX.", djiStat);return djiStat;}djiStat = osalHandler->MutexDestroy(s_userXPortMutex);if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("Destroy test xport mutex error: 0x%08llX.", djiStat);return djiStat;}djiStat = DjiXPort_DeInit();if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("XPort de-init error: 0x%08llX.", djiStat);return djiStat;}s_isUserXPortInited = false;return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
//获取系统状态信息
T_DjiReturnCode DjiTest_XPortGetSystemState(T_DjiGimbalSystemState *systemState)
{T_DjiReturnCode returnCode;T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();if (s_isUserXPortInited != true || s_isUserXPortSystemStateVaild != true) {USER_LOG_ERROR("user XPort has not inited.");return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;}returnCode = osalHandler->MutexLock(s_userXPortMutex);if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("user XPort mutex lock error: 0x%08llX.", returnCode);return returnCode;}memcpy(systemState, &s_userXPortSystemState, sizeof(T_DjiGimbalSystemState));returnCode = osalHandler->MutexUnlock(s_userXPortMutex);if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("user XPort mutex unlock error: 0x%08llX.", returnCode);return returnCode;}return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}static int flag_status = 0;
static void *UserXPort_Task(void *arg)
{T_DjiReturnCode djiStat;T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();USER_UTIL_UNUSED(arg);// 初始化旋转值和属性T_DjiAttitude3d rotationValue = {0};T_DjiGimbalRotationProperty rotationProperty = {0};// 设置无效标志rotationProperty.rotationValueInvalidFlag.pitch = false; rotationProperty.rotationValueInvalidFlag.roll = false;  rotationProperty.rotationValueInvalidFlag.yaw = true;  rotationProperty.relativeAngleRotation.actionTime = 100; // 单位是0.01srotationProperty.absoluteAngleRotation.actionTime = 100;  rotationProperty.absoluteAngleRotation.jointAngleValid = false; osalHandler->TaskSleepMs(500);int loopCounter = 0; // 循环计数器float theta = 0.0f;  // 角度变量// const float step = 0.628f; // 每次增加的角度步长const float step = 1.256f; // 每次增加的角度步长const int radius = 300;   // 圆的半径while (1) {if (s_isXortOn) {if(flag_status == 0){DjiXPort_ResetSync(DJI_GIMBAL_RESET_MODE_PITCH_DOWNWARD_UPWARD_AND_YAW);flag_status = 1;osalHandler->TaskSleepMs(500);}// 计算俯仰和滚转值rotationValue.pitch = radius * sin(theta); // 俯仰值rotationValue.roll = radius * cos(theta);  // 滚转值// 调用 API 旋转云台djiStat = DjiXPort_RotateSync(DJI_GIMBAL_ROTATION_MODE_SPEED, rotationProperty, rotationValue);if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("set XPort gimbal angle error: 0x%08llX.", djiStat);}// 更新角度theta += step;if (theta > 2 * M_PI) { // 如果超过 2π，回到 0theta -= 2 * M_PI;}// 每循环一定次数调用一次重置loopCounter++;if (loopCounter >= 180) { // 每 100 次循环重置一次DjiXPort_ResetSync(DJI_GIMBAL_RESET_MODE_PITCH_DOWNWARD_UPWARD_AND_YAW);loopCounter = 0; // 重置计数器}}else if (!s_isXortOn && flag_status == 1){DjiXPort_ResetSync(DJI_GIMBAL_RESET_MODE_PITCH_DOWNWARD_UPWARD_AND_YAW);flag_status = 0;osalHandler->TaskSleepMs(250);DjiXPort_ResetSync(DJI_GIMBAL_RESET_MODE_ROLL_ONLY);osalHandler->TaskSleepMs(250);}osalHandler->TaskSleepMs(100);}
}#ifndef __CC_ARM
#pragma GCC diagnostic pop
#endif
//接收系统状态回调函数
static T_DjiReturnCode ReceiveXPortSystemState(T_DjiGimbalSystemState systemState)
{T_DjiReturnCode returnCode;T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();returnCode = osalHandler->MutexLock(s_userXPortMutex);if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("user XPort mutex lock error: 0x%08llX.", returnCode);return returnCode;}s_isUserXPortSystemStateVaild = true;memcpy(&s_userXPortSystemState, &systemState, sizeof(T_DjiGimbalSystemState));returnCode = osalHandler->MutexUnlock(s_userXPortMutex);if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("user XPort mutex unlock error: 0x%08llX.", returnCode);return returnCode;}USER_LOG_DEBUG("receive XPort system state: mounted upward flag %d, gimbal mode %d.",systemState.mountedUpward, systemState.gimbalMode);USER_LOG_DEBUG("XPort fine tune: %d %d %d.", systemState.fineTuneAngle.pitch,systemState.fineTuneAngle.roll, systemState.fineTuneAngle.yaw);return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
//接收姿态信息回调函数
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);return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}//新添加的复位云台接口
T_DjiReturnCode DjiXPort_ResetGimbal(void) {T_DjiReturnCode 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;}USER_LOG_ERROR("XPort gimbal reset successfully.");return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/