OSDK 示例代码解析--飞行控制

关键词：OSDK飞行控制

本文再接着聊聊DJI Onboard SDK基础飞行控制功能sample中飞行控制（djiosdk-flightcontrol-sample：b）
  前面起飞降落比较简单，罗列的信息也不多，本文代码查看思路与之前的基本一致，这篇也将不再重复码字。

  直接找主题，main (b选项) --> moveByPositionOffset  --> positionAndYawCtrl

  函数贴出来（OSDK4.0.1版本，貌似因为OSDK4.x仅支持M210 v2和M300机型，这些API统一修改了支持的平台，3.x的版本也是支持该API的）：

/*! @Brief Control the position and yaw angle of the vehicle.
   *  The reference frame is the DJI::OSDK::Control::HORIZONTAL_GROUND (NEU).
   *
   *  @platforms M210V2, M300
   *  @param x position set-point in x axis of ground frame (m)
   *  @param y position set-point in y axis of ground frame (m)
   *  @param z position set-point in z axis of ground frame (m), input limit see
   * DJI::OSDK::Control::VERTICAL_POSITION
   *  @param yaw yaw set-point (deg)
   *
   */
  void positionAndYawCtrl(float32_t x, float32_t y, float32_t z, float32_t yaw);

先简单查看一下该API，其是通过位移控制指令来控制飞机进行移动和飞行的控制接口。给的参数分别为x,y,z和YAW角度参数，参数说明是目标点位置。实际上这个参数无法直接指定目标点，给的仅是位移控制有效指令，具体在跟读应用端代码实现时详细介绍下。先有个初步概念，该参数并不是指定目标点位移值。

函数实现：

void Control::positionAndYawCtrl(float32_t x, float32_t y, float32_t z,
                                                                float32_t yaw)
    {
        //! @Note 145 is the flag value of this mode
        uint8_t ctrl_flag = (VERTICAL_POSITION | HORIZONTAL_POSITION | YAW_ANGLE |
                                                   HORIZONTAL_GROUND | STABLE_ENABLE);
        CtrlData data(ctrl_flag, x, y, z, yaw);

        this->flightCtrl(data);
    }

代码不多，分别为设置flag ,整合参数，通过API：flightCtrl实现控制。似乎没这么简单，继续查看代码：flightCtrl也基本就是调用协议层发送指令了。我们再看看参数：
dji_control.hpp文件中，说的比较详细（本文中都是从代码截取出来的）

   /*! @brief Control the vehicle using user-specified mode
   *
   *  @platforms M210V2, M300
   *  @param data control set-points and flags
   *
   * @details Control Mode Byte
   *
   *  bit 7:6                  | bit 5:4           | bit 3          | bit 2:1            | bit 0                |
   *  ----------------         | ----------------- | -------------- | -----------------  | -----------------    |
   *  0b00: HORI_ATTI_TILT_ANG | 0b00: VERT_VEL    | 0b00: YAW_ANG  | 0b00: ground frame | 0b0: non-stable mode |
   *  0b01: HORI_VEL           | 0b01: VERT_POS    | 0b01: YAW_RATE | 0b01: body frame   | 0b1: stable mode     |
   *  0b10: HORI_POS           | 0b10: VERT_THRUST |                | (horizontal frame) |                      |
   *  0b11: HORI_ANG_VEL       |                   |                |                    |                      |
   *
   * @details Command Value
   *
   * ctrl_flag | ctrl_byte | roll_or_x | pitch_or_y | thr_z   | yaw     | feedforward_x | feedforward_y
   * --------  | --------  | --------- | ---------- | ------- | ------- | -----------   | -------------
   * uint8_t   | uint8_t   | float32   | float32    | float32 | float32 | float32       | float32
   *
   *
   * | Mode        | Input Limit |
   * |-------------|-------------|
   * | vert_thrust | 0 to 100 %  |
   * | vert_vel    | -5 to 5 m/s |
   * | vert_pos    | 0 to 120m   |
   * | hori_ang    | 35 degrees  |
   * | hori_vel    | 30 m/s      |
   * | hori_acc    | 7 m/s^2     |
   * | yaw_rate    | 150 deg/s   |
   *
   */
  void flightCtrl(CtrlData data);

  先简单理解一下，CtrlData构成分为flag, x,y,z,yaw, 其中x,y,z,yaw分别对应上三轴向和yaw参数控制量，但是具体含义会因flag设置不一样而表示不一样的控制量。
  再具体看看flag：
  继续看说明，flag是一个8bits变量，按位来设置水平面控制逻辑，垂直控制逻辑，yaw控制逻辑，水平方向坐标系

