MAVLINK消息在Ardupilot中的接收和发送过程

---

由于现在网上很多的都是APM旧版本的解释，因此把自己的一些学习所得记录下来。截至写博客日期（2020/10/21），本文APM版本为4.0（针对各种车辆的）。

---

SCHED_TASK

关于MAVLINK消息在APM中的初始化过程，在之前的博文里面已经写过了：MAVLINK在Ardupilot中的初始化过程。建议先阅读一下，虽然写得不是很完整，但还是能理一下思路的，后面有时间的话会进一步更新修改。

打开Ardupilot源码项目，进入随机车辆类型的主目录文件下找到对应的主程序文件，如在ArduCopter路径下，Copter.cpp中，对应文件下始终实现了两个函数的任务调度器。

SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180),
SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_send,    400, 550),

又如在ArduSub文件夹下的ArduSub.cpp文件中，同样实现了

SCHED_TASK_CLASS(GCS,                 (GCS*)&sub._gcs,   update_receive,     400, 180),
SCHED_TASK_CLASS(GCS,                 (GCS*)&sub._gcs,   update_send,        400, 550),

这两个函数始终以400Hz的频率运行，在APM上进行MAVLINK消息的发送和接收。

update_receive()

这个函数实现在ardupilot/libraries/GCS_MAVLINK/GCS_Common.cpp文件中。在之前的博文里面我们说过，GCS类相当于是在APM中实现了一个“地面站”的功能，用于发送和接收消息（当然并不完全正确）。具体来看里面的内容：

void GCS::update_receive(void)
{
    for (uint8_t i=0; i<num_gcs(); i++) {
        chan(i)->update_receive();
    }
    // also update UART pass-thru, if enabled
    update_passthru();
}

虽然这个函数在之前已经介绍过了，这边还是再说一下。num_gcs()获取到总共的MAVLINK数据传输接口（或链路）。然后对于每一个通道执行更新接收操作update_receive()。这个函数才是实际上对于每一个通道接口真正执行接收MAVLINK消息的函数。

这部分代码有点长就不全部放上来了，实际上对于MAVLINK接收消息并且解包的过程之前也有博文讲过（重读Ardupilot中stabilize model+MAVLINK解包过程），但是前面的一些知识点还是比较零散的，这边还是再汇总一下。

还是挑重点说一下，首先是在原函数中找到该段

        // Try to get a new message
        if (mavlink_parse_char(chan, c, &msg, &status)) {
            hal.util->persistent_data.last_mavlink_msgid = msg.msgid;
            hal.util->perf_begin(_perf_packet);
            packetReceived(status, msg);
            hal.util->perf_end(_perf_packet);
            parsed_packet = true;
            gcs_alternative_active[chan] = false;
            alternative.last_mavlink_ms = now_ms;
            hal.util->persistent_data.last_mavlink_msgid = 0;
        }

其他的可以先不追究细节，重点找到里面的

packetReceived(status, msg);

进入该函数

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);
}

packetReceived() 实现了对于MAVLINK消息包的接收。前面一些内容大家看注释理解一下即可。我们只需要关注，packetReceived()函数在最后面调用了 handleMessage()这个函数。这个函数才是对于接收到的消息包进行真正处理的函数。

定位到该函数定义于库内的GCS.h文件中

virtual void        handleMessage(const mavlink_message_t &msg) = 0;

在GCS_MAVLINK类中实现为一个虚函数，因此我们必须在各个具体的车辆类型中找到对应派生类中的具体实现。

以ArduSub为例，在上层车辆文件夹中的GCS_Mavlink.h中实现了GCS_MAVLINK_Sub类对于GCS_MAVLINK消息的继承，并且在GCS_Mavlink.cpp文件中实现了对于handleMessage()的重写。

void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg)
{
    switch (msg.msgid) {

    case MAVLINK_MSG_ID_HEARTBEAT: {    // MAV ID: 0
        // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
        if (msg.sysid != sub.g.sysid_my_gcs) {
            break;
        }
        sub.failsafe.last_heartbeat_ms = AP_HAL::millis();
        break;
    }

	case ...
	...
}

可以看到在内部通过switch-case语句实现了根据具体的mavlink消息id进行不同的事件处理的功能（内容太长就不全部放上来了）。

关于update_receive()函数了解到这里就差不多了。

update_send()

重新回到ArduSub.cpp中，我们对于update_send（）进行介绍。

同样在ardupilot/libraries/GCS_MAVLINK/GCS_Common.cpp文件中

void GCS::update_send()
{
    if (!initialised_missionitemprotocol_objects) {
        initialised_missionitemprotocol_objects = true;
        // once-only initialisation of MissionItemProtocol objects:
        AP_Mission *mission = AP::mission();
        if (mission != nullptr) {
            _missionitemprotocol_waypoints = new MissionItemProtocol_Waypoints(*mission);
        }
        AP_Rally *rally = AP::rally();
        if (rally != nullptr) {
            _missionitemprotocol_rally = new MissionItemProtocol_Rally(*rally);
        }
    }
    if (_missionitemprotocol_waypoints != nullptr) {
        _missionitemprotocol_waypoints->update();
    }
    if (_missionitemprotocol_rally != nullptr) {
        _missionitemprotocol_rally->update();
    }
    for (uint8_t i=0; i<num_gcs(); i++) {
        chan(i)->update_send();
    }
    WITH_SEMAPHORE(_statustext_sem);
    service_statustext();
}

多余部分不再过多讲解，定位到chan(i)->update_send()函数，该语句实现对于每一个通道上的MAVLINK消息的发送。在同一文件下的 void GCS_MAVLINK::update_send()函数中（内容过长不放上来了），找到do_try_send_message(),其是最主要的功能部分实现。

这个函数会在合适的时间调用try_send_message()函数，这也是do_try_send_message()函数的最重要的功能。

try_send_message()函数内部同样使用了switch-case语句通过判断具体的MAVLINK消息id来实现对于不同APM系统信息的发送功能，并返回布尔类型的值，发送成功即返回true。

bool GCS_MAVLINK::try_send_message(const enum ap_message id)
{
    bool ret = true;

    switch(id) {

    case MSG_ATTITUDE:
        CHECK_PAYLOAD_SIZE(ATTITUDE);
        send_attitude();
        break;
    
    case …
    …
}

以MSG_ATTITUDE消息为例，该case语句中首先对PAYLOAD长度进行判断，长度合适返回true。然后调用具体的消息处理函数。

void GCS_MAVLINK::send_attitude() const
{
    const AP_AHRS &ahrs = AP::ahrs();
    const Vector3f omega = ahrs.get_gyro();
    mavlink_msg_attitude_send(
        chan,
        AP_HAL::millis(),
        ahrs.roll,
        ahrs.pitch,
        ahrs.yaw,
        omega.x,
        omega.y,
        omega.z);
}

这个函数（其他消息也类似）会在内部调用由消息头文件所提供的函数接口，用来实现MAVLINK消息的发送功能（不理解的同学看这边：QGC添加自定义组件和发送自定义MAVLINK消息，这篇博文的第二章）。

这个函数实现在对应消息的头文件中，通过将需要传输的信息生成buf或者packet（究竟是哪个就需要看协议是怎么定的啦，可以全局找一下protocol.h这个文件），然后通过_mav_finalize_message_chan_send()函数发送出去（怎么发送的就不用关心太多啦～）。

关于这部分内容，了解这么多应该就足够了。