APM源码和MAVLINK解析学习——重读stabilize
之前写的模式都是基于master版本的，这次重读stable版本的Sub-4.0，想着重新把这部分写一下。这次打算把MAVLINK包接收也稍微点一下。写的过程中难免会有错，如果发现错误请务必提出。
首先需要提到的一点是，在APM的源码中，具体的车辆类型文件夹如ArduCopter、Ardusub等都是作为上层存在的，它内部提供了与下层的连接，具体来说一些库中的类都会在车辆文件夹中进行声明并且得到面向于实际车辆类型的具体实现。
stabilize model
这次主要论述的版本是Sub-4.0，首先查看control_stabilize.cpp文件。主要分为init()和run()两部分。
init()
// stabilize_init - initialise stabilize controller
bool Sub::stabilize_init()
{
// set target altitude to zero for reporting
pos_control.set_alt_target(0);
if (prev_control_mode == ALT_HOLD) {
last_roll = ahrs.roll_sensor;
last_pitch = ahrs.pitch_sensor;
} else {
last_roll = 0;
last_pitch = 0;
}
last_yaw = ahrs.yaw_sensor;
last_input_ms = AP_HAL::millis();
return true;
}
为了方便说明我们在文章一开始所说的车辆文件夹下的文件是作为上层接口存在的意思。我们就以这段代码开头的pos_control.set_alt_target(0)为例进行解释。
首先水下潜艇车辆类型Sub的位置控制器pos_control对象是在Sub.h中的Sub类中进行了声明，而这个AC_PosControl_Sub则是实现在了libraries路径下。简单来说libraries提供了最基本的类和功能方便你在上层车辆文件夹内进行调用，然后根据具体需要使用一部分功能或者重写部分函数。
AC_PosControl_Sub pos_control;
这个AC_PosControl_Sub实现在ardupilot/libraries/AC_AttitudeControl/路径下，其是继承自AC_PosControl类的，在其内部实现了set_alt_target()方法如下
/// set_alt_target - set altitude target in cm above home
void set_alt_target(float alt_cm) {
_pos_target.z = alt_cm; }
...
// position controller internal variables
Vector3f _pos_target; // target location in cm from home
完成该语句之后，通过if-else判断语句，首先判断当前车辆是否处于定深状态，注意prev_control_mode会在set_model()方法中进行赋值。如果是的话，保存当前传感器值为上一次的roll和pitch角。如果不是的话，那么就将上次的roll、pitch角设定为0。注意ahrs的归属如下：
在Sub类中定义了ahrs为AP_AHRS_NavEKF类对象并对参数进行赋值
AP_AHRS_NavEKF ahrs{EKF2, EKF3, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
class AP_AHRS_NavEKF : public AP_AHRS_DCM {
public:
enum Flags {
FLAG_NONE = 0,
FLAG_ALWAYS_USE_EKF = 0x1,
};
// Constructor
AP_AHRS_NavEKF(NavEKF2 &_EKF2, NavEKF3 &_EKF3, Flags flags = FLAG_NONE);
...
如上，AP_AHRS_NavEKF继承自AP_AHRS_DCM
AP_AHRS_DCM继承自AP_AHRS，在AP_AHRS类中，定义有
// integer Euler angles (Degrees * 100)
int32_t roll_sensor;
int32_t pitch_sensor;
int32_t yaw_sensor;
在init()原函数中，最后再获取上一次的yaw角和时间。
run()
// stabilize_run - runs the main stabilize controller
// should be called at 100hz or more
void Sub::stabilize_run()
{
// if not armed set throttle to zero and exit immediately
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers();
last_roll = 0;
last_pitch = 0;
last_yaw = ahrs.yaw_sensor;
return;
}
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
handle_attitude();
// output pilot's throttle
attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt);
//control_in is range -1000-1000
//radio_in is raw pwm value
motors.set_forward(channel_forward->norm_input());
motors.set_lateral(channel_lateral->norm_input());
}
这部分与之前讲的master版本的大同小异（Ardusub源码解析学习（一）——Ardusub主程序）。重点在于中间这个handle_attitude()，他把master中的大部分功能都集中在了这里面。
handle_attitude()
这个handle_attitude()函数在control_althold.cpp中实现。
void Sub::handle_attitude()
{
uint32_t tnow = AP_HAL::millis();
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// get pilot desired lean angles
float target_roll, target_pitch, target_yaw;
// Check if set_attitude_target_no_gps is valid
if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {
Quaternion(
set_attitude_target_no_gps.packet.q
).to_euler(
target_roll,
target_pitch,
target_yaw
);
target_roll = 100 * degrees(target_roll);
target_pitch = 100 * degrees(target_pitch);
target_yaw = 100 * degrees(target_yaw);
last_roll = target_roll;
last_pitch = target_pitch;
last_yaw = target_yaw;
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, target_yaw, true);
} else {
// If we don't have a mavlink attitude target, we use the pilot's input instead
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (abs(target_roll) > 50 || abs(target_pitch) > 50) {
last_roll = ahrs.roll_sensor;
last_pitch = ahrs.pitch_sensor;
last_yaw = ahrs.yaw_sensor;
last_input_ms = tnow;
attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
} else if (abs(target_yaw) > 50) {
// if only yaw is being controlled, don't update pitch and roll
attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, target_yaw);
last_yaw = ahrs.yaw_sensor;
last_input_ms = tnow;
} else if (tnow < last_input_ms + 250) {
// just brake for a few mooments so we don't bounce
last_yaw = ahrs.yaw_sensor;
attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, 0);
} else {
// Lock attitude
attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_yaw, true);
}
}
}
这里挑部分说一下，首先看if-else语句部分，在开始之前大家再回去看看原来master里面对于stabilize_run()里面的控制是怎么样的，你会发现非常明显的对比。
该部分语句判断的是你是否从地面站接收mavlink消息进行控制，if语句里面实现解析mavlink消息包里面的内容从而完成控制，而else里面则是在如果在没有接收到mavlink消息包的情况下（消息包超时），进行操作员的手动控制。如果你仔细阅读，你会发现master里面的stabilize_run()中的控制内容实际上与else内部的内容一致。具体原因我们先看下去再说。
首先我们回到if语句的最开始部分，这里我们查看Sub类中定义的结构体如下：
// used to allow attitude and depth control without a position system
struct attitude_no_gps_struct {
uint32_t last_message_ms;
mavlink_set_attitude_target_t packet;
};
attitude_no_gps_struct set_attitude_target_no_gps {
0};
这个set_attitude_target_no_gps内部有两个成员，其中last_message_ms记录的是上一次接收到message的时间，而最主要的部分就是这个packet包。仔细查看if部分的程序，就会发现主要是对这个set_attitude_target_no_gps中的packet成员进行处理。
回推去查看mavlink_set_attitude_target_t，其中MAVPACKED()是宏定义打包结构，仔细查看内部结构，发现里面定义了如四元数等重要结构参数（仔细看type_mask的注释，后面会有需要）
MAVPACKED(
typedef struct __mavlink_set_attitude_target_t {
uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/
float q[4]; /*< Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0)*/
float body_roll_rate; /*< [rad/s] Body roll rate*/
float body_pitch_rate; /*< [rad/s] Body pitch rate*/
float body_yaw_rate; /*< [rad/s] Body yaw rate*/
float thrust; /*< Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust)*/
uint8_t target_system; /*< System ID*/
uint8_t target_component; /*< Component ID*/
uint8_t type_mask; /*< Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude*/
}) mavlink_set_attitude_target_t;
回到if语句中，我们将从packet包中得到的四元数q转换为欧拉角，然后获取期望的RPY角（此处需要乘以100笔者也还没弄清楚是为什么），并且将其保存为上一次的欧拉角的值。最后调用姿态控制器的input_euler_angle_roll_pitch_yaw()方法进行具体的控制（这个函数内部会把获取的欧拉角的值乘以0.01，或许这就是前面为什么乘以100的原因，可能是为了保证精度吧？有懂的大佬务必留言告知）。
MAVLINK消息包姿态信息传输过程
那么肯定会有好奇的同学想知道packet里面的四元数q到底是怎么得到的，或者准确来说set_attitude_target_no_gps这个对象是如何获取到对应的数据的呢？在开始之前，希望你对MAVLINK有一个基本的认知：MAVLink Developer Guide
后面有时间的话我也想稍微写一些关于MAVLINK方面的文章
handleMessage
我们追根溯源到ardupilot/Ardusub/GCS_Mavlink.cpp中，这里面实现了一些针对于Sub车辆类型的mavlink消息包的处理函数，当然其他车辆类型也是类似。定位到handleMessage()这个函数，这是根据具体的message的id编号来进行对应的消息处理。
对于Attitude的mavlink消息包为MAVLINK_MSG_ID_SET_ATTITUDE_TARGET，对应id=82。根据switch进入到实际处理用的程序段。
注意到了吗，开头就是定义了mavlink_set_attitude_target_t类型的packet用来接收message，其中mavlink_msg_set_attitude_target_decode()用来将msg中的信息解码到packet，这个函数很重要，我们放到后面详细来讲。
void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg)
{
switch (msg.msgid) {
...
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: {
// MAV ID: 82
// decode packet
mavlink_set_attitude_target_t packet;
mavlink_msg_set_attitude_target_decode(&msg, &packet);
// ensure type_mask specifies to use attitude
// the thrust can be used from the altitude hold
if (packet.type_mask & (1<<6)) {
sub.set_attitude_target_no_gps = {
AP_HAL::millis(), packet};
}
// ensure type_mask specifies to use attitude and thrust
if ((packet.type_mask & ((1<<7)|(1<<6))) != 0) {
break;
}
// convert thrust to climb rate
packet.thrust = constrain_float(packet.thrust, 0.0f, 1.0f);
float climb_rate_cms = 0.0f;
if (is_equal(packet.thrust, 0.5f)) {
climb_rate_cms = 0.0f;
} else if (packet.thrust > 0.5f) {
// climb at up to WPNAV_SPEED_UP
climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * sub.wp_nav.get_default_speed_up();
} else {
// descend at up to WPNAV_SPEED_DN
climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * fabsf(sub.wp_nav.get_default_speed_down());
}
sub.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), climb_rate_cms);
break;
}
...
}
处理步骤如下：
- 将数据包msg中的信息解码到packet中。
- 判断packet中的type_mask位（这个在mavlink_set_attitude_target_t结构中有让大家仔细记住，想不起来了的话我把它放在下面）。
uint8_t type_mask; /< Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude/
- 如果type_mask中的第7位throttle位置1了，那么我们获取当前时间和packet保存到set_attitude_target_no_gps中，这回答了我们前面关于set_attitude_target_no_gps这个对象是如何获取到对应的数据的问题。
- 如果type_mask中的第7位throttle位或第八位attitude位不为0，那么直接推出当前程序（说实话没咋看懂）。
- 对packet中的thrust控制量进行限幅，然后对值进行判断。
- 如果thrust控制量=0.5，则表明控制信号处于中位，爬升速率为0；
- 如果thrust>0.5，则表明上浮，sub.wp_nav.get_default_speed_up()获取到的爬升速率默认参数表中定义为250cm/s；同理<0.5时的计算，此时使用fabs()是因为前面的(packet.thrust - 0.5f)已经为负。
- 最后调用guided_set_angle()方法设置引导模式角度目标，函数如下。
// set guided mode angle target
void Sub::guided_set_angle(const Quaternion &q, float climb_rate_cms)
{
// check we are in velocity control mode
if (guided_mode != Guided_Angle) {
guided_angle_control_start();
}
// convert quaternion to euler angles
q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd);
guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f;
guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f;
guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f);
guided_angle_state.climb_rate_cms = climb_rate_cms;
guided_angle_state.update_time_ms = AP_HAL::millis();
}
mavlink_msg_set_attitude_target_decode
解析完上面的handleMessage()函数之后，我们进入这个decode函数内部来看看实际解码过程。
还是很简单的哈（注意根据宏定义，#if里面的代码不执行，执行的是#else下面的程序），主要就是将msg内容拷贝进set_attitude_target里面，这里就不多说了，大家自己看看吧。
/**
* @brief Decode a set_attitude_target message into a struct
*
* @param msg The message to decode
* @param set_attitude_target C-struct to decode the message contents into
*/
static inline void mavlink_msg_set_attitude_target_decode(const mavlink_message_t* msg, mavlink_set_attitude_target_t* set_attitude_target)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
set_attitude_target->time_boot_ms = mavlink_msg_set_attitude_target_get_time_boot_ms(msg);
mavlink_msg_set_attitude_target_get_q(msg, set_attitude_target->q);
set_attitude_target->body_roll_rate = mavlink_msg_set_attitude_target_get_body_roll_rate(msg);
set_attitude_target->body_pitch_rate = mavlink_msg_set_attitude_target_get_body_pitch_rate(msg);
set_attitude_target->body_yaw_rate = mavlink_msg_set_attitude_target_get_body_yaw_rate(msg);
set_attitude_target->thrust = mavlink_msg_set_attitude_target_get_thrust(msg);
set_attitude_target->target_system = mavlink_msg_set_attitude_target_get_target_system(msg);
set_attitude_target->target_component = mavlink_msg_set_attitude_target_get_target_component(msg);
set_attitude_target->type_mask = mavlink_msg_set_attitude_target_get_type_mask(msg);
#else
uint8_t len = msg->len < MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN? msg->len : MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN;
memset(set_attitude_target, 0, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN);
memcpy(set_attitude_target, _MAV_PAYLOAD(msg), len);
#endif
}
再往上一步呢？
在ardupilot/libraries/GCS_MAVLink/GCS_Common.cpp中的packetReceived()调用了handleMessage()函数。代码如下：
void GCS_MAVLINK::packetReceived(const mavlink_status_t &status,
const mavlink_message_t &msg)
{
// we exclude radio packets because we historically used this to
// make it possible to use the CLI over the radio
if (msg.msgid != MAVLINK_MSG_ID_RADIO && msg.msgid != MAVLINK_MSG_ID_RADIO_STATUS) {
const uint8_t mask = (1U<<(chan-MAVLINK_COMM_0));
if (!(mask & mavlink_private)) {
mavlink_active |= mask;
}
}
if (!(status.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) &&
(status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) &&
AP::serialmanager().get_mavlink_protocol(chan) == AP_SerialManager::SerialProtocol_MAVLink2) {
// if we receive any MAVLink2 packets on a connection
// currently sending MAVLink1 then switch to sending
// MAVLink2
mavlink_status_t *cstatus = mavlink_get_channel_status(chan);
if (cstatus != nullptr) {
cstatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
}
}
if (!routing.check_and_forward(chan, msg)) {
// the routing code has indicated we should not handle this packet locally
return;
}
if (!accept_packet(status, msg)) {
// e.g. enforce-sysid says we shouldn't look at this packet
return;
}
handleMessage(msg);
}
然后，在我们继续往前推的时候，在同一路径文件下发现GCS_MAVLINK::update_receive(uint32_t max_time_us)调用packetReceived()这个函数，这边由于太长了就不放进来了，大家自己去看看吧。
再再往前呢？终于到头了，我们回到了最初的车辆文件夹下，而且是主文件下！以Ardusub为例，我们回到了Ardusub.cpp中，找到的对应位置为
SCHED_TASK_CLASS(GCS, (GCS*)&sub._gcs, update_receive, 400, 180),
没错，就在任务列表中！这个函数以400Hz的频率运行，超时时间为180ms。至此，我们推完了整个姿态消息解包的过程～
这边再帮大家把思路理一下：
- 在上层车辆文件夹下，我们使用任务调度器，调用了update_receive()这个函数。
- update_receive()这个函数会调用packetReceived()用来接收mavlink消息包。
- packetReceived()中调用了handleMessage()函数处理包中的内容（如获取四元数保存之类的）。
有时间的话，这部分再更新一点内容，把stable版本的althold model这部分也加进来。再把MAVLINK这部分消息解包过程再令写一篇博文介绍一下，疯狂挖坑（逃…）