此组别内的文章

查看更多

5.8 云台功能组件（gimbal_emu）

kyle.cai
  • 更新
关注

5.8 云台功能组件（gimbal_emu）

5.8.1 基本功能介绍

云台功能组件（gimbal emu）是原PSDK端口功能，简单来说该功能用于开发第三方的云台设备（非DJI云台），然后将第三方云台设备挂载到DJI无人机上，通过PSDK开发接入的云台就可以像DJI官方云台一样，受遥控器或OSDK云台管理功能控制或使用。此功能组件提供了云台与飞机以及遥控APP端的通信接口，开发者使用此功能组件需要有云台及相关结构的开发能力。DJI官方也提供了一个标准的云台X-PORT，开发者使用X-PORT就无需再注册gimbal emu功能，可以大大缩减云台部分开发。有关X-PORT的使用请参考5.19 标准云台组件部分。

5.8.2 主要代码文件

imbal_emu/
├── test_payload_gimbal_emu.c
└── test_payload_gimbal_emu.h

PSDK lib头文件

dji_gimbal.h

5.8.3 sample代码实现

功能入口函数

#ifdef CONFIG_MODULE_SAMPLE_GIMBAL_ON
        if (aircraftInfoBaseInfo.djiAdapterType == DJI_SDK_ADAPTER_TYPE_SKYPORT_V2 ||
            aircraftInfoBaseInfo.djiAdapterType == DJI_SDK_ADAPTER_TYPE_NONE) {
            if (DjiTest_GimbalStartService() != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
                USER_LOG_ERROR("psdk gimbal init error");
            }
        }
#endif

说明：

  • PSDK 3.x版本支持的M300机型支持转接环Skyport v2和X-PORT，因为X-PORT不需要注册此模块，所以判断是skyport v2转接环后才启动该服务。

  • PSDK 3.x在M30上开发，不再支持skyport/X-PORT接入，但是云台组件功能还是支持的，也就是DJI_SDK_ADAPTER_TYPE_NONE时也会启动该服务。

初始化

包括订阅模块初始化和gimbal初始化

djiStat = DjiFcSubscription_Init();
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
    USER_LOG_ERROR("init data subscription module error.");
    return djiStat;
}
​
djiStat = DjiGimbal_Init();
if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
    USER_LOG_ERROR("init gimbal module error: 0x%08llX", djiStat);
    return djiStat;
}
​
/**
 * @brief Initialise gimbal module, and user should call this function before using gimbal features.
 * @return Execution result.
 */
T_DjiReturnCode DjiGimbal_Init(void);

说明：

  • 第三方的云台角度与飞机对齐时会涉及到飞机姿态角度获取以及计算，所以需要初始化订阅模块。

  • gimbal emu功能使用前需先初始化该模块。

注册gimbal emu功能回调函数句柄

/**
 * @brief Register common handler functions/callback functions of gimbal. After registering, callback functions will be
 * called automatically.
 * @param commonHandler: pointer to common handler functions of gimbal.
 * @return Execution result.
 */
T_DjiReturnCode DjiGimbal_RegCommonHandler(const T_DjiGimbalCommonHandler *commonHandler);

API简介

  • 注册云台组件的功能回调函数，注册到PSDK lib的回调对应与特定的云台功能物理实现，当遥控、APP或OSDK控制端触发云台控制时，此回调将会对应被触发。

参数

  • commonHandler，功能回调句柄

    /**
     * @brief Common handler function prototypes of gimbal, that is, some common callback function prototypes of gimbal.
     * @warning User can not execute blocking style operations or functions in callback function, because that will block DJI
     * root thread, causing problems such as slow system response, payload disconnection or infinite loop.
     */
    typedef struct {
        //获取云台系统状态，包括挂载位置，云台模式，微调角度，速度控制参数等
        T_DjiReturnCode (*GetSystemState)(T_DjiGimbalSystemState *systemState);
        //获取云台角度值，APP端会实时触发此函数来获取当前的云台角度并在APP上显示。PSDK端要将当前云台值通过此回调回传。
        T_DjiReturnCode (*GetAttitudeInformation)(T_DjiGimbalAttitudeInformation *attitudeInformation);
        //获取校准状态，物理云台的校准状态通过此API回传。
        T_DjiReturnCode (*GetCalibrationState)(T_DjiGimbalCalibrationState *calibrationState);
        //旋转云台，此回调在遥控、APP或OSDK发送旋转指令被触发，需要在此接口中实现云台的物理旋转
        T_DjiReturnCode (*Rotate)(E_DjiGimbalRotationMode rotationMode, T_DjiGimbalRotationProperty rotationProperty,
                                  T_DjiAttitude3d rotationValue);
        //开始校准，当APP端发起校准时会触发此回调
        T_DjiReturnCode (*StartCalibrate)(void);
        //设置平滑参数，设置值范围0~30，值越大，加速度越小。
        T_DjiReturnCode (*SetControllerSmoothFactor)(uint8_t smoothingFactor, E_DjiGimbalAxis axis);
        //设置使能PITCH限位角扩展
        T_DjiReturnCode (*SetPitchRangeExtensionEnabled)(bool enabledFlag);
        //设置云台转动的最大速度，参数值为百分比
        T_DjiReturnCode (*SetControllerMaxSpeedPercentage)(uint8_t maxSpeedPercentage, E_DjiGimbalAxis axis);
        //保存设置参数
        T_DjiReturnCode (*RestoreFactorySettings)(void);
        //设置云台模式，Free，FPV，YAW Follow
        T_DjiReturnCode (*SetMode)(E_DjiGimbalMode mode);
        //云台复位
        T_DjiReturnCode (*Reset)(E_DjiGimbalResetMode mode);
        //微调云台角度，单位0.1度
        T_DjiReturnCode (*FineTuneAngle)(T_DjiAttitude3d fineTuneAngle);
        //获取云台旋转速度，50HZ频率触发
        T_DjiReturnCode (*GetRotationSpeed)(T_DjiAttitude3d *rotationSpeed);
        //获取云台关节角，单位0.1度
        T_DjiReturnCode (*GetJointAngle)(T_DjiAttitude3d *jointAngle);
    } T_DjiGimbalCommonHandler;

