接前一篇【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。
本文基本列出了这个功能中的比较重点的部分,后面的功能有空继续。。。
|