前言

APM 4.2.3
以ROVER为例进行阅读
如有疏漏或谬误，恳请指出
一个人可以走的更快，一群人才能走的更远
可加文章底部个人微信

一、update_receive

APM的MAVLINK包含收和发两个任务进行调度，本节先对接收部分进行讲解
如下，这里的rover._gcs是一个GCS_Rover对象，被转换为其父类GCS的指针，父类指针指向子类对象，实现多态

update_receive函数的定义如下：这里的chan是指针数组，里面相当于存放了不同的mavlink通道，例如和地面站通信的通道还有和机载电脑通信的通道。

前面的父类指针指向的是子类GCS_Rover的对象，所以这里的chan数组也是GCS_Rover中定义的，如下：

GCS_MAVLINK_Rover是GCS_MAVLINK的子类，其继承了GCS_MAVLINK的update_receive方法，因此chan(i)->update_receive();是调用了GCS_MAVLINK的update_receive方法，如下：
这个方法的作用就是读取通信端口的数据，并调用MAVLINK库函数的解包函数mavlink_parse_char进行解包，一旦一包数据解析完成，调用packetReceived来完成具体的数据处理逻辑

void
GCS_MAVLINK::update_receive(uint32_t max_time_us)
{
    
    
    // do absolutely nothing if we are locked
    if (locked()) {
    
    
        return;
    }

    // receive new packets
    mavlink_message_t msg;
    mavlink_status_t status;
    uint32_t tstart_us = AP_HAL::micros();
    uint32_t now_ms = AP_HAL::millis();

    status.packet_rx_drop_count = 0;

    const uint16_t nbytes = _port->available();
    for (uint16_t i=0; i<nbytes; i++)
    {
    
    
        const uint8_t c = (uint8_t)_port->read();
        const uint32_t protocol_timeout = 4000;
        
        if (alternative.handler &&
            now_ms - alternative.last_mavlink_ms > protocol_timeout) {
    
    
            /*
              we have an alternative protocol handler installed and we
              haven't parsed a MAVLink packet for 4 seconds. Try
              parsing using alternative handler
             */
            if (alternative.handler(c, mavlink_comm_port[chan])) {
    
    
                alternative.last_alternate_ms = now_ms;
                gcs_alternative_active[chan] = true;
            }
            
            /*
              we may also try parsing as MAVLink if we haven't had a
              successful parse on the alternative protocol for 4s
             */
            if (now_ms - alternative.last_alternate_ms <= protocol_timeout) {
    
    
                continue;
            }
        }

        bool parsed_packet = false;

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

        if (parsed_packet || i % 100 == 0) {
    
    
            // make sure we don't spend too much time parsing mavlink messages
            if (AP_HAL::micros() - tstart_us > max_time_us) {
    
    
                break;
            }
        }
    }

    const uint32_t tnow = AP_HAL::millis();

    // send a timesync message every 10 seconds; this is for data
    // collection purposes
    if (tnow - _timesync_request.last_sent_ms > _timesync_request.interval_ms && !is_private()) {
    
    
        if (HAVE_PAYLOAD_SPACE(chan, TIMESYNC)) {
    
    
            send_timesync();
            _timesync_request.last_sent_ms = tnow;
        }
    }

    // consider logging mavlink stats:
    if (is_active() || is_streaming()) {
    
    
        if (tnow - last_mavlink_stats_logged > 1000) {
    
    
            log_mavlink_stats();
            last_mavlink_stats_logged = tnow;
        }
    }
}

packetReceived如下，主要对一些特殊的消息进行了处理，同时检查MAVLINk协议版本，如果收到MAVLINk2的数据，则将发送的数据也切成MAVLINk2协议。主要的数据处理逻辑在handleMessage(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) {
    
    
        mavlink_active |= (1U<<(chan-MAVLINK_COMM_0));
    }
    const auto mavlink_protocol = uartstate->get_protocol();
    if (!(status.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) &&
        (status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) &&
        (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2 ||
         mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLinkHL)) {
    
    
        // 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 (msg.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {
    
    
#if HAL_MOUNT_ENABLED
        // allow mounts to see the location of other vehicles
        handle_mount_message(msg);
#endif
    }
    if (!accept_packet(status, msg)) {
    
    
        // e.g. enforce-sysid says we shouldn't look at this packet
        return;
    }
    handleMessage(msg);
}

二、handleMessage

