大疆社区

标题: 【OSDK】sample查看--飞行控制（2） [打印本页]

作者: 懒猫吃鱼 时间: 2020-12-14
标题: 【OSDK】sample查看--飞行控制（2）
  接前一篇【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。

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

作者: djiuser_agY646d 时间: 2021-5-12
请问在室内需要gps吗

欢迎光临大疆社区 (https://bbs.dji.com/)