开启gimbal task

if (osalHandler->TaskCreate("user_gimbal_task", UserGimbal_Task,
                            PAYLOAD_GIMBAL_EMU_TASK_STACK_SIZE, NULL, &s_userGimbalThread) !=
    DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
    USER_LOG_ERROR("user gimbal task create error");
    return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}

创建一个线程通过一个死循环来处理云台相关功能，包括云台角度与飞机姿态角度对齐，云台实时角度的计算，以及云台的旋转控制等。事实上，云台的功能部分都是围绕云台旋转控制实现，其中云台旋转角度控制就需要结合飞机的姿态角，以及转动的角度量进行计算。

  • 首先会订阅飞机的四元数，飞机四元数表示的是飞机的姿态角。具体订阅功能的使用可以查阅遥测数据订阅部分。

  • 然后会进入一个死循环，1000HZ的循环频率（sleep 1ms）。

  • 以50HZ的频率获取四元数并通过四元数计算欧拉角，计算的飞机姿态角会在下面的云台角度计算中用到。

    • 欧拉角计算方法（四元数计算出来的是弧度值，可转换成角度值）：

      /**
       * @brief
       * @param quaternion
       * @param attitude Unit: 0.1 degree.
       * @return
       */
      static T_DjiReturnCode DjiTest_GimbalCalculateGroundAttitudeBaseQuaternion(T_DjiFcSubscriptionQuaternion quaternion,
                                                                                 T_DjiAttitude3d *attitude)
      {
          double aircraftPitchInRad;
          double aircraftRollInRad;
          double aircraftYawInRad;
      ​
          if (attitude == NULL) {
              USER_LOG_ERROR("Input argument is null.");
              return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER;
          }
      ​
          aircraftPitchInRad = asin(2 * ((double) quaternion.q0 * quaternion.q2 - (double) quaternion.q3 * quaternion.q1));
          attitude->pitch = aircraftPitchInRad * 180 / DJI_PI * 10;
      ​
          aircraftRollInRad = atan2(2 * ((double) quaternion.q0 * quaternion.q1 + (double) quaternion.q2 * quaternion.q3),
                                    (double) 1 -
                                    2 * ((double) quaternion.q1 * quaternion.q1 + (double) quaternion.q2 * quaternion.q2));
          attitude->roll = aircraftRollInRad * 180 / DJI_PI * 10;
      ​
          aircraftYawInRad = atan2(2 * ((double) quaternion.q0 * quaternion.q3 + (double) quaternion.q1 * quaternion.q2),
                                   (double) 1 -
                                   2 * ((double) quaternion.q2 * quaternion.q2 + (double) quaternion.q3 * quaternion.q3));
          attitude->yaw = aircraftYawInRad * 180 / DJI_PI * 10;
      ​
          return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
      }

云台角度旋转sample解释说明

  1. 这部分变量较多且易混淆，我们先简单列举一下：

    全局变量

    s_attitudeInformation，云台的姿态角

    s_aircraftAttitude，飞机的姿态角

    s_lastAircraftAttitude，本次旋转前的飞机姿态角

    s_attitudeHighPrecision，云台高精度的姿态角（float型）

    s_targetAttitude，云台本次旋转之后的目标位置角度

    局部变量

    attitudeFTemp，用于校验角度值的临时角度参数，作为函数：DjiTest_GimbalAngleLegalization的输入参数。

    nextAttitude，执行旋转过程中基于本次循环计算出下一个循环的目标角度

  2. 不同模式下角度云台姿态角与飞机姿态角关系

    FREE

    自由模式，云台姿态角与飞机姿态角没有依赖关系。

    FPV

    第一视角模式，云台ROLL(横滚)、YAW(偏航)与飞机ROLL，YAW需要保持一致，即第一视角。

    YAW FOLLOW

    YAW跟随模式，云台YAW（偏航）与飞机YAW需要保持一致。

    说明：

    • 云台角度与飞机姿态角保持一致的计算方式，每次循环时都计算飞机姿态角的变化，然后云台角做对应的转动变化。例：

      s_attitudeInformation.attitude.roll += (s_aircraftAttitude.roll - s_lastAircraftAttitude.roll);

  3. 校验云台角度合法性，就是将云台姿态在飞机机体坐标系下完成是否达到限位角的校验。涉及欧拉角（NEU）与飞机姿态角度关系，以及关节角限位对云台角度的限制。其中关节角指的是云台关节（电机）可以支持转动的最大角度，欧拉角是云台姿态的一种表示方式，通过NEU下的欧拉角描述，欧拉角也可在关节角范围内的指定一个支持的姿态范围。

    看具体的代码实现：

    /**
     * @brief
     * @param attitude: in ground coordinate
     * @param aircraftAttitude: in ground coordinate
     * @param reachLimitFlag
     * @return
     */
    static T_DjiReturnCode DjiTest_GimbalAngleLegalization(T_DjiAttitude3f *attitude, T_DjiAttitude3d aircraftAttitude,
                                                           T_DjiGimbalReachLimitFlag *reachLimitFlag)

    • 使用云台的姿态角度减去飞机的姿态角度，即可描述机体坐标系下云台角范围。举例，比如云台pitch姿态角范围为30 ~ -90，飞机姿态角（NED）下pitch为30，此时云台姿态角范围（NEU）就是0 ~ -120。（pitch注意一个是NEU，一个NED）

    • 结合飞机姿态计算后的云台姿态角再与关节角取交集，便是云台姿态角旋转过程中的限位角度。

    • 然后将传入需要校验的云台姿态角attitude，减去飞机的姿态角与上述计算出来的限位角取交集，得到机体坐标系下的云台姿态角。

    • 校验后再将云台角度加上飞机姿态角恢复到NEU坐标系，就完成了姿态角的合法性校验。

  4. 旋转过程

    • 旋转角度理解，线程是以PAYLOAD_GIMBAL_TASK_FREQ频率循环，nextAttitude表示单次循环中旋转的角度，速度 * 时间 + 当前角度

    nextAttitude.pitch = (float) s_attitudeHighPrecision.pitch + (float) s_speed.pitch / (float) PAYLOAD_GIMBAL_TASK_FREQ;
    nextAttitude.roll = (float) s_attitudeHighPrecision.roll + (float) s_speed.roll / (float) PAYLOAD_GIMBAL_TASK_FREQ;
    nextAttitude.yaw = (float) s_attitudeHighPrecision.yaw + (float) s_speed.yaw / (float) PAYLOAD_GIMBAL_TASK_FREQ;
    ​

    • 如果是角度控制，通过下面的方式来判断是否已经到达目标角度。当前角度与目标角度的差值与旋转的方向（速度）判定：

    if (s_controlType == TEST_GIMBAL_CONTROL_TYPE_ANGLE) {
        nextAttitude.pitch =(nextAttitude.pitch - s_targetAttitude.pitch) * s_speed.pitch >= 0 ? s_targetAttitude.pitch:nextAttitude.pitch;
        nextAttitude.roll = (nextAttitude.roll - s_targetAttitude.roll) * s_speed.roll >= 0 ? s_targetAttitude.roll:nextAttitude.roll;
        nextAttitude.yaw =(nextAttitude.yaw - s_targetAttitude.yaw) * s_speed.yaw >= 0 ? s_targetAttitude.yaw : nextAttitude.yaw;
    }

    然后将本次循环结束后的云台角度赋给全局变量，s_attitudeInformation，这样APP端也就可以实时的获取云台角度。

5.8.4 开发注意事项

  1. X-PORT不需要注册gimbal_emu模块，可以看做是X-PORT中已经实现了gimbal_emu。

  2. 回调函数中不要使用延时过高或阻塞的功能或模块，回调阻塞可能会导致PSDK主线程卡死。

  3. gimbal task中提供的是云台旋转的模拟sample，本身没有涉及到物理云台的控制，开发自己的云台时需要将对应的角度与物理云台的控制关联起来。

  4. PSDK中的gimbal emu与camera emu功能实际上是独立的，但是目前Pilot APP上的负载相机和云台是相关联的，也就是如果不注册camera emu，仅注册gimbal emu在遥控器或Pilot上在一些情况下控制不到。比如挂载多个负载设备，因为没有注册camera emu功能，APP上无法切换到PSDK开发的负载上，也就控制不到这个云台。但是使用MSDK或OSDK（端口）来指定负载安装口就可以控制，与X-PORT使用一样。

相关文章

评论

1 条评论

日期 投票数
  • LeeFree

    对这段话：“使用云台的姿态角度减去飞机的姿态角度，即可描述机体坐标系下云台角范围。举例，比如云台pitch姿态角范围为30 ~ -90，飞机姿态角（NED）下pitch为30，此时云台姿态角范围（NEU）就是0 ~ -120。”

    如果云台和无人机偏航角相同时我表示认同，但如果两者偏航角相差180°呢？

    另外常用的坐标体系不是NED和ENU嘛？

    1
    评论操作 固定链接

请登录写评论。

close