handleMessage是GCS_MAVLINK的一个纯虚函数，在其子类GCS_MAVLINK_Rover中进行了覆盖，其定义如下：
在handleMessage首先对控制接口进行了处理，这些了控制接口主要是在guided模式下使用，一般是由机载电脑发送给飞控，在使用这些接口时需要注意掩码type_mask的设置。同时对电台消息也进行了处理。最后 通过handle_common_message(msg);对普通的消息进行处理。

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

    case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
        handle_set_attitude_target(msg);
        break;

    case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
        handle_set_position_target_local_ned(msg);
        break;

    case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
        handle_set_position_target_global_int(msg);
        break;

    case MAVLINK_MSG_ID_RADIO:
    case MAVLINK_MSG_ID_RADIO_STATUS:
        handle_radio(msg);
        break;

    default:
        handle_common_message(msg);
        break;
    }
}

三、handle_common_message

除了上面的几个消息，剩下的所有的消息都在这个里面处理，每个消息（或几个消息）对应一个handle处理函数。到这个函数为止，MAVLINK的接收处理就完成了。

void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg)
{
    
    
    switch (msg.msgid) {
    
    

    case MAVLINK_MSG_ID_HEARTBEAT: {
    
    
        handle_heartbeat(msg);
        break;
    }

    case MAVLINK_MSG_ID_COMMAND_ACK: {
    
    
        handle_command_ack(msg);
        break;
    }

    case MAVLINK_MSG_ID_SETUP_SIGNING:
        handle_setup_signing(msg);
        break;

    case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
    case MAVLINK_MSG_ID_PARAM_SET:
    case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
        handle_common_param_message(msg);
        break;

    case MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN:
        handle_set_gps_global_origin(msg);
        break;

    case MAVLINK_MSG_ID_DEVICE_OP_READ:
        handle_device_op_read(msg);
        break;
    case MAVLINK_MSG_ID_DEVICE_OP_WRITE:
        handle_device_op_write(msg);
        break;
    case MAVLINK_MSG_ID_TIMESYNC:
        handle_timesync(msg);
        break;
    case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
    case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
    case MAVLINK_MSG_ID_LOG_ERASE:
    case MAVLINK_MSG_ID_LOG_REQUEST_END:
    case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS:
        AP::logger().handle_mavlink_msg(*this, msg);
        break;

    case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
        handle_file_transfer_protocol(msg);
        break;

#if AP_CAMERA_ENABLED
    case MAVLINK_MSG_ID_DIGICAM_CONTROL:
    case MAVLINK_MSG_ID_GOPRO_HEARTBEAT: // heartbeat from a GoPro in Solo gimbal
        {
    
    
            AP_Camera *camera = AP::camera();
            if (camera == nullptr) {
    
    
                return;
            }
            camera->handle_message(chan, msg);
        }
        break;
#endif

    case MAVLINK_MSG_ID_SET_MODE:
        handle_set_mode(msg);
        break;

    case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST:
        handle_send_autopilot_version(msg);
        break;

    case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
    case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
    case MAVLINK_MSG_ID_MISSION_COUNT:
    case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
    case MAVLINK_MSG_ID_MISSION_ITEM:
    case MAVLINK_MSG_ID_MISSION_ITEM_INT:
    case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
    case MAVLINK_MSG_ID_MISSION_REQUEST:
    case MAVLINK_MSG_ID_MISSION_ACK:
    case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
        handle_common_mission_message(msg);
        break;

    case MAVLINK_MSG_ID_COMMAND_LONG:
        handle_command_long(msg);
        break;

    case MAVLINK_MSG_ID_COMMAND_INT:
        handle_command_int(msg);
        break;

    case MAVLINK_MSG_ID_FENCE_POINT:
    case MAVLINK_MSG_ID_FENCE_FETCH_POINT:
        handle_fence_message(msg);
        break;

#if HAL_MOUNT_ENABLED
    case MAVLINK_MSG_ID_MOUNT_CONFIGURE: // deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE
        send_received_message_deprecation_warning("MOUNT_CONFIGURE");
        handle_mount_message(msg);
        break;
    case MAVLINK_MSG_ID_MOUNT_CONTROL: // deprecated. Use MAV_CMD_DO_MOUNT_CONTROL
        send_received_message_deprecation_warning("MOUNT_CONTROL");
        handle_mount_message(msg);
        break;
    case MAVLINK_MSG_ID_GIMBAL_REPORT:
    case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION:
    case MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS:
        handle_mount_message(msg);
        break;
#endif

    case MAVLINK_MSG_ID_PARAM_VALUE:
        handle_param_value(msg);
        break;

    case MAVLINK_MSG_ID_SERIAL_CONTROL:
        handle_serial_control(msg);
        break;

    case MAVLINK_MSG_ID_GPS_RTCM_DATA:
    case MAVLINK_MSG_ID_GPS_INPUT:
    case MAVLINK_MSG_ID_HIL_GPS:
    case MAVLINK_MSG_ID_GPS_INJECT_DATA:
        AP::gps().handle_msg(msg);
        break;

    case MAVLINK_MSG_ID_STATUSTEXT:
        handle_statustext(msg);
        break;

#if AP_NOTIFY_MAVLINK_LED_CONTROL_SUPPORT_ENABLED
    case MAVLINK_MSG_ID_LED_CONTROL:
        // send message to Notify
        AP_Notify::handle_led_control(msg);
        break;
#endif

    case MAVLINK_MSG_ID_MANUAL_CONTROL:
        handle_manual_control(msg);
        break;

#if AP_NOTIFY_MAVLINK_PLAY_TUNE_SUPPORT_ENABLED
    case MAVLINK_MSG_ID_PLAY_TUNE:
        // send message to Notify
        AP_Notify::handle_play_tune(msg);
        break;
#endif

#if HAL_RALLY_ENABLED
    case MAVLINK_MSG_ID_RALLY_POINT:
    case MAVLINK_MSG_ID_RALLY_FETCH_POINT:
        handle_common_rally_message(msg);
        break;
#endif

    case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
        // only pass if override is not selected 
        if (!(_port->get_options() & _port->OPTION_NOSTREAMOVERRIDE)) {
    
    
            handle_request_data_stream(msg);
        }
        break;

    case MAVLINK_MSG_ID_DATA96:
        handle_data_packet(msg);
        break;        

#if HAL_VISUALODOM_ENABLED
    case MAVLINK_MSG_ID_VISION_POSITION_DELTA:
        handle_vision_position_delta(msg);
        break;

    case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
        handle_vision_position_estimate(msg);
        break;

    case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE:
        handle_global_vision_position_estimate(msg);
        break;

    case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE:
        handle_vicon_position_estimate(msg);
        break;

    case MAVLINK_MSG_ID_ODOMETRY:
        handle_odometry(msg);
        break;

    case MAVLINK_MSG_ID_ATT_POS_MOCAP:
        handle_att_pos_mocap(msg);
        break;

    case MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE:
        handle_vision_speed_estimate(msg);
        break;
#endif  // HAL_VISUALODOM_ENABLED

    case MAVLINK_MSG_ID_SYSTEM_TIME:
        handle_system_time_message(msg);
        break;

    case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
        handle_rc_channels_override(msg);
        break;

#if AP_OPTICALFLOW_ENABLED
    case MAVLINK_MSG_ID_OPTICAL_FLOW:
        handle_optical_flow(msg);
        break;
#endif

    case MAVLINK_MSG_ID_DISTANCE_SENSOR:
        handle_distance_sensor(msg);
        break;

    case MAVLINK_MSG_ID_OBSTACLE_DISTANCE:
        handle_obstacle_distance(msg);
        break;
    
    case MAVLINK_MSG_ID_OBSTACLE_DISTANCE_3D:
        handle_obstacle_distance_3d(msg);
        break;

    case MAVLINK_MSG_ID_OSD_PARAM_CONFIG:
    case MAVLINK_MSG_ID_OSD_PARAM_SHOW_CONFIG:
        handle_osd_param_config(msg);
        break;

    case MAVLINK_MSG_ID_ADSB_VEHICLE:
    case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG:
    case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC:
    case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT:
    case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CONTROL:
        handle_adsb_message(msg);
        break;

    case MAVLINK_MSG_ID_LANDING_TARGET:
        handle_landing_target(msg);
        break;

    case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
        handle_named_value(msg);
        break;

    case MAVLINK_MSG_ID_CAN_FRAME:
    case MAVLINK_MSG_ID_CANFD_FRAME:
        handle_can_frame(msg);
        break;

    case MAVLINK_MSG_ID_CAN_FILTER_MODIFY:
#if HAL_CANMANAGER_ENABLED
        AP::can().handle_can_filter_modify(msg);
#endif
        break;

#if AP_OPENDRONEID_ENABLED
    case MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS:
    case MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID:
    case MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID:
    case MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID:
    case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM:
    case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE:
        AP::opendroneid().handle_msg(chan, msg);
        break;
#endif

#if AP_SIGNED_FIRMWARE
    case MAVLINK_MSG_ID_SECURE_COMMAND:
    case MAVLINK_MSG_ID_SECURE_COMMAND_REPLY:
        AP_CheckFirmware::handle_msg(chan, msg);
        break;
#endif
    }

四、update_send

MAVLINK发送的消息可分为两类，一类是应答式，包含任务航点、集结点和电子围栏的应答消息。这类消息只有在收到特定消息的情况下才会发送。还有一类就是固定频率进行发送的。
在下面的代码中，应答式消息通过指针数组missionitemprotocols分别对三种应答式消息进行轮询发送，prot->update()的定义在下面列出。
chan(i)->update_send()就是对各个mavlink通道的消息进行发送的程序，这个函数比较重要，也将在下面讲解。

void GCS::update_send()
{
    
    
    update_send_has_been_called = true;

    if (!initialised_missionitemprotocol_objects) {
    
    
        initialised_missionitemprotocol_objects = true;
        // once-only initialisation of MissionItemProtocol objects:
#if AP_MISSION_ENABLED
        AP_Mission *mission = AP::mission();
        if (mission != nullptr) {
    
    
            missionitemprotocols[MAV_MISSION_TYPE_MISSION] = new MissionItemProtocol_Waypoints(*mission);
        }
#endif
#if HAL_RALLY_ENABLED
        AP_Rally *rally = AP::rally();
        if (rally != nullptr) {
    
    
            missionitemprotocols[MAV_MISSION_TYPE_RALLY] = new MissionItemProtocol_Rally(*rally);
        }
#endif
#if AP_FENCE_ENABLED
        AC_Fence *fence = AP::fence();
        if (fence != nullptr) {
    
    
            missionitemprotocols[MAV_MISSION_TYPE_FENCE] = new MissionItemProtocol_Fence(*fence);
        }
#endif
    }

    for (auto *prot : missionitemprotocols) {
    
    
        if (prot == nullptr) {
    
    
            continue;
        }
        prot->update();
    }

    // round-robin the GCS_MAVLINK backend that gets to go first so
    // one backend doesn't monopolise all of the time allowed for sending
    // messages
    for (uint8_t i=first_backend_to_send; i<num_gcs(); i++) {
    
    
        chan(i)->update_send();
    }
    for (uint8_t i=0; i<first_backend_to_send; i++) {
    
    
        chan(i)->update_send();
    }

    service_statustext();

    first_backend_to_send++;
    if (first_backend_to_send >= num_gcs()) {
    
    
        first_backend_to_send = 0;
    }
}

MissionItemProtocol::update()

这个方法用于发送应答消息，未收到数据或者未建立链接不应答，如果接收航电超时，则停止发送停止接收消息。否则的话通过send_message方法发送请求航点消息（传输航点时，飞控请求后地面站才会发下一个点，具体参考MAVLINk手册https://mavlink.io/en/services/mission.html）

void MissionItemProtocol::update()
{
    
    
    if (!receiving) {
    
    
        // we don't need to do anything unless we're sending requests
        return;
    }
    if (link == nullptr) {
    
    
        INTERNAL_ERROR(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
        return;
    }
    // stop waypoint receiving if timeout
    const uint32_t tnow = AP_HAL::millis();
    if (tnow - timelast_receive_ms > upload_timeout_ms) {
    
    
        receiving = false;
        timeout();
        const mavlink_channel_t chan = link->get_chan();
        if (HAVE_PAYLOAD_SPACE(chan, MISSION_ACK)) {
    
    
            mavlink_msg_mission_ack_send(chan,
                                         dest_sysid,
                                         dest_compid,
                                         MAV_MISSION_OPERATION_CANCELLED,
                                         mission_type());
        }
        link = nullptr;
        free_upload_resources();
        return;
    }
    // resend request if we haven't gotten one:
    const uint32_t wp_recv_timeout_ms = 1000U + link->get_stream_slowdown_ms();
    if (tnow - timelast_request_ms > wp_recv_timeout_ms) {
    
    
        timelast_request_ms = tnow;
        link->send_message(next_item_ap_message_id());
    }
}

send_message方法的定义如下：
这个方法就是把要发送的消息的id通过pushed_ap_message_ids.set添加要发送的队列中，在下面的程序中通过相应的mavlink通道进行获取并发送。

void GCS_MAVLINK::send_message(enum ap_message id)
{
    
    
    if (id == MSG_HEARTBEAT || id == MSG_HIGH_LATENCY2) {
    
    
        save_signing_timestamp(false);
        // update the mask of all streaming channels
        if (is_streaming()) {
    
    
            GCS_MAVLINK::chan_is_streaming |= (1U<<(chan-MAVLINK_COMM_0));
        } else {
    
    
            GCS_MAVLINK::chan_is_streaming &= ~(1U<<(chan-MAVLINK_COMM_0));
        }
    }

    pushed_ap_message_ids.set(id);
}

GCS_MAVLINK::update_send()

初始化等操作

void GCS_MAVLINK::update_send()
{
    
    
#if !defined(HAL_BUILD_AP_PERIPH) || HAL_LOGGING_ENABLED
    if (!hal.scheduler->in_delay_callback()) {
    
    
        // AP_Logger will not send log data if we are armed.
        AP::logger().handle_log_send();
    }
#endif
    send_ftp_replies();

    if (!deferred_messages_initialised) {
    
    
        initialise_message_intervals_from_streamrates();
#if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED
        initialise_message_intervals_from_config_files();
#endif
        deferred_messages_initialised = true;
    }

#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
    uint32_t retry_deferred_body_start = AP_HAL::micros();
#endif

mavlink发送的最小时间间隔为5ms

    const uint32_t start = AP_HAL::millis();
    const uint16_t start16 = start & 0xFFFF;
    while (AP_HAL::millis() - start < 5) {
    
     // spend a max of 5ms sending messages.  This should never trigger - out_of_time() should become true
        if (gcs().out_of_time()) {
    
    
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
            try_send_message_stats.out_of_time++;
#endif
            break;
        }

这里处理的是定时发送的消息，通过deferred_message_to_send_index获取id

#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
        retry_deferred_body_start = AP_HAL::micros();
#endif

        // check if any "specially handled" messages should be sent out
        {
    
    
            const int8_t next = deferred_message_to_send_index(start16);
            if (next != -1) {
    
    
                if (!do_try_send_message(deferred_message[next].id)) {
    
    
                    break;
                }
                // we try to keep output on a regular clock to avoid
                // user support questions:
                const uint16_t interval_ms = deferred_message[next].interval_ms;
                deferred_message[next].last_sent_ms += interval_ms;
                // but we do not want to try to catch up too much:
                if (uint16_t(start16 - deferred_message[next].last_sent_ms) > interval_ms) {
    
    
                    deferred_message[next].last_sent_ms = start16;
                }

                next_deferred_message_to_send_cache = -1; // deferred_message_to_send will recalculate
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
                const uint32_t stop = AP_HAL::micros();
                const uint32_t delta = stop - retry_deferred_body_start;
                if (delta > try_send_message_stats.max_retry_deferred_body_us) {
    
    
                    try_send_message_stats.max_retry_deferred_body_us = delta;
                    try_send_message_stats.max_retry_deferred_body_type = 1;
                }
#endif
                continue;
            }
        }

这里处理的是上面的应答式消息，通过pushed_ap_message_ids.first_set()获取id

       // check for any messages that the code has explicitly sent
        const int16_t fs = pushed_ap_message_ids.first_set();
        if (fs != -1) {
    
    
            ap_message next = (ap_message)fs;
            if (!do_try_send_message(next)) {
    
    
                break;
            }
            pushed_ap_message_ids.clear(next);
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
            const uint32_t stop = AP_HAL::micros();
            const uint32_t delta = stop - retry_deferred_body_start;
            if (delta > try_send_message_stats.max_retry_deferred_body_us) {
    
    
                try_send_message_stats.max_retry_deferred_body_us = delta;
                try_send_message_stats.max_retry_deferred_body_type = 2;
            }
#endif
            continue;
        }

这里发送的消息会考虑剩余的发送缓冲区大小，从而动态调整发送间隔

       ap_message next = next_deferred_bucket_message_to_send(start16);
        if (next != no_message_to_send) {
    
    
            if (!do_try_send_message(next)) {
    
    
                break;
            }
            bucket_message_ids_to_send.clear(next);
            if (bucket_message_ids_to_send.count() == 0) {
    
    
                // we sent everything in the bucket.  Reschedule it.
                // we try to keep output on a regular clock to avoid
                // user support questions:
                const uint16_t interval_ms = get_reschedule_interval_ms(deferred_message_bucket[sending_bucket_id]);
                deferred_message_bucket[sending_bucket_id].last_sent_ms += interval_ms;
                // but we do not want to try to catch up too much:
                if (uint16_t(start16 - deferred_message_bucket[sending_bucket_id].last_sent_ms) > interval_ms) {
    
    
                    deferred_message_bucket[sending_bucket_id].last_sent_ms = start16;
                }
                find_next_bucket_to_send(start16);
            }
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
                const uint32_t stop = AP_HAL::micros();
                const uint32_t delta = stop - retry_deferred_body_start;
                if (delta > try_send_message_stats.max_retry_deferred_body_us) {
    
    
                    try_send_message_stats.max_retry_deferred_body_us = delta;
                    try_send_message_stats.max_retry_deferred_body_type = 3;
                }
#endif
            continue;
        }
        break;
    }
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
    const uint32_t stop = AP_HAL::micros();
    const uint32_t delta = stop - retry_deferred_body_start;
    if (delta > try_send_message_stats.max_retry_deferred_body_us) {
    
    
        try_send_message_stats.max_retry_deferred_body_us = delta;
        try_send_message_stats.max_retry_deferred_body_type = 4;
    }
#endif

    // update the number of packets transmitted base on seqno, making
    // the assumption that we don't send more than 256 messages
    // between the last pass through here
    mavlink_status_t *status = mavlink_get_channel_status(chan);
    if (status != nullptr) {
    
    
        send_packet_count += uint8_t(status->current_tx_seq - last_tx_seq);
        last_tx_seq = status->current_tx_seq;
    }
}

上面都是通过do_try_send_message方法进行消息的发送的，其定义如下：这个里卖主要是调用了try_send_message消息进行发送

bool GCS_MAVLINK::do_try_send_message(const ap_message id)
{
    
    
    const bool in_delay_callback = hal.scheduler->in_delay_callback();
    if (in_delay_callback && !should_send_message_in_delay_callback(id)) {
    
    
        return true;
    }
    if (telemetry_delayed()) {
    
    
        return false;
    }
    WITH_SEMAPHORE(comm_chan_lock(chan));
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
    void *data = hal.scheduler->disable_interrupts_save();
    uint32_t start_send_message_us = AP_HAL::micros();
#endif
    if (!try_send_message(id)) {
    
    
        // didn't fit in buffer...
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
        try_send_message_stats.no_space_for_message++;
        hal.scheduler->restore_interrupts(data);
#endif
        return false;
    }
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
    const uint32_t delta_us = AP_HAL::micros() - start_send_message_us;
    hal.scheduler->restore_interrupts(data);
    if (delta_us > try_send_message_stats.longest_time_us) {
    
    
        try_send_message_stats.longest_time_us = delta_us;
        try_send_message_stats.longest_id = id;
    }
#endif
    return true;
}

try_send_message方法会根据id调用相应的MAVLINK打包函数，对消息进行打包发送，以send_attitude为例分析

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_QUATERNION:
        CHECK_PAYLOAD_SIZE(ATTITUDE_QUATERNION);
        send_attitude_quaternion();
        break;

    case MSG_NEXT_PARAM:
        CHECK_PAYLOAD_SIZE(PARAM_VALUE);
        queued_param_send();
        break;

    case MSG_HEARTBEAT:
        CHECK_PAYLOAD_SIZE(HEARTBEAT);
        last_heartbeat_time = AP_HAL::millis();
        send_heartbeat();
        break;

    case MSG_HWSTATUS:
        CHECK_PAYLOAD_SIZE(HWSTATUS);
        send_hwstatus();
        break;

    case MSG_LOCATION:
        CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
        send_global_position_int();
        break;

    case MSG_HOME:
        CHECK_PAYLOAD_SIZE(HOME_POSITION);
        send_home_position();
        break;

    case MSG_ORIGIN:
        CHECK_PAYLOAD_SIZE(GPS_GLOBAL_ORIGIN);
        send_gps_global_origin();
        break;

#if AP_RPM_ENABLED
    case MSG_RPM:
        CHECK_PAYLOAD_SIZE(RPM);
        send_rpm();
        break;
#endif

    case MSG_CURRENT_WAYPOINT:
    case MSG_MISSION_ITEM_REACHED:
    case MSG_NEXT_MISSION_REQUEST_WAYPOINTS:
    case MSG_NEXT_MISSION_REQUEST_RALLY:
    case MSG_NEXT_MISSION_REQUEST_FENCE:
        ret = try_send_mission_message(id);
        break;

#if COMPASS_CAL_ENABLED
    case MSG_MAG_CAL_PROGRESS:
        ret = AP::compass().send_mag_cal_progress(*this);
        break;
    case MSG_MAG_CAL_REPORT:
        ret = AP::compass().send_mag_cal_report(*this);
        break;
#endif

    case MSG_BATTERY_STATUS:
        send_battery_status();
        break;

#if AP_MAVLINK_BATTERY2_ENABLED
    case MSG_BATTERY2:
        CHECK_PAYLOAD_SIZE(BATTERY2);
        send_battery2();
        break;
#endif

    case MSG_EKF_STATUS_REPORT:
        CHECK_PAYLOAD_SIZE(EKF_STATUS_REPORT);
        AP::ahrs().send_ekf_status_report(*this);
        break;

    case MSG_MEMINFO:
        CHECK_PAYLOAD_SIZE(MEMINFO);
        send_meminfo();
        break;

    case MSG_FENCE_STATUS:
        CHECK_PAYLOAD_SIZE(FENCE_STATUS);
        send_fence_status();
        break;

    case MSG_RANGEFINDER:
        CHECK_PAYLOAD_SIZE(RANGEFINDER);
        send_rangefinder();
        break;

    case MSG_DISTANCE_SENSOR:
        send_distance_sensor();
        break;

#if AP_CAMERA_ENABLED
    case MSG_CAMERA_FEEDBACK:
        {
    
    
            AP_Camera *camera = AP::camera();
            if (camera == nullptr) {
    
    
                break;
            }
            CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK);
            camera->send_feedback(chan);
        }
        break;
#endif

    case MSG_SYSTEM_TIME:
        CHECK_PAYLOAD_SIZE(SYSTEM_TIME);
        send_system_time();
        break;
    case MSG_GPS_RAW:
        CHECK_PAYLOAD_SIZE(GPS_RAW_INT);
        AP::gps().send_mavlink_gps_raw(chan);
        break;
    case MSG_GPS_RTK:
        CHECK_PAYLOAD_SIZE(GPS_RTK);
        AP::gps().send_mavlink_gps_rtk(chan, 0);
        break;
    case MSG_GPS2_RAW:
#if GPS_MAX_RECEIVERS > 1
        CHECK_PAYLOAD_SIZE(GPS2_RAW);
        AP::gps().send_mavlink_gps2_raw(chan);
#endif
        break;
    case MSG_GPS2_RTK:
#if GPS_MAX_RECEIVERS > 1
        CHECK_PAYLOAD_SIZE(GPS2_RTK);
        AP::gps().send_mavlink_gps_rtk(chan, 1);
#endif
        break;
    case MSG_LOCAL_POSITION:
        CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED);
        send_local_position();
        break;

    case MSG_GIMBAL_DEVICE_ATTITUDE_STATUS:
