大疆社区

标题: 【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/) Powered by Discuz! X3.2