uint8_t flag;  /*!< control data flag consists of 8 bits.

                      - CtrlData.flag = ( DJI::OSDK::Control::HorizontalLogic |
                      DJI::OSDK::Control::VerticalLogic |
                      DJI::OSDK::Control::YawLogic |
                      DJI::OSDK::Control::HorizontalCoordinate |
                      DJI::OSDK::Control::StableMode)

配合代码注释，可以慢慢的理解相关控制API，下面再简单罗列一下具体的参数
  bit 7:6 HorizontalLogic：水平控制逻辑
  HORIZONTAL_ANGLE    角度
  HORIZONTAL_VELOCITY  速度
  HORIZONTAL_POSITION  位置
  HORIZONTAL_ANGULAR_RATE  角速度

  bit 5:4 VerticalLogic：垂直方向控制逻辑
  VERTICAL_VELOCITY 速度
  VERTICAL_POSITION 位置
  VERTICAL_THRUST    油门

  bit 3 YawLogic  YAW控制逻辑
  YAW_ANGLE Yaw角度                （基于Ground frame）
  YAW_RATE    Yaw角速度

  bit 2:1 HorizontalCoordinate 参数坐标系设置
  HORIZONTAL_GROUND       大地坐标系NEU(北东天)
  HORIZONTAL_BODY          机体坐标系FRU（前右上）

  bit 0 StableMode： stable mode使能设置

  再参考API：positionAndYawCtrl flag设置，使用起来貌似没有那么难，直接使用位运算就可以设置参数（可以再回去看看枚举定义值）
  uint8_t ctrl_flag = (VERTICAL_POSITION | HORIZONTAL_POSITION | YAW_ANGLE | HORIZONTAL_GROUND | STABLE_ENABLE);
  即大地坐标系参数x/y/z方向上位置控制移动，YAW角度控制。对应x/y/z参数意义也就出来了。如果还比较模糊，继续看官方给出的API：

  结合参数说明，这个代码注解还是比较详细的了。要还是看不懂，那就上飞机直接修改参数飞一飞。

  /*! @brief Control the velocity and yaw rate of the vehicle.
   *  The reference frame is the DJI::OSDK::Control::HORIZONTAL_GROUND (NEU).
   *
   *  @platforms M210V2, M300
   *  @param Vx velocity set-point in x axis of ground frame (m/s), input limit
   * see DJI::OSDK::Control::HORIZONTAL_VELOCITY
   *  @param Vy velocity set-point in y axis of ground frame (m/s), input limit
   * see DJI::OSDK::Control::HORIZONTAL_VELOCITY
   *  @param Vz velocity set-point in z axis of ground frame (m/s), input limit
   * see DJI::OSDK::Control::VERTICAL_VELOCITY
   *  @param yawRate yawRate set-point (deg/s)
   */
  void velocityAndYawRateCtrl(float32_t Vx, float32_t Vy, float32_t Vz, float32_t yawRate);

  /*! @brief Control the attitude and vertical position of the vehicle
   *
   *  @platforms M210V2, M300
   *  @param roll   attitude set-point in x axis of body frame (FRU) (deg),
   * input limit see DJI::OSDK::Control::HORIZONTAL_ANGLE
   *  @param pitch  attitude set-point in y axis of body frame (FRU) (deg),
   * input limit see DJI::OSDK::Control::HORIZONTAL_ANGLE
   *  @param z      z position set-point in z axis of ground frame (NED) (m),
   * input limit see DJI::OSDK::Control::VERTICAL_POSITION
   *  @param yaw    attitude set-point in z axis of ground frame (NED) (deg)
   */
  void attitudeAndVertPosCtrl(float32_t roll, float32_t pitch, float32_t yaw, float32_t z);

  /*! @brief Control the attitude rate and vertical position of the vehicle
   *
   *  @platforms M210V2, M300
   *  @param rollRate   attitude rate set-point in x axis of body frame (FRU)
   * (deg/s)
   *  @param pitchRate  attitude rate set-point in y axis of body frame (FRU)
   * (deg/s)
   *  @param yawRate    attitude rate set-point in z axis of body frame (FRU)
   * (deg/s), input limit see DJI::OSDK::Control::YAW_RATE
   *  @param z          z position set-point in z axis of ground frame (NED)
   * (m), input limit see DJI::OSDK::Control::VERTICAL_POSITION
   */
  void angularRateAndVertPosCtrl(float32_t rollRate, float32_t pitchRate, float32_t yawRate, float32_t z);

  当然除了上述组合，还可以根据自己的需要来自定义设置，SDK demo就是用来了解SDK API特性再为我们所用的。当然，官方没有推荐的方式，还是需要多飞多试，不然。。。

  至此，飞行控制的API基本已经过了一遍，老套路回到sample查看官方使用demo。sample API便是moveByPositionOffset，这个函数实现比起起飞和降落稍微复杂一下，简单写写读代码后的理解。
  先大致浏览一下函数内代码，（解读官方demo外加本人比较业余，就不用专业的流程图来说明了，简单用文字描述，仅供参考。）
  1、定义局部变量，名字命名可以查看到点信息，可以在后面具体使用上再进一步确认
  2、订阅遥测数据，TOPIC: TOPIC_QUATERNION, TOPIC_GPS_FUSED
  3、通过GPS获取到的位置及位置控制参数计算位移（根据输入位置参数计算剩余飞行距离）
  4、while循环发送位置控制指令，并实时获取GPS位置信息来变更传入参数（小于speedFactor参数时变更，否则一直传speedFactor）。
  5、判断剩余距离及设定的阈值，如果在阈值范围内，飞行结束。

  提取一下，
  1、调用位置控制API：
   vehicle->control->positionAndYawCtrl(xCmd, yCmd, zCmd, yawDesiredRad / DEG2RAD);
是在while循环中以一定的频率进行发送的，频率控制参数也就出来了：controlFreqInHz，cycleTimeInMs

  2、while条件：elapsedTimeInMs < timeoutInMilSec，发送控制受timeout时间限制：timeoutInMilSec，而且timeoutInMilSec参数初始化给了一个固定值，所以可能会影响发送指令。

  3、位置参数是通过外部计算后动态传给飞控的。

  理解一下，
  该sample实现与我们理解的控制方式貌似不一样，初次理解应该是外部将位移参数传给飞控，飞控直接去控制飞行就可以了，现在看来是由应用端做实时控制，且位移参数也是实时传入的。
  理解不了就咨询官方，官方说明是OSDK3.x版本与飞机间没有心跳检测机制，如果在控制飞行过程中，出现OSDK与飞机异常断开，此时飞行就处于不可控状态，存在安全问题（OSDK4.x貌似已经接入心跳机制，看OSDK飞行控制功能还在更新中，有空再研究研究更新的功能是不是有对这部分进行优化）。根据这个解释，实测一下将while拿掉，飞机将不会持续飞行。

  总结一下,
  1、外部调用飞行控制API需要以一定频率发送。（类推速度控制、姿态控制）
  2、因为以一定的频率发送位置，如果一直发送固定的位置信息，飞控将一直刷新位置参数，在条件允许的情况下，设定的这个位置可能就一直在更新
   所以飞机位置参数实际控制在于应用端的实时控制，sample提供了一个方案，知道上述原理之后，可以根据自己需要来设计。
   这个功能对于速度控制，姿态控制来说，其实外部应用控制参数更为灵活了。
  3、Z轴高度参数始终传入的是相对起飞点高度，无需动态计算传入。
  4、YAW角度是基于NED的角度值，OSDK获取飞机姿态角需要使用四元数计算，toEulerAngle。

  本文基本列出了这个功能中的比较重点的部分，后面的功能有空继续。。。

此组别内的文章

评论

此组别内的文章

相关文章