#if HAL_MOUNT_ENABLED
        CHECK_PAYLOAD_SIZE(GIMBAL_DEVICE_ATTITUDE_STATUS);
        send_gimbal_device_attitude_status();
#endif
        break;
    case MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE:
#if HAL_MOUNT_ENABLED
        CHECK_PAYLOAD_SIZE(AUTOPILOT_STATE_FOR_GIMBAL_DEVICE);
        send_autopilot_state_for_gimbal_device();
#endif
        break;

    case MSG_OPTICAL_FLOW:
#if AP_OPTICALFLOW_ENABLED
        CHECK_PAYLOAD_SIZE(OPTICAL_FLOW);
        send_opticalflow();
#endif
        break;

    case MSG_ATTITUDE_TARGET:
        CHECK_PAYLOAD_SIZE(ATTITUDE_TARGET);
        send_attitude_target();
        break;

    case MSG_POSITION_TARGET_GLOBAL_INT:
        CHECK_PAYLOAD_SIZE(POSITION_TARGET_GLOBAL_INT);
        send_position_target_global_int();
        break;

    case MSG_POSITION_TARGET_LOCAL_NED:
        CHECK_PAYLOAD_SIZE(POSITION_TARGET_LOCAL_NED);
        send_position_target_local_ned();
        break;

    case MSG_POWER_STATUS:
        CHECK_PAYLOAD_SIZE(POWER_STATUS);
        send_power_status();
        break;

    case MSG_MCU_STATUS:
#if HAL_WITH_MCU_MONITORING
        CHECK_PAYLOAD_SIZE(MCU_STATUS);
        send_mcu_status();
#endif
        break;

    case MSG_RC_CHANNELS:
        CHECK_PAYLOAD_SIZE(RC_CHANNELS);
        send_rc_channels();
        break;

    case MSG_RC_CHANNELS_RAW:
        CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
        send_rc_channels_raw();
        break;

    case MSG_RAW_IMU:
        CHECK_PAYLOAD_SIZE(RAW_IMU);
        send_raw_imu();
        break;

    case MSG_SCALED_IMU:
        CHECK_PAYLOAD_SIZE(SCALED_IMU);
        send_scaled_imu(0, mavlink_msg_scaled_imu_send);
        break;

    case MSG_SCALED_IMU2:
        CHECK_PAYLOAD_SIZE(SCALED_IMU2);
        send_scaled_imu(1, mavlink_msg_scaled_imu2_send);
        break;

    case MSG_SCALED_IMU3:
        CHECK_PAYLOAD_SIZE(SCALED_IMU3);
        send_scaled_imu(2, mavlink_msg_scaled_imu3_send);
        break;

    case MSG_SCALED_PRESSURE:
        CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
        send_scaled_pressure();
        break;

    case MSG_SCALED_PRESSURE2:
        CHECK_PAYLOAD_SIZE(SCALED_PRESSURE2);
        send_scaled_pressure2();
        break;

    case MSG_SCALED_PRESSURE3:
        CHECK_PAYLOAD_SIZE(SCALED_PRESSURE3);
        send_scaled_pressure3();
        break;

    case MSG_SERVO_OUTPUT_RAW:
        CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
        send_servo_output_raw();
        break;

    case MSG_SIMSTATE:
#if AP_SIM_ENABLED
        CHECK_PAYLOAD_SIZE(SIMSTATE);
        send_simstate();
#endif
        break;

    case MSG_SIM_STATE:
#if AP_SIM_ENABLED
        CHECK_PAYLOAD_SIZE(SIM_STATE);
        send_sim_state();
#endif
        break;

    case MSG_SYS_STATUS:
        CHECK_PAYLOAD_SIZE(SYS_STATUS);
        send_sys_status();
        break;

    case MSG_AHRS2:
        CHECK_PAYLOAD_SIZE(AHRS2);
        send_ahrs2();
        break;

    case MSG_PID_TUNING:
        CHECK_PAYLOAD_SIZE(PID_TUNING);
        send_pid_tuning();
        break;

    case MSG_NAV_CONTROLLER_OUTPUT:
        CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
        send_nav_controller_output();
        break;

    case MSG_AHRS:
        CHECK_PAYLOAD_SIZE(AHRS);
        send_ahrs();
        break;

    case MSG_EXTENDED_SYS_STATE:
        CHECK_PAYLOAD_SIZE(EXTENDED_SYS_STATE);
        send_extended_sys_state();
        break;

    case MSG_VFR_HUD:
        CHECK_PAYLOAD_SIZE(VFR_HUD);
        send_vfr_hud();
        break;

    case MSG_VIBRATION:
        CHECK_PAYLOAD_SIZE(VIBRATION);
        send_vibration();
        break;

    case MSG_GENERATOR_STATUS:
#if HAL_GENERATOR_ENABLED
    	CHECK_PAYLOAD_SIZE(GENERATOR_STATUS);
    	send_generator_status();
#endif
    	break;

    case MSG_AUTOPILOT_VERSION:
        CHECK_PAYLOAD_SIZE(AUTOPILOT_VERSION);
        send_autopilot_version();
        break;

    case MSG_ESC_TELEMETRY:
#if HAL_WITH_ESC_TELEM
        AP::esc_telem().send_esc_telemetry_mavlink(uint8_t(chan));
#endif
        break;

    case MSG_EFI_STATUS: {
    
    
#if HAL_EFI_ENABLED
        CHECK_PAYLOAD_SIZE(EFI_STATUS);
        AP_EFI *efi = AP::EFI();
        if (efi) {
    
    
            efi->send_mavlink_status(chan);
        }
#endif
        break;
    }

    case MSG_WINCH_STATUS:
        CHECK_PAYLOAD_SIZE(WINCH_STATUS);
        send_winch_status();
        break;

    case MSG_WATER_DEPTH:
#if 9APM_BUILD_TYPE(APM_BUILD_Rover)
        CHECK_PAYLOAD_SIZE(WATER_DEPTH);
        send_water_depth();
#endif
        break;

    case MSG_HIGH_LATENCY2:
#if HAL_HIGH_LATENCY2_ENABLED
        CHECK_PAYLOAD_SIZE(HIGH_LATENCY2);
        send_high_latency2();
#endif // HAL_HIGH_LATENCY2_ENABLED
        break;

    case MSG_AIS_VESSEL: {
    
    
#if AP_AIS_ENABLED
        AP_AIS *ais = AP_AIS::get_singleton();
        if (ais) {
    
    
            ais->send(chan);
        }
#endif
        break;
    }


    case MSG_UAVIONIX_ADSB_OUT_STATUS:
#if HAL_ADSB_ENABLED
        CHECK_PAYLOAD_SIZE(UAVIONIX_ADSB_OUT_STATUS);
        send_uavionix_adsb_out_status();
#endif
        break;

    default:
        // try_send_message must always at some stage return true for
        // a message, or we will attempt to infinitely retry the
        // message as part of send_message.
        // This message will be sent out at the same rate as the
        // unknown message, so should be safe.
        gcs().send_text(MAV_SEVERITY_DEBUG, "Sending unknown message (%u)", id);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        AP_HAL::panic("Sending unknown ap_message %u", id);
#endif
        break;
    }

    return ret;
}

send_attitude的定义如下：就是获取姿态数据，然后调用mavlink_msg_attitude_send函数对姿态数据进行发送

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_msg_attitude_send的定义如下：改函数调用mavlink库函数对姿态进行打包发送。到此mavlink的发送流程就结束了

static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed)
{
    
    
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
    char buf[MAVLINK_MSG_ID_ATTITUDE_LEN];
    _mav_put_uint32_t(buf, 0, time_boot_ms);
    _mav_put_float(buf, 4, roll);
    _mav_put_float(buf, 8, pitch);
    _mav_put_float(buf, 12, yaw);
    _mav_put_float(buf, 16, rollspeed);
    _mav_put_float(buf, 20, pitchspeed);
    _mav_put_float(buf, 24, yawspeed);

    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC);
#else
    mavlink_attitude_t packet;
    packet.time_boot_ms = time_boot_ms;
    packet.roll = roll;
    packet.pitch = pitch;
    packet.yaw = yaw;
    packet.rollspeed = rollspeed;
    packet.pitchspeed = pitchspeed;
    packet.yawspeed = yawspeed;

    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC);
#endif
}