Ardupilot飞控Mavlink代码学习

本文主要是介绍Ardupilot飞控Mavlink代码学习，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

摘自：https://blog.csdn.net/lixiaoweimashixiao/article/details/80919995

Ardupilot飞控Mavlink代码学习

魔城烟雨 2018-07-05 23:36:33 4302 收藏 19

分类专栏： ardupilot学习

版权

摘要

本节主要记录自己学习Ardupilot的Mavlink协议的过程，欢迎一起交流分析！

1.Ardupilot怎么实现Mavlink初始化

（1）初始化USB端口，进行Mavlink通信
USB初始化

可以看出gcs_chan是GCS_MAVLINK_Copter的对象

其中 AP_SerialManager serial_manager;
AP_SerialManager::SerialProtocol_MAVLink（SerialProtocol_MAVLink = 1,）
我们看下初始化函数：

void GCS_MAVLINK::setup_uart(const AP_SerialManager& serial_manager, AP_SerialManager::SerialProtocol protocol, uint8_t instance)
{serialmanager_p = &serial_manager;//寻找串口-------------search for serial portAP_HAL::UARTDriver *uart;uart = serial_manager.find_serial(protocol, instance); //寻找有没有串口if (uart == nullptr){//如果没有找到串口立即返回--------return immediately if not foundreturn;}//获取Mavlink协议通道---------get associated mavlink channelmavlink_channel_t mav_chan;if (!serial_manager.get_mavlink_channel(protocol, instance, mav_chan)){//没找到就返回--------return immediately in unlikely case mavlink channel cannot be foundreturn;}/*Now try to cope with SiK radios that may be stuck in bootloadermode because CTS was held while powering on. This tells thebootloader to wait for a firmware. It affects any SiK radio withCTS connected that is externally powered. To cope we send 0x300x20 at 115200 on startup, which tells the bootloader to resetand boot normally现在试着处理可能在Bootloader中插入的SIK收音机。模式，因为CTS在供电时被保持。这告诉了等待加载固件的Bootloader。它影响任何SIK收音机CTS连接的是外部供电。为了应付我们发送0x3 0x20在启动时115200，它告诉引导加载程序重置正常启动*/uart->begin(115200); //设置波特率115200AP_HAL::UARTDriver::flow_control old_flow_control = uart->get_flow_control();uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);for (uint8_t i=0; i<3; i++){hal.scheduler->delay(1);uart->write(0x30);uart->write(0x20);}// since tcdrain() and TCSADRAIN may not be implemented...hal.scheduler->delay(1);uart->set_flow_control(old_flow_control);//现在回到预期的波特率----- now change back to desired baudrateuart->begin(serial_manager.find_baudrate(protocol, instance));//串口初始化and init the gcs instanceinit(uart, mav_chan); //初始化GCS实例AP_SerialManager::SerialProtocol mavlink_protocol = serialmanager_p->get_mavlink_protocol(mav_chan);mavlink_status_t *status = mavlink_get_channel_status(chan);if (status == nullptr){return;}if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2) //可以增加密钥{// load signing keyload_signing_key();if (status->signing == nullptr) {// if signing is off start by sending MAVLink1.status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;}// announce that we are MAVLink2 capablehal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MAVLINK2);} else if (status){// user has asked to only send MAVLink1status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;}if (chan == MAVLINK_COMM_0) {// Always start with MAVLink1 on first port for now, to allow for recovery// after experiments with MAVLink2status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;}
}

该函数大体的作用：判断有没有串口，已经串口通道，设置串口波特率，初始化串口协议，判断是否使用Mavlink2加密协议等等
我们重点看下init(uart, mav_chan); //初始化GCS实例

void GCS_MAVLINK::init(AP_HAL::UARTDriver *port, mavlink_channel_t mav_chan)
{if (!valid_channel(mav_chan)){return;}_port = port;chan = mav_chan;mavlink_comm_port[chan] = _port;initialised = true;_queued_parameter = nullptr;reset_cli_timeout();if (!_perf_packet) {_perf_packet = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "GCS_Packet");}if (!_perf_update) {_perf_update = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "GCS_Update");}
}

在init中主要对_port进行串口赋值, mavlink_comm_port[] 赋值。_port当前串口的指针，
mavlink_comm_port，记录对应通道的串口指针。

在进行地面站与飞控的链接前，还需要设置串口5ms延迟等待。

hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);
static void mavlink_delay_cb_static()
{
copter.mavlink_delay_cb();
}
我们重点看下这个函数 copter.mavlink_delay_cb();在主线程没有启动之前，通过该函数实现通信。

void Copter::mavlink_delay_cb()
{static uint32_t last_1hz, last_50hz, last_5s;if (!gcs_chan[0].initialised || in_mavlink_delay) return;in_mavlink_delay = true;uint32_t tnow = millis();if (tnow - last_1hz > 1000) //大于1s{last_1hz = tnow;gcs_send_heartbeat(); //发送心跳包gcs_send_message(MSG_EXTENDED_STATUS1); //发送外部状态}if (tnow - last_50hz > 20) //大于50ms{last_50hz = tnow;gcs_check_input(); //进行输入信息解包gcs_data_stream_send(); //发送数据流信息gcs_send_deferred(); //发送队列消息,status_text, 和data_stream.notify.update(); //更新通知RGBled}if (tnow - last_5s > 5000) //大于5s,发送初始化APM{last_5s = tnow;gcs_send_text(MAV_SEVERITY_INFO, "Initialising APM");}check_usb_mux(); //检查USB连接in_mavlink_delay = false;
}

这个函数是：一个处理MAVLink分组的延迟的回调函数。我们把这个设置为在长时间运行的库函数初始化例程中，回调以允许MavLink在等待初始化时处理数据包完成。
（2）选择其他串口初始化，进行Mavlink通信，可以选择无线数传

for (uint8_t i = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++)  //这里初始化三路串口
{gcs_chan[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i);
}

到这里初始化Mavlink通信准备已经完成

2.Mavlink消息通信过程

主要用到四条任务函数：

    SCHED_TASK(gcs_check_input,      400,    180), //检查串口数据，对串口数据解析，解包操作SCHED_TASK(gcs_send_heartbeat,     1,    110), //发送心跳包SCHED_TASK(gcs_send_deferred,     50,    550), //发送队列消息，statustext和data_streamSCHED_TASK(gcs_data_stream_send,  50,    550), //发送数据流

（1）首先分析SCHED_TASK(gcs_check_input, 400, 180), //检查串口数据，对串口数据解析，解包操作
这里写图片描述

void GCS_MAVLINK::update(run_cli_fn run_cli, uint32_t max_time_us)
{// receive new packetsmavlink_message_t msg;     //协议mavlink_status_t status;   //协议状态uint32_t tstart_us = AP_HAL::micros(); //开始时间hal.util->perf_begin(_perf_update);status.packet_rx_drop_count = 0;// process received bytesuint16_t nbytes = comm_get_available(chan); //接收串口数据for (uint16_t i=0; i<nbytes; i++){uint8_t c = comm_receive_ch(chan); //接收串口通道if (run_cli)  //运行计算{/* allow CLI to be started by hitting enter 3 times, if no*  heartbeat packets have been received */if ((mavlink_active==0) && (AP_HAL::millis() - _cli_timeout) < 20000 && comm_is_idle(chan)) {if (c == '\n' || c == '\r') {crlf_count++;} else {crlf_count = 0;}if (crlf_count == 3) {run_cli(_port);}}}bool parsed_packet = false;// Try to get a new messageif (mavlink_parse_char(chan, c, &msg, &status)) //开始获取新信息，进行数据解包{hal.util->perf_begin(_perf_packet);packetReceived(status, msg);hal.util->perf_end(_perf_packet);parsed_packet = true;}if (parsed_packet || i % 100 == 0) {//确保我们不会花太多时间解析MavLink消息---- make sure we don't spend too much time parsing mavlink messagesif (AP_HAL::micros() - tstart_us > max_time_us){break;}}}if (!waypoint_receiving){hal.util->perf_end(_perf_update);    return;}uint32_t tnow = AP_HAL::millis();uint32_t wp_recv_time = 1000U + (stream_slowdown*20);// stop waypoint receiving if timeoutif (waypoint_receiving && (tnow - waypoint_timelast_receive) > wp_recv_time+waypoint_receive_timeout){waypoint_receiving = false;} else if (waypoint_receiving &&(tnow - waypoint_timelast_request) > wp_recv_time){waypoint_timelast_request = tnow;send_message(MSG_NEXT_WAYPOINT);}hal.util->perf_end(_perf_update);    
}

1》我们重点看下几个函数：mavlink_parse_char(chan, c, &msg, &status)

MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
{uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status);if (msg_received == MAVLINK_FRAMING_BAD_CRC ||msg_received == MAVLINK_FRAMING_BAD_SIGNATURE) {// we got a bad CRC. Treat as a parse failuremavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan);mavlink_status_t* status = mavlink_get_channel_status(chan);_mav_parse_error(status);status->msg_received = MAVLINK_FRAMING_INCOMPLETE;status->parse_state = MAVLINK_PARSE_STATE_IDLE;if (c == MAVLINK_STX){status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;rxmsg->len = 0;mavlink_start_checksum(rxmsg);}return 0;}return msg_received;
}

**这是一个处理完整的MavLink解析的便利函数。函数将一次解析一个字节，并返回完整的数据包一次。它可以被成功解码。这个函数将返回0或1。
消息被解析为内部缓冲区（每个通道一个）。当完成时收到消息后，将其复制到returnMsg中，并且该信道的状态为复制到returnStats。*

看下其中的一个函数：uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status);

MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
{return mavlink_frame_char_buffer(mavlink_get_channel_buffer(chan),mavlink_get_channel_status(chan),c,r_message,r_mavlink_status);
}

继续看代码:
return mavlink_frame_char_buffer(mavlink_get_channel_buffer(chan),
mavlink_get_channel_status(chan),
c,
r_message,
r_mavlink_status);

MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, mavlink_status_t* status,uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
{/* Enable this option to check the length of each message.This allows invalid messages to be caught much sooner. Use if the transmissionmedium is prone to missing (or extra) characters (e.g. a radio that fades inand out). Only use if the channel will only contain messages types listed inthe headers.*/
#ifdef MAVLINK_CHECK_MESSAGE_LENGTH
#ifndef MAVLINK_MESSAGE_LENGTHstatic const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
#define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid]
#endif
#endifint bufferIndex = 0;status->msg_received = MAVLINK_FRAMING_INCOMPLETE;switch (status->parse_state){case MAVLINK_PARSE_STATE_UNINIT:case MAVLINK_PARSE_STATE_IDLE:if (c == MAVLINK_STX){status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;rxmsg->len = 0;rxmsg->magic = c;status->flags &= ~MAVLINK_STATUS_FLAG_IN_MAVLINK1;mavlink_start_checksum(rxmsg);} else if (c == MAVLINK_STX_MAVLINK1){status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;rxmsg->len = 0;rxmsg->magic = c;status->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1;mavlink_start_checksum(rxmsg);}break;case MAVLINK_PARSE_STATE_GOT_STX:if (status->msg_received 
/* Support shorter buffers than thedefault maximum packet size */
#if (MAVLINK_MAX_PAYLOAD_LEN < 255)|| c > MAVLINK_MAX_PAYLOAD_LEN
#endif){status->buffer_overrun++;_mav_parse_error(status);status->msg_received = 0;status->parse_state = MAVLINK_PARSE_STATE_IDLE;}else{// NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2rxmsg->len = c;status->packet_idx = 0;mavlink_update_checksum(rxmsg, c);if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {rxmsg->incompat_flags = 0;rxmsg->compat_flags = 0;status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;} else {status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;}}break;case MAVLINK_PARSE_STATE_GOT_LENGTH:rxmsg->incompat_flags = c;if ((rxmsg->incompat_flags & ~MAVLINK_IFLAG_MASK) != 0) {// message includes an incompatible feature flag_mav_parse_error(status);status->msg_received = 0;status->parse_state = MAVLINK_PARSE_STATE_IDLE;break;}mavlink_update_checksum(rxmsg, c);status->parse_state = MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS;break;case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS:rxmsg->compat_flags = c;mavlink_update_checksum(rxmsg, c);status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;break;case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS:rxmsg->seq = c;mavlink_update_checksum(rxmsg, c);status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;break;case MAVLINK_PARSE_STATE_GOT_SEQ:rxmsg->sysid = c;mavlink_update_checksum(rxmsg, c);status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;break;case MAVLINK_PARSE_STATE_GOT_SYSID:rxmsg->compid = c;mavlink_update_checksum(rxmsg, c);status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;break;case MAVLINK_PARSE_STATE_GOT_COMPID:rxmsg->msgid = c;mavlink_update_checksum(rxmsg, c);if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
#ifdef MAVLINK_CHECK_MESSAGE_LENGTHif (rxmsg->len != MAVLINK_MESSAGE_LENGTH(rxmsg->msgid)){_mav_parse_error(status);status->parse_state = MAVLINK_PARSE_STATE_IDLE;break;}
#endif} else {status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID1;}break;case MAVLINK_PARSE_STATE_GOT_MSGID1:rxmsg->msgid |= c<<8;mavlink_update_checksum(rxmsg, c);status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID2;break;case MAVLINK_PARSE_STATE_GOT_MSGID2:rxmsg->msgid |= c<<16;mavlink_update_checksum(rxmsg, c);status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
#ifdef MAVLINK_CHECK_MESSAGE_LENGTHif (rxmsg->len != MAVLINK_MESSAGE_LENGTH(rxmsg->msgid)){_mav_parse_error(status);status->parse_state = MAVLINK_PARSE_STATE_IDLE;break;}
#endifbreak;case MAVLINK_PARSE_STATE_GOT_MSGID3:_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;mavlink_update_checksum(rxmsg, c);if (status->packet_idx == rxmsg->len){status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;}break;case MAVLINK_PARSE_STATE_GOT_PAYLOAD: {const mavlink_msg_entry_t *e = mavlink_get_msg_entry(rxmsg->msgid);uint8_t crc_extra = e?e->crc_extra:0;mavlink_update_checksum(rxmsg, crc_extra);if (c != (rxmsg->checksum & 0xFF)) {status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1;} else {status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;}rxmsg->ck[0] = c;// zero-fill the packet to cope with short incoming packetsif (e && status->packet_idx < e->msg_len) {memset(&_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx], 0, e->msg_len - status->packet_idx);}break;}case MAVLINK_PARSE_STATE_GOT_CRC1:case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:if (status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c != (rxmsg->checksum >> 8)) {// got a bad CRC messagestatus->msg_received = MAVLINK_FRAMING_BAD_CRC;} else {// Successfully got messagestatus->msg_received = MAVLINK_FRAMING_OK;}rxmsg->ck[1] = c;if (rxmsg->incompat_flags & MAVLINK_IFLAG_SIGNED) {status->parse_state = MAVLINK_PARSE_STATE_SIGNATURE_WAIT;status->signature_wait = MAVLINK_SIGNATURE_BLOCK_LEN;// If the CRC is already wrong, don't overwrite msg_received,// otherwise we can end up with garbage flagged as valid.if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) {status->msg_received = MAVLINK_FRAMING_INCOMPLETE;}} else {if (status->signing &&(status->signing->accept_unsigned_callback == NULL ||!status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {// If the CRC is already wrong, don't overwrite msg_received.if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) {status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;}}status->parse_state = MAVLINK_PARSE_STATE_IDLE;memcpy(r_message, rxmsg, sizeof(mavlink_message_t));}break;case MAVLINK_PARSE_STATE_SIGNATURE_WAIT:rxmsg->signature[MAVLINK_SIGNATURE_BLOCK_LEN-status->signature_wait] = c;status->signature_wait--;if (status->signature_wait == 0) {// we have the whole signature, check it is OKbool sig_ok = mavlink_signature_check(status->signing, status->signing_streams, rxmsg);if (!sig_ok &&(status->signing->accept_unsigned_callback &&status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {// accepted via application level overridesig_ok = true;}if (sig_ok) {status->msg_received = MAVLINK_FRAMING_OK;} else {status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;}status->parse_state = MAVLINK_PARSE_STATE_IDLE;memcpy(r_message, rxmsg, sizeof(mavlink_message_t));}break;}bufferIndex++;// If a message has been sucessfully decoded, check indexif (status->msg_received == MAVLINK_FRAMING_OK){//while(status->current_seq != rxmsg->seq)//{//  status->packet_rx_drop_count++;//               status->current_seq++;//}status->current_rx_seq = rxmsg->seq;// Initial condition: If no packet has been received so far, drop count is undefinedif (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;// Count this packet as receivedstatus->packet_rx_success_count++;}r_message->len = rxmsg->len; // Provide visibility on how far we are into current msgr_mavlink_status->parse_state = status->parse_state;r_mavlink_status->packet_idx = status->packet_idx;r_mavlink_status->current_rx_seq = status->current_rx_seq+1;r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;r_mavlink_status->packet_rx_drop_count = status->parse_error;r_mavlink_status->flags = status->flags;status->parse_error = 0;if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) {/*the CRC came out wrong. We now need to overwrite themsg CRC with the one on the wire so that if thecaller decides to forward the message anyway thatmavlink_msg_to_send_buffer() won't overwrite thechecksum*/r_message->checksum = rxmsg->ck[0] | (rxmsg->ck[1]<<8);}return status->msg_received;
}

这里写图片描述

要想看懂上面代码，我们需要熟悉下Mavlink协议：

Mavlink协议最早由苏黎世联邦理工学院计算机视觉与几何实验组的 Lorenz Meier于2009年发布，并遵循LGPL开源协议。Mavlink协议是在串口通讯基础上的一种更高层的开源通讯协议，主要应用在微型飞行器（micro aerial vehicle）的通讯上。Mavlink是为小型飞行器和地面站（或者其他飞行器）通讯时常常用到的那些数据制定一种发送和接收的规则并加入了校验（checksum）功能。
协议以消息库的形式定义了参数传输的规则。MavLink协议支持无人固定翼飞行器、无人旋翼飞行器、无人车辆等多种类型的无人机。MAVLink协议是在CAN总线和SAE AS-4 标准的基础上设计形成的。

如图所示，每个消息帧都是上述的结构，除了灰色外，其他的格子都代表了一个字节的数据。灰色格子里面的数据长度是不固定的。

看到这再看代码是不是熟悉多了？
我们在看看这些表示什么意思：

1.红色的是起始标志位（stx），在v1.0版本中以“FE”作为起始标志。这个标志位在mavlink消息帧接收端进行消息解码时有用处。

2.第二个格子代表的是灰色部分（payload，称作有效载荷，要用的数据在有效载荷里面）的字节长度（len），范围从0到255之间。在mavlink消息帧接收端可以用它和实际收到的有效载荷的长度比较，以验证有效载荷的长度是否正确。

3.第三个格子代表的是本次消息帧的序号（seq），每次发完一个消息，这个字节的内容会加1，加到255后会从0重新开始。这个序号用于mavlink消息帧接收端计算消息丢失比例用的，相当于是信号强度。

4.第四个格子代表了发送本条消息帧的设备的系统编号（sys），使用PIXHAWK刷PX4固件时默认的系统编号为1，用于mavlink消息帧接收端识别是哪个设备发来的消息。

5.第五个格子代表了发送本条消息帧的设备的单元编号（comp），使用PIXHAWK刷PX4固件时默认的单元编号为50，用于mavlink消息帧接收端识别是设备的哪个单元发来的消息（暂时没什么用）。

6.第六个格子代表了有效载荷中消息包的编号（msg），注意它和序号是不同的，这个字节很重要，mavlink消息帧接收端要根据这个编号来确定有效载荷里到底放了什么消息包并根据编号选择对应的方式来处理有效载荷里的信息包。

7.最后两个字节是16位校验位，ckb是高八位，cka是低八位。校验码由crc16算法得到，算法将整个消息（从起始位开始到有效载荷结束，还要额外加上个MAVLINK_CRC_EXTRA字节）进行crc16计算，得出一个16位的校验码。之前提到的每种有效载荷里信息包（由消息包编号来表明是哪种消息包）会对应一个MAVLINK_CRC_EXTRA，这个 MAVLINK_CRC_EXTRA 是由生成mavlink代码的xml文件生成的，加入这个额外的东西是为了当飞行器和地面站使用不同版本的mavlink协议时，双方计算得到的校验码会不同，这样不同版本间的mavlink协议就不会在一起正常工作，避免了由于不同版本间通讯时带来的重大潜在问题。

为了方便叙述，消息包将称作包，包所代表的信息称作消息。上图中的sys将称为sysid，comp将称为compid，msg将称为msgid。

我们看下官网上的介绍：

这里写图片描述

上文中已经提到了在mavlink消息帧里最重要的两个东西，一个是msgid；一个是payload，前者是payload中内容的编号，后者则存放了消息。消息有许多种类型，在官网的网页中中以蓝色的“#”加数字的方式来表示消息的编号如 “#0”（这样的表示方法应该是为了方便在网页中查找相应编号消息的定义）。在官网介绍网页里往下拉，大概拉到二分之一的位置处，开始出现“MAVLink Messages”的介绍，往下看是各种消息的数据组成说明。**

到这里我们不再往下分析，继续看飞控代码

如果我们解析数据成功了，我们继续干嘛？

   hal.util->perf_begin(_perf_packet);packetReceived(status, msg);hal.util->perf_end(_perf_packet);parsed_packet = true;if (parsed_packet || i % 100 == 0) {//确保我们不会花太多时间解析MavLink消息---- make sure we don't spend too much time parsing mavlink messagesif (AP_HAL::micros() - tstart_us > max_time_us){break;}}

void GCS_MAVLINK_Copter::packetReceived(const mavlink_status_t &status,mavlink_message_t &msg)
{if (copter.g2.dev_options.get() & DevOptionADSBMAVLink) {// optional handling of GLOBAL_POSITION_INT as a MAVLink based avoidance sourcecopter.avoidance_adsb.handle_msg(msg);}GCS_MAVLINK::packetReceived(status, msg);
}

void GCS_MAVLINK::packetReceived(const mavlink_status_t &status,mavlink_message_t &msg)
{// we exclude radio packets to make it possible to use the// CLI over the radioif (msg.msgid != MAVLINK_MSG_ID_RADIO && msg.msgid != MAVLINK_MSG_ID_RADIO_STATUS) {mavlink_active |= (1U<<(chan-MAVLINK_COMM_0));}if (!(status.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) &&(status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) &&serialmanager_p &&serialmanager_p->get_mavlink_protocol(chan) == AP_SerialManager::SerialProtocol_MAVLink2) {// if we receive any MAVLink2 packets on a connection// currently sending MAVLink1 then switch to sending// MAVLink2mavlink_status_t *cstatus = mavlink_get_channel_status(chan);if (cstatus != nullptr) {cstatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;}}// if a snoop handler has been setup then use itif (msg_snoop != nullptr) {msg_snoop(&msg);}if (routing.check_and_forward(chan, &msg) &&accept_packet(status, msg)) {handleMessage(&msg);}
}

我们重点看这个函数：
if (routing.check_and_forward(chan, &msg) &&
accept_packet(status, msg))
{
handleMessage(&msg);
}

bool GCS_MAVLINK_Copter::accept_packet(const mavlink_status_t &status, mavlink_message_t &msg)
{if (!copter.g2.sysid_enforce) {return true;}if (msg.msgid == MAVLINK_MSG_ID_RADIO || msg.msgid == MAVLINK_MSG_ID_RADIO_STATUS) {return true;}return (msg.sysid == copter.g.sysid_my_gcs);
}

我们在来看下用户信息包处理：

void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
{uint8_t result = MAV_RESULT_FAILED;         // assume failure.  Each messages id is responsible for return ACK or NAK if requiredswitch (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 purposesif(msg->sysid != copter.g.sysid_my_gcs) break;copter.failsafe.last_heartbeat_ms = AP_HAL::millis();copter.pmTest1++;break;}case MAVLINK_MSG_ID_SET_MODE:       // MAV ID: 11{
#ifdef DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFEif (!copter.failsafe.radio) {handle_set_mode(msg, FUNCTOR_BIND(&copter, &Copter::gcs_set_mode, bool, uint8_t));} else {// don't allow mode changes while in radio failsafemavlink_msg_command_ack_send_buf(msg, chan, MAVLINK_MSG_ID_SET_MODE, MAV_RESULT_FAILED);}
#elsehandle_set_mode(msg, FUNCTOR_BIND(&copter, &Copter::gcs_set_mode, bool, uint8_t));
#endifbreak;}case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:         // MAV ID: 21{// if we have not yet initialised (including allocating the motors// object) we drop this request. That prevents the GCS from getting// a confusing parameter count during bootupif (!copter.ap.initialised_params && !AP_BoardConfig::in_sensor_config_error()) {break;}// mark the firmware version in the tlogsend_text(MAV_SEVERITY_INFO, FIRMWARE_STRING);#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)send_text(MAV_SEVERITY_INFO, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION);
#endifGCS_MAVLINK::send_statustext_chan(MAV_SEVERITY_INFO, chan, "Frame: %s", copter.get_frame_string());handle_param_request_list(msg);break;}case MAVLINK_MSG_ID_PARAM_SET:     // 23{handle_param_set(msg, &copter.DataFlash);break;}case MAVLINK_MSG_ID_PARAM_VALUE:{
#if MOUNT == ENABLEDcopter.camera_mount.handle_param_value(msg);
#endifbreak;}case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: // MAV ID: 38{handle_mission_write_partial_list(copter.mission, msg);break;}// GCS has sent us a mission item, store to EEPROMcase MAVLINK_MSG_ID_MISSION_ITEM:           // MAV ID: 39{if (handle_mission_item(msg, copter.mission)) {copter.DataFlash.Log_Write_EntireMission(copter.mission);}break;}case MAVLINK_MSG_ID_MISSION_ITEM_INT:{if (handle_mission_item(msg, copter.mission)) {copter.DataFlash.Log_Write_EntireMission(copter.mission);}break;}// read an individual command from EEPROM and send it to the GCScase MAVLINK_MSG_ID_MISSION_REQUEST_INT:case MAVLINK_MSG_ID_MISSION_REQUEST:     // MAV ID: 40, 51{handle_mission_request(copter.mission, msg);break;}case MAVLINK_MSG_ID_MISSION_SET_CURRENT:    // MAV ID: 41{handle_mission_set_current(copter.mission, msg);break;}// GCS request the full list of commands, we return just the number and leave the GCS to then request each command individuallycase MAVLINK_MSG_ID_MISSION_REQUEST_LIST:       // MAV ID: 43{handle_mission_request_list(copter.mission, msg);break;}// GCS provides the full number of commands it wishes to upload//  individual commands will then be sent from the GCS using the MAVLINK_MSG_ID_MISSION_ITEM messagecase MAVLINK_MSG_ID_MISSION_COUNT:          // MAV ID: 44{handle_mission_count(copter.mission, msg);break;}case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:      // MAV ID: 45{handle_mission_clear_all(copter.mission, msg);break;}case MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN:{mavlink_set_gps_global_origin_t packet;mavlink_msg_set_gps_global_origin_decode(msg, &packet);// sanity check locationif (!check_latlng(packet.latitude, packet.longitude)) {break;}Location ekf_origin {};ekf_origin.lat = packet.latitude;ekf_origin.lng = packet.longitude;ekf_origin.alt = packet.altitude / 10;copter.set_ekf_origin(ekf_origin);break;}case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:    // MAV ID: 66{handle_request_data_stream(msg, false);break;}case MAVLINK_MSG_ID_STATUSTEXT:{// ignore any statustext messages not from our GCS:if (msg->sysid != copter.g.sysid_my_gcs) {break;}mavlink_statustext_t packet;mavlink_msg_statustext_decode(msg, &packet);char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1+4] = { 'G','C','S',':'};memcpy(&text[4], packet.text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);copter.DataFlash.Log_Write_Message(text);break;}case MAVLINK_MSG_ID_GIMBAL_REPORT:{
#if MOUNT == ENABLEDhandle_gimbal_report(copter.camera_mount, msg);
#endifbreak;}case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:       // MAV ID: 70{// allow override of RC channel values for HIL// or for complete GCS control of switch position// and RC PWM values.if(msg->sysid != copter.g.sysid_my_gcs) break;                         // Only accept control from our gcsmavlink_rc_channels_override_t packet;int16_t v[8];mavlink_msg_rc_channels_override_decode(msg, &packet);v[0] = packet.chan1_raw;v[1] = packet.chan2_raw;v[2] = packet.chan3_raw;v[3] = packet.chan4_raw;v[4] = packet.chan5_raw;v[5] = packet.chan6_raw;v[6] = packet.chan7_raw;v[7] = packet.chan8_raw;// record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstationcopter.failsafe.rc_override_active = hal.rcin->set_overrides(v, 8);// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposescopter.failsafe.last_heartbeat_ms = AP_HAL::millis();break;}case MAVLINK_MSG_ID_MANUAL_CONTROL:{if(msg->sysid != copter.g.sysid_my_gcs) break;                         // Only accept control from our gcsmavlink_manual_control_t packet;mavlink_msg_manual_control_decode(msg, &packet);if (packet.z < 0) { // Copter doesn't do negative thrustbreak;}bool override_active = false;int16_t roll = (packet.y == INT16_MAX) ? 0 : copter.channel_roll->get_radio_min() + (copter.channel_roll->get_radio_max() - copter.channel_roll->get_radio_min()) * (packet.y + 1000) / 2000.0f;int16_t pitch = (packet.x == INT16_MAX) ? 0 : copter.channel_pitch->get_radio_min() + (copter.channel_pitch->get_radio_max() - copter.channel_pitch->get_radio_min()) * (-packet.x + 1000) / 2000.0f;int16_t throttle = (packet.z == INT16_MAX) ? 0 : copter.channel_throttle->get_radio_min() + (copter.channel_throttle->get_radio_max() - copter.channel_throttle->get_radio_min()) * (packet.z) / 1000.0f;int16_t yaw = (packet.r == INT16_MAX) ? 0 : copter.channel_yaw->get_radio_min() + (copter.channel_yaw->get_radio_max() - copter.channel_yaw->get_radio_min()) * (packet.r + 1000) / 2000.0f;override_active |= hal.rcin->set_override(uint8_t(copter.rcmap.roll() - 1), roll);override_active |= hal.rcin->set_override(uint8_t(copter.rcmap.pitch() - 1), pitch);override_active |= hal.rcin->set_override(uint8_t(copter.rcmap.throttle() - 1), throttle);override_active |= hal.rcin->set_override(uint8_t(copter.rcmap.yaw() - 1), yaw);// record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstationcopter.failsafe.rc_override_active = override_active;// a manual control message is considered to be a 'heartbeat' from the ground station for failsafe purposescopter.failsafe.last_heartbeat_ms = AP_HAL::millis();break;}case MAVLINK_MSG_ID_COMMAND_INT:{// decode packetmavlink_command_int_t packet;mavlink_msg_command_int_decode(msg, &packet);switch(packet.command){case MAV_CMD_DO_SET_ROI: {// param1 : /* Region of interest mode (not used)*/// param2 : /* MISSION index/ target ID (not used)*/// param3 : /* ROI index (not used)*/// param4 : /* empty */// x : lat// y : lon// z : alt// sanity check locationif (!check_latlng(packet.x, packet.y)) {break;}Location roi_loc;roi_loc.lat = packet.x;roi_loc.lng = packet.y;roi_loc.alt = (int32_t)(packet.z * 100.0f);copter.set_auto_yaw_roi(roi_loc);result = MAV_RESULT_ACCEPTED;break;}default:result = MAV_RESULT_UNSUPPORTED;break;}// send ACK or NAKmavlink_msg_command_ack_send_buf(msg, chan, packet.command, result);break;}// Pre-Flight calibration requestscase MAVLINK_MSG_ID_COMMAND_LONG:       // MAV ID: 76{// decode packetmavlink_command_long_t packet;mavlink_msg_command_long_decode(msg, &packet);switch(packet.command) {case MAV_CMD_START_RX_PAIR:result = handle_rc_bind(packet);break;case MAV_CMD_NAV_TAKEOFF: {// param3 : horizontal navigation by pilot acceptable// param4 : yaw angle   (not supported)// param5 : latitude    (not supported)// param6 : longitude   (not supported)// param7 : altitude [metres]float takeoff_alt = packet.param7 * 100;      // Convert m to cmif(copter.do_user_takeoff(takeoff_alt, is_zero(packet.param3))) {result = MAV_RESULT_ACCEPTED;} else {result = MAV_RESULT_FAILED;}break;}case MAV_CMD_NAV_LOITER_UNLIM:if (copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {result = MAV_RESULT_ACCEPTED;}break;case MAV_CMD_NAV_RETURN_TO_LAUNCH:if (copter.set_mode(RTL, MODE_REASON_GCS_COMMAND)) {result = MAV_RESULT_ACCEPTED;}break;case MAV_CMD_NAV_LAND:if (copter.set_mode(LAND, MODE_REASON_GCS_COMMAND)) {result = MAV_RESULT_ACCEPTED;}break;case MAV_CMD_CONDITION_YAW:// param1 : target angle [0-360]// param2 : speed during change [deg per second]// param3 : direction (-1:ccw, +1:cw)// param4 : relative offset (1) or absolute angle (0)if ((packet.param1 >= 0.0f)   &&(packet.param1 <= 360.0f) &&(is_zero(packet.param4) || is_equal(packet.param4,1.0f))) {copter.set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, is_positive(packet.param4));result = MAV_RESULT_ACCEPTED;} else {result = MAV_RESULT_FAILED;}break;case MAV_CMD_DO_CHANGE_SPEED:// param1 : unused// param2 : new speed in m/s// param3 : unused// param4 : unusedif (packet.param2 > 0.0f) {copter.wp_nav->set_speed_xy(packet.param2 * 100.0f);result = MAV_RESULT_ACCEPTED;} else {result = MAV_RESULT_FAILED;}break;case MAV_CMD_DO_SET_HOME:// param1 : use current (1=use current location, 0=use specified location)// param5 : latitude// param6 : longitude// param7 : altitude (absolute)result = MAV_RESULT_FAILED; // assume failureif(is_equal(packet.param1,1.0f) || (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7))) {if (copter.set_home_to_current_location_and_lock()) {result = MAV_RESULT_ACCEPTED;}} else {// sanity check locationif (!check_latlng(packet.param5, packet.param6)) {break;}Location new_home_loc;new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f);new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);if (copter.set_home_and_lock(new_home_loc)) {result = MAV_RESULT_ACCEPTED;}}break;case MAV_CMD_DO_FLIGHTTERMINATION:if (packet.param1 > 0.5f) {copter.init_disarm_motors();result = MAV_RESULT_ACCEPTED;}break;case MAV_CMD_DO_SET_ROI:// param1 : regional of interest mode (not supported)// param2 : mission index/ target id (not supported)// param3 : ROI index (not supported)// param5 : x / lat// param6 : y / lon// param7 : z / alt// sanity check locationif (!check_latlng(packet.param5, packet.param6)) {break;}Location roi_loc;roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f);roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f);roi_loc.alt = (int32_t)(packet.param7 * 100.0f);copter.set_auto_yaw_roi(roi_loc);result = MAV_RESULT_ACCEPTED;break;#if CAMERA == ENABLEDcase MAV_CMD_DO_DIGICAM_CONFIGURE:copter.camera.configure(packet.param1,packet.param2,packet.param3,packet.param4,packet.param5,packet.param6,packet.param7);result = MAV_RESULT_ACCEPTED;break;case MAV_CMD_DO_DIGICAM_CONTROL:if (copter.camera.control(packet.param1,packet.param2,packet.param3,packet.param4,packet.param5,packet.param6)) {copter.log_picture();}result = MAV_RESULT_ACCEPTED;break;
#endif // CAMERA == ENABLEDcase MAV_CMD_DO_MOUNT_CONTROL:
#if MOUNT == ENABLEDif(!copter.camera_mount.has_pan_control()) {copter.set_auto_yaw_look_at_heading((float)packet.param3 / 100.0f,0.0f,0,0);}copter.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7);result = MAV_RESULT_ACCEPTED;
#endifbreak;case MAV_CMD_MISSION_START:if (copter.motors->armed() && copter.set_mode(AUTO, MODE_REASON_GCS_COMMAND)) {copter.set_auto_armed(true);if (copter.mission.state() != AP_Mission::MISSION_RUNNING) {copter.mission.start_or_resume();}result = MAV_RESULT_ACCEPTED;}break;case MAV_CMD_PREFLIGHT_CALIBRATION:// exit immediately if armedif (copter.motors->armed()) {result = MAV_RESULT_FAILED;break;}if (is_equal(packet.param1,1.0f)) {if (copter.calibrate_gyros()) {result = MAV_RESULT_ACCEPTED;} else {result = MAV_RESULT_FAILED;}} else if (is_equal(packet.param3,1.0f)) {// fast barometer calibrationcopter.init_barometer(false);result = MAV_RESULT_ACCEPTED;} else if (is_equal(packet.param4,1.0f)) {result = MAV_RESULT_UNSUPPORTED;} else if (is_equal(packet.param5,1.0f)) {// 3d accel calibrationresult = MAV_RESULT_ACCEPTED;if (!copter.calibrate_gyros()) {result = MAV_RESULT_FAILED;break;}copter.ins.acal_init();copter.ins.get_acal()->start(this);} else if (is_equal(packet.param5,2.0f)) {// calibrate gyrosif (!copter.calibrate_gyros()) {result = MAV_RESULT_FAILED;break;}// accel trimfloat trim_roll, trim_pitch;if(copter.ins.calibrate_trim(trim_roll, trim_pitch)) {// reset ahrs's trim to suggested values from calibration routinecopter.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));result = MAV_RESULT_ACCEPTED;} else {result = MAV_RESULT_FAILED;}} else if (is_equal(packet.param6,1.0f)) {// compassmot calibrationresult = copter.mavlink_compassmot(chan);}break;case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:{uint8_t compassNumber = -1;if (is_equal(packet.param1, 2.0f)) {compassNumber = 0;} else if (is_equal(packet.param1, 5.0f)) {compassNumber = 1;} else if (is_equal(packet.param1, 6.0f)) {compassNumber = 2;}if (compassNumber != (uint8_t) -1) {copter.compass.set_and_save_offsets(compassNumber, packet.param2, packet.param3, packet.param4);result = MAV_RESULT_ACCEPTED;}break;}case MAV_CMD_COMPONENT_ARM_DISARM:if (is_equal(packet.param1,1.0f)) {// attempt to arm and return success or failureif (copter.init_arm_motors(true)) {result = MAV_RESULT_ACCEPTED;}} else if (is_zero(packet.param1) && (copter.ap.land_complete || is_equal(packet.param2,21196.0f)))  {// force disarming by setting param2 = 21196 is deprecatedcopter.init_disarm_motors();result = MAV_RESULT_ACCEPTED;} else {result = MAV_RESULT_UNSUPPORTED;}break;case MAV_CMD_GET_HOME_POSITION:if (copter.ap.home_state != HOME_UNSET) {send_home(copter.ahrs.get_home());Location ekf_origin;if (copter.ahrs.get_origin(ekf_origin)) {send_ekf_origin(ekf_origin);}result = MAV_RESULT_ACCEPTED;} else {result = MAV_RESULT_FAILED;}break;case MAV_CMD_DO_SET_SERVO:if (copter.ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) {result = MAV_RESULT_ACCEPTED;}break;case MAV_CMD_DO_REPEAT_SERVO:if (copter.ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000)) {result = MAV_RESULT_ACCEPTED;}break;case MAV_CMD_DO_SET_RELAY:if (copter.ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) {result = MAV_RESULT_ACCEPTED;}break;case MAV_CMD_DO_REPEAT_RELAY:if (copter.ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) {result = MAV_RESULT_ACCEPTED;}break;case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) {AP_Notify::flags.firmware_update = 1;copter.update_notify();hal.scheduler->delay(200);// when packet.param1 == 3 we reboot to hold in bootloaderhal.scheduler->reboot(is_equal(packet.param1,3.0f));result = MAV_RESULT_ACCEPTED;}break;case MAV_CMD_DO_FENCE_ENABLE:
#if AC_FENCE == ENABLEDresult = MAV_RESULT_ACCEPTED;switch ((uint16_t)packet.param1) {case 0:copter.fence.enable(false);break;case 1:copter.fence.enable(true);break;default:result = MAV_RESULT_FAILED;break;}
#else// if fence code is not included return failureresult = MAV_RESULT_FAILED;
#endifbreak;#if PARACHUTE == ENABLEDcase MAV_CMD_DO_PARACHUTE:// configure or release parachuteresult = MAV_RESULT_ACCEPTED;switch ((uint16_t)packet.param1) {case PARACHUTE_DISABLE:copter.parachute.enabled(false);copter.Log_Write_Event(DATA_PARACHUTE_DISABLED);break;case PARACHUTE_ENABLE:copter.parachute.enabled(true);copter.Log_Write_Event(DATA_PARACHUTE_ENABLED);break;case PARACHUTE_RELEASE:// treat as a manual release which performs some additional check of altitudecopter.parachute_manual_release();break;default:result = MAV_RESULT_FAILED;break;}break;
#endifcase MAV_CMD_DO_MOTOR_TEST:// param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)// param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)// param3 : throttle (range depends upon param2)// param4 : timeout (in seconds)result = copter.mavlink_motor_test_start(chan, (uint8_t)packet.param1, (uint8_t)packet.param2, (uint16_t)packet.param3, packet.param4);break;#if GRIPPER_ENABLED == ENABLEDcase MAV_CMD_DO_GRIPPER:// param1 : gripper number (ignored)// param2 : action (0=release, 1=grab). See GRIPPER_ACTIONS enum.if(!copter.g2.gripper.enabled()) {result = MAV_RESULT_FAILED;} else {result = MAV_RESULT_ACCEPTED;switch ((uint8_t)packet.param2) {case GRIPPER_ACTION_RELEASE:copter.g2.gripper.release();break;case GRIPPER_ACTION_GRAB:copter.g2.gripper.grab();break;default:result = MAV_RESULT_FAILED;break;}}break;
#endifcase MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {if (is_equal(packet.param1,1.0f)) {send_autopilot_version(FIRMWARE_VERSION);result = MAV_RESULT_ACCEPTED;}break;}case MAV_CMD_DO_START_MAG_CAL:case MAV_CMD_DO_ACCEPT_MAG_CAL:case MAV_CMD_DO_CANCEL_MAG_CAL:result = copter.compass.handle_mag_cal_command(packet);break;case MAV_CMD_DO_SEND_BANNER: {result = MAV_RESULT_ACCEPTED;send_text(MAV_SEVERITY_INFO, FIRMWARE_STRING);#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)send_text(MAV_SEVERITY_INFO, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION);#endifGCS_MAVLINK::send_statustext_chan(MAV_SEVERITY_INFO, chan, "Frame: %s", copter.get_frame_string());// send system ID if we canchar sysid[40];if (hal.util->get_system_id(sysid)) {send_text(MAV_SEVERITY_INFO, sysid);}break;}/* Solo user presses Fly button */case MAV_CMD_SOLO_BTN_FLY_CLICK: {result = MAV_RESULT_ACCEPTED;if (copter.failsafe.radio) {break;}// set mode to Loiter or fall back to AltHoldif (!copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {copter.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND);}break;}/* Solo user holds down Fly button for a couple of seconds */case MAV_CMD_SOLO_BTN_FLY_HOLD: {result = MAV_RESULT_ACCEPTED;if (copter.failsafe.radio) {break;}if (!copter.motors->armed()) {// if disarmed, arm motorscopter.init_arm_motors(true);} else if (copter.ap.land_complete) {// if armed and landed, takeoffif (copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {copter.do_user_takeoff(packet.param1*100, true);}} else {// if flying, landcopter.set_mode(LAND, MODE_REASON_GCS_COMMAND);}break;}/* Solo user presses pause button */case MAV_CMD_SOLO_BTN_PAUSE_CLICK: {result = MAV_RESULT_ACCEPTED;if (copter.failsafe.radio) {break;}if (copter.motors->armed()) {if (copter.ap.land_complete) {// if landed, disarm motorscopter.init_disarm_motors();} else {// assume that shots modes are all done in guided.// NOTE: this may need to change if we add a non-guided shot modebool shot_mode = (!is_zero(packet.param1) && (copter.control_mode == GUIDED || copter.control_mode == GUIDED_NOGPS));if (!shot_mode) {if (copter.set_mode(BRAKE, MODE_REASON_GCS_COMMAND)) {copter.brake_timeout_to_loiter_ms(2500);} else {copter.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND);}} else {// SoloLink is expected to handle pause in shots}}}break;}case MAV_CMD_ACCELCAL_VEHICLE_POS:result = MAV_RESULT_FAILED;if (copter.ins.get_acal()->gcs_vehicle_position(packet.param1)) {result = MAV_RESULT_ACCEPTED;}break;default:result = MAV_RESULT_UNSUPPORTED;break;}// send ACK or NAKmavlink_msg_command_ack_send_buf(msg, chan, packet.command, result);break;}case MAVLINK_MSG_ID_COMMAND_ACK:        // MAV ID: 77{copter.command_ack_counter++;break;}case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:   // MAV ID: 82{// decode packetmavlink_set_attitude_target_t packet;mavlink_msg_set_attitude_target_decode(msg, &packet);// exit if vehicle is not in Guided mode or Auto-Guided modeif ((copter.control_mode != GUIDED) && (copter.control_mode != GUIDED_NOGPS) && !(copter.control_mode == AUTO && copter.auto_mode == Auto_NavGuided)) {break;}// ensure type_mask specifies to use attitude and thrustif ((packet.type_mask & ((1<<7)|(1<<6))) != 0) {break;}// convert thrust to climb ratepacket.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_UPclimb_rate_cms = (packet.thrust - 0.5f) * 2.0f * copter.wp_nav->get_speed_up();} else {// descend at up to WPNAV_SPEED_DNclimb_rate_cms = (0.5f - packet.thrust) * 2.0f * -fabsf(copter.wp_nav->get_speed_down());}// if the body_yaw_rate field is ignored, use the commanded yaw position// otherwise use the commanded yaw ratebool use_yaw_rate = false;if ((packet.type_mask & (1<<2)) == 0) {use_yaw_rate = true;}copter.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]),climb_rate_cms, use_yaw_rate, packet.body_yaw_rate);break;}case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:     // MAV ID: 84{// decode packetmavlink_set_position_target_local_ned_t packet;mavlink_msg_set_position_target_local_ned_decode(msg, &packet);// exit if vehicle is not in Guided mode or Auto-Guided modeif ((copter.control_mode != GUIDED) && !(copter.control_mode == AUTO && copter.auto_mode == Auto_NavGuided)) {break;}// check for supported coordinate framesif (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&packet.coordinate_frame != MAV_FRAME_BODY_NED &&packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {break;}bool pos_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;bool vel_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;bool acc_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;bool yaw_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;/** for future use:* bool force           = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE;*/// prepare positionVector3f pos_vector;if (!pos_ignore) {// convert to cmpos_vector = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f);// rotate to body-frame if necessaryif (packet.coordinate_frame == MAV_FRAME_BODY_NED ||packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {copter.rotate_body_frame_to_NE(pos_vector.x, pos_vector.y);}// add body offset if necessaryif (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED ||packet.coordinate_frame == MAV_FRAME_BODY_NED ||packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {pos_vector += copter.inertial_nav.get_position();} else {// convert from alt-above-home to alt-above-ekf-originpos_vector.z = copter.pv_alt_above_origin(pos_vector.z);}}// prepare velocityVector3f vel_vector;if (!vel_ignore) {// convert to cmvel_vector = Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f);// rotate to body-frame if necessaryif (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {copter.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y);}}// prepare yawfloat yaw_cd = 0.0f;bool yaw_relative = false;float yaw_rate_cds = 0.0f;if (!yaw_ignore) {yaw_cd = ToDeg(packet.yaw) * 100.0f;yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;}if (!yaw_rate_ignore) {yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;}// send requestif (!pos_ignore && !vel_ignore && acc_ignore) {copter.guided_set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);} else if (pos_ignore && !vel_ignore && acc_ignore) {copter.guided_set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);} else if (!pos_ignore && vel_ignore && acc_ignore) {if (!copter.guided_set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {result = MAV_RESULT_FAILED;}} else {result = MAV_RESULT_FAILED;}break;}case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:    // MAV ID: 86{// decode packetmavlink_set_position_target_global_int_t packet;mavlink_msg_set_position_target_global_int_decode(msg, &packet);// exit if vehicle is not in Guided mode or Auto-Guided modeif ((copter.control_mode != GUIDED) && !(copter.control_mode == AUTO && copter.auto_mode == Auto_NavGuided)) {break;}// check for supported coordinate framesif (packet.coordinate_frame != MAV_FRAME_GLOBAL_INT &&packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && // solo shot manager incorrectly sends RELATIVE_ALT instead of RELATIVE_ALT_INTpacket.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT &&packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) {break;}bool pos_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;bool vel_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;bool acc_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;bool yaw_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;/** for future use:* bool force           = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE;*/Vector3f pos_ned;if(!pos_ignore) {// sanity check locationif (!check_latlng(packet.lat_int, packet.lon_int)) {result = MAV_RESULT_FAILED;break;}Location loc;loc.lat = packet.lat_int;loc.lng = packet.lon_int;loc.alt = packet.alt*100;switch (packet.coordinate_frame) {case MAV_FRAME_GLOBAL_RELATIVE_ALT: // solo shot manager incorrectly sends RELATIVE_ALT instead of RELATIVE_ALT_INTcase MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:loc.flags.relative_alt = true;loc.flags.terrain_alt = false;break;case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:loc.flags.relative_alt = true;loc.flags.terrain_alt = true;break;case MAV_FRAME_GLOBAL_INT:default:// Copter does not support navigation to absolute altitudes. This convert the WGS84 altitude// to a home-relative altitude before passing it to the navigation controllerloc.alt -= copter.ahrs.get_home().alt;loc.flags.relative_alt = true;loc.flags.terrain_alt = false;break;}pos_ned = copter.pv_location_to_vector(loc);}// prepare yawfloat yaw_cd = 0.0f;bool yaw_relative = false;float yaw_rate_cds = 0.0f;if (!yaw_ignore) {yaw_cd = ToDeg(packet.yaw) * 100.0f;yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;}if (!yaw_rate_ignore) {yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;}if (!pos_ignore && !vel_ignore && acc_ignore) {copter.guided_set_destination_posvel(pos_ned, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);} else if (pos_ignore && !vel_ignore && acc_ignore) {copter.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);} else if (!pos_ignore && vel_ignore && acc_ignore) {if (!copter.guided_set_destination(pos_ned, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {result = MAV_RESULT_FAILED;}} else {result = MAV_RESULT_FAILED;}break;}case MAVLINK_MSG_ID_DISTANCE_SENSOR:{result = MAV_RESULT_ACCEPTED;copter.rangefinder.handle_msg(msg);
#if PROXIMITY_ENABLED == ENABLEDcopter.g2.proximity.handle_msg(msg);
#endifbreak;}case MAVLINK_MSG_ID_GPS_RTCM_DATA:case MAVLINK_MSG_ID_GPS_INPUT:case MAVLINK_MSG_ID_HIL_GPS:{result = MAV_RESULT_ACCEPTED;copter.gps.handle_msg(msg);break;}#if HIL_MODE != HIL_MODE_DISABLEDcase MAVLINK_MSG_ID_HIL_STATE:          // MAV ID: 90{mavlink_hil_state_t packet;mavlink_msg_hil_state_decode(msg, &packet);// sanity check locationif (!check_latlng(packet.lat, packet.lon)) {break;}// set gps hil sensorLocation loc;loc.lat = packet.lat;loc.lng = packet.lon;loc.alt = packet.alt/10;Vector3f vel(packet.vx, packet.vy, packet.vz);vel *= 0.01f;gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D,packet.time_usec/1000,loc, vel, 10, 0);// rad/secVector3f gyros;gyros.x = packet.rollspeed;gyros.y = packet.pitchspeed;gyros.z = packet.yawspeed;// m/s/sVector3f accels;accels.x = packet.xacc * (GRAVITY_MSS/1000.0f);accels.y = packet.yacc * (GRAVITY_MSS/1000.0f);accels.z = packet.zacc * (GRAVITY_MSS/1000.0f);ins.set_gyro(0, gyros);ins.set_accel(0, accels);copter.barometer.setHIL(packet.alt*0.001f);copter.compass.setHIL(0, packet.roll, packet.pitch, packet.yaw);copter.compass.setHIL(1, packet.roll, packet.pitch, packet.yaw);break;}
#endif //  HIL_MODE != HIL_MODE_DISABLEDcase MAVLINK_MSG_ID_RADIO:case MAVLINK_MSG_ID_RADIO_STATUS:       // MAV ID: 109{handle_radio_status(msg, copter.DataFlash, copter.should_log(MASK_LOG_PM));break;}case MAVLINK_MSG_ID_LOG_REQUEST_DATA:copter.in_log_download = true;/* no break */case MAVLINK_MSG_ID_LOG_ERASE:/* no break */case MAVLINK_MSG_ID_LOG_REQUEST_LIST:if (!copter.in_mavlink_delay && !copter.motors->armed()) {handle_log_message(msg, copter.DataFlash);}break;case MAVLINK_MSG_ID_LOG_REQUEST_END:copter.in_log_download = false;if (!copter.in_mavlink_delay && !copter.motors->armed()) {handle_log_message(msg, copter.DataFlash);}break;case MAVLINK_MSG_ID_SERIAL_CONTROL:handle_serial_control(msg, copter.gps);break;case MAVLINK_MSG_ID_GPS_INJECT_DATA:handle_gps_inject(msg, copter.gps);result = MAV_RESULT_ACCEPTED;break;#if PRECISION_LANDING == ENABLEDcase MAVLINK_MSG_ID_LANDING_TARGET:result = MAV_RESULT_ACCEPTED;copter.precland.handle_msg(msg);break;
#endif#if AC_FENCE == ENABLED// send or receive fence points with GCScase MAVLINK_MSG_ID_FENCE_POINT:            // MAV ID: 160case MAVLINK_MSG_ID_FENCE_FETCH_POINT:copter.fence.handle_msg(chan, msg);break;
#endif // AC_FENCE == ENABLED#if CAMERA == ENABLED//deprecated.  Use MAV_CMD_DO_DIGICAM_CONFIGUREcase MAVLINK_MSG_ID_DIGICAM_CONFIGURE:      // MAV ID: 202break;//deprecated.  Use MAV_CMD_DO_DIGICAM_CONTROLcase MAVLINK_MSG_ID_DIGICAM_CONTROL:copter.camera.control_msg(msg);copter.log_picture();break;
#endif // CAMERA == ENABLED#if MOUNT == ENABLED//deprecated. Use MAV_CMD_DO_MOUNT_CONFIGUREcase MAVLINK_MSG_ID_MOUNT_CONFIGURE:        // MAV ID: 204copter.camera_mount.configure_msg(msg);break;//deprecated. Use MAV_CMD_DO_MOUNT_CONTROLcase MAVLINK_MSG_ID_MOUNT_CONTROL:if(!copter.camera_mount.has_pan_control()) {copter.set_auto_yaw_look_at_heading(mavlink_msg_mount_control_get_input_c(msg)/100.0f, 0.0f, 0, 0);}copter.camera_mount.control_msg(msg);break;
#endif // MOUNT == ENABLEDcase MAVLINK_MSG_ID_TERRAIN_DATA:case MAVLINK_MSG_ID_TERRAIN_CHECK:
#if AP_TERRAIN_AVAILABLE && AC_TERRAINcopter.terrain.handle_data(chan, msg);
#endifbreak;#if AC_RALLY == ENABLED// receive a rally point from GCS and store in EEPROMcase MAVLINK_MSG_ID_RALLY_POINT: {mavlink_rally_point_t packet;mavlink_msg_rally_point_decode(msg, &packet);if (packet.idx >= copter.rally.get_rally_total() ||packet.idx >= copter.rally.get_rally_max()) {send_text(MAV_SEVERITY_NOTICE,"Bad rally point message ID");break;}if (packet.count != copter.rally.get_rally_total()) {send_text(MAV_SEVERITY_NOTICE,"Bad rally point message count");break;}// sanity check locationif (!check_latlng(packet.lat, packet.lng)) {break;}RallyLocation rally_point;rally_point.lat = packet.lat;rally_point.lng = packet.lng;rally_point.alt = packet.alt;rally_point.break_alt = packet.break_alt;rally_point.land_dir = packet.land_dir;rally_point.flags = packet.flags;if (!copter.rally.set_rally_point_with_index(packet.idx, rally_point)) {send_text(MAV_SEVERITY_CRITICAL, "Error setting rally point");}break;}//send a rally point to the GCScase MAVLINK_MSG_ID_RALLY_FETCH_POINT: {mavlink_rally_fetch_point_t packet;mavlink_msg_rally_fetch_point_decode(msg, &packet);if (packet.idx > copter.rally.get_rally_total()) {send_text(MAV_SEVERITY_NOTICE, "Bad rally point index");break;}RallyLocation rally_point;if (!copter.rally.get_rally_point_with_index(packet.idx, rally_point)) {send_text(MAV_SEVERITY_NOTICE, "Failed to set rally point");break;}mavlink_msg_rally_point_send_buf(msg,chan, msg->sysid, msg->compid, packet.idx,copter.rally.get_rally_total(), rally_point.lat, rally_point.lng,rally_point.alt, rally_point.break_alt, rally_point.land_dir,rally_point.flags);break;}
#endif // AC_RALLY == ENABLEDcase MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS:copter.DataFlash.remote_log_block_status_msg(chan, msg);break;case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST:send_autopilot_version(FIRMWARE_VERSION);break;case MAVLINK_MSG_ID_LED_CONTROL:// send message to NotifyAP_Notify::handle_led_control(msg);break;case MAVLINK_MSG_ID_PLAY_TUNE:// send message to NotifyAP_Notify::handle_play_tune(msg);break;case MAVLINK_MSG_ID_SET_HOME_POSITION:{mavlink_set_home_position_t packet;mavlink_msg_set_home_position_decode(msg, &packet);if((packet.latitude == 0) && (packet.longitude == 0) && (packet.altitude == 0)) {copter.set_home_to_current_location_and_lock();} else {// sanity check locationif (!check_latlng(packet.latitude, packet.longitude)) {break;}Location new_home_loc;new_home_loc.lat = packet.latitude;new_home_loc.lng = packet.longitude;new_home_loc.alt = packet.altitude / 10;copter.set_home_and_lock(new_home_loc);}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:
#if ADSB_ENABLED == ENABLEDcopter.adsb.handle_message(chan, msg);
#endifbreak;case MAVLINK_MSG_ID_VISION_POSITION_DELTA:
#if VISUAL_ODOMETRY_ENABLED == ENABLEDcopter.g2.visual_odom.handle_msg(msg);
#endifbreak;default:handle_common_message(msg);break;}     // end switch
} // end handle mavlink

到这里可以看到这个代码很长，主要就是解析地面站发送给飞控的信息，
再往后看就是处理接收信息是否超时的处理。

 if (!waypoint_receiving){hal.util->perf_end(_perf_update);    return;}uint32_t tnow = AP_HAL::millis();uint32_t wp_recv_time = 1000U + (stream_slowdown*20);// stop waypoint receiving if timeoutif (waypoint_receiving && (tnow - waypoint_timelast_receive) > wp_recv_time+waypoint_receive_timeout){waypoint_receiving = false;} else if (waypoint_receiving &&(tnow - waypoint_timelast_request) > wp_recv_time){waypoint_timelast_request = tnow;send_message(MSG_NEXT_WAYPOINT);}hal.util->perf_end(_perf_update);

这些代码就不讲解，后续继续研究

（2）SCHED_TASK(gcs_send_heartbeat, 1, 110), //发送心跳包

void Copter::gcs_send_heartbeat(void)
{gcs_send_message(MSG_HEARTBEAT); //心跳包
}

void Copter::gcs_send_message(enum ap_message id)
{for (uint8_t i=0; i<num_gcs; i++){if (gcs_chan[i].initialised) //初始化完成，进行{gcs_chan[i].send_message(id);}}
}

void GCS_MAVLINK::send_message(enum ap_message id)
{uint8_t i, nextid;if (id == MSG_HEARTBEAT) //心跳{save_signing_timestamp(false); //定期保存时间信息}// see if we can send the deferred messages, if anywhile (num_deferred_messages != 0){if (!try_send_message(deferred_messages[next_deferred_message])){break;}next_deferred_message++;if (next_deferred_message == MSG_RETRY_DEFERRED){next_deferred_message = 0;}num_deferred_messages--;}if (id == MSG_RETRY_DEFERRED){return;}//此消息ID可能已经被推迟。---- this message id might already be deferredfor (i=0, nextid = next_deferred_message; i < num_deferred_messages; i++){if (deferred_messages[nextid] == id) {// it's already deferred, discardreturn;}nextid++;if (nextid == MSG_RETRY_DEFERRED){nextid = 0;}}if (num_deferred_messages != 0 ||!try_send_message(id)) {// can't send it now, so defer itif (num_deferred_messages == MSG_RETRY_DEFERRED) {// the defer buffer is full, discardreturn;}nextid = next_deferred_message + num_deferred_messages;if (nextid >= MSG_RETRY_DEFERRED) {nextid -= MSG_RETRY_DEFERRED;}deferred_messages[nextid] = id;num_deferred_messages++;}
}

到这里有个比较重要的函数，那就是：

bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
{if (telemetry_delayed(chan)) {return false;}#if HIL_MODE != HIL_MODE_SENSORS// if we don't have at least 250 micros remaining before the main loop// wants to fire then don't send a mavlink message. We want to// prioritise the main flight control loop over communications// the check for nullptr here doesn't just save a nullptr// dereference; it means that we send messages out even if we're// failing to detect a PX4 board type (see delay(3000) in px_drivers).//telemetry_delayed(), 在启动时，不能不能立刻发送数据包，因为xbee的启动时间延后。//同时，为了保证系统的实时性，保证main_loop 最高的优先级，只有当剩余时间大于250ms 时才执行发送命令操作。if (copter.motors != nullptr && copter.scheduler.time_available_usec() < 250 && copter.motors->armed()){copter.gcs_out_of_time = true;return false;}
#endifswitch(id){case MSG_HEARTBEAT:CHECK_PAYLOAD_SIZE(HEARTBEAT);last_heartbeat_time = AP_HAL::millis();copter.send_heartbeat(chan); //心跳包break;case MSG_EXTENDED_STATUS1:// send extended status only once vehicle has been initialised// to avoid unnecessary errors being reported to userif (copter.ap.initialised){CHECK_PAYLOAD_SIZE(SYS_STATUS);copter.send_extended_status1(chan);CHECK_PAYLOAD_SIZE(POWER_STATUS);send_power_status();}break;case MSG_EXTENDED_STATUS2:CHECK_PAYLOAD_SIZE(MEMINFO);send_meminfo();break;case MSG_ATTITUDE:CHECK_PAYLOAD_SIZE(ATTITUDE);copter.send_attitude(chan);break;case MSG_LOCATION:CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);copter.send_location(chan);break;case MSG_LOCAL_POSITION:CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED);send_local_position(copter.ahrs);break;case MSG_NAV_CONTROLLER_OUTPUT:CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);copter.send_nav_controller_output(chan);break;case MSG_GPS_RAW:return send_gps_raw(copter.gps);case MSG_SYSTEM_TIME:CHECK_PAYLOAD_SIZE(SYSTEM_TIME);send_system_time(copter.gps);break;case MSG_RADIO_IN:CHECK_PAYLOAD_SIZE(RC_CHANNELS);send_radio_in(copter.receiver_rssi);break;case MSG_SERVO_OUTPUT_RAW:CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);send_servo_output_raw(false);break;case MSG_VFR_HUD:CHECK_PAYLOAD_SIZE(VFR_HUD);copter.send_vfr_hud(chan);break;case MSG_RAW_IMU1:CHECK_PAYLOAD_SIZE(RAW_IMU);send_raw_imu(copter.ins, copter.compass);break;case MSG_RAW_IMU2:CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);send_scaled_pressure(copter.barometer);break;case MSG_RAW_IMU3:CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);send_sensor_offsets(copter.ins, copter.compass, copter.barometer);break;case MSG_CURRENT_WAYPOINT:CHECK_PAYLOAD_SIZE(MISSION_CURRENT);copter.send_current_waypoint(chan);break;case MSG_NEXT_PARAM:CHECK_PAYLOAD_SIZE(PARAM_VALUE);queued_param_send();break;case MSG_NEXT_WAYPOINT:CHECK_PAYLOAD_SIZE(MISSION_REQUEST);queued_waypoint_send();break;case MSG_RANGEFINDER:
#if RANGEFINDER_ENABLED == ENABLEDCHECK_PAYLOAD_SIZE(RANGEFINDER);copter.send_rangefinder(chan);
#endifCHECK_PAYLOAD_SIZE(DISTANCE_SENSOR);copter.send_proximity(chan, comm_get_txspace(chan) / (packet_overhead()+9));break;case MSG_RPM:CHECK_PAYLOAD_SIZE(RPM);copter.send_rpm(chan);break;case MSG_TERRAIN:
#if AP_TERRAIN_AVAILABLE && AC_TERRAINCHECK_PAYLOAD_SIZE(TERRAIN_REQUEST);copter.terrain.send_request(chan);
#endifbreak;case MSG_CAMERA_FEEDBACK:
#if CAMERA == ENABLEDCHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK);copter.camera.send_feedback(chan, copter.gps, copter.ahrs, copter.current_loc);
#endifbreak;case MSG_STATUSTEXT:// depreciated, use GCS_MAVLINK::send_statustext*return false;case MSG_FENCE_STATUS:
#if AC_FENCE == ENABLEDCHECK_PAYLOAD_SIZE(FENCE_STATUS);copter.send_fence_status(chan);
#endifbreak;case MSG_AHRS:CHECK_PAYLOAD_SIZE(AHRS);send_ahrs(copter.ahrs);break;case MSG_SIMSTATE:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITLCHECK_PAYLOAD_SIZE(SIMSTATE);copter.send_simstate(chan);
#endifCHECK_PAYLOAD_SIZE(AHRS2);send_ahrs2(copter.ahrs);break;case MSG_HWSTATUS:CHECK_PAYLOAD_SIZE(HWSTATUS);copter.send_hwstatus(chan);break;case MSG_MOUNT_STATUS:
#if MOUNT == ENABLEDCHECK_PAYLOAD_SIZE(MOUNT_STATUS);    copter.camera_mount.status_msg(chan);
#endif // MOUNT == ENABLEDbreak;case MSG_BATTERY2:CHECK_PAYLOAD_SIZE(BATTERY2);send_battery2(copter.battery);break;case MSG_OPTICAL_FLOW:
#if OPTFLOW == ENABLEDCHECK_PAYLOAD_SIZE(OPTICAL_FLOW);send_opticalflow(copter.ahrs, copter.optflow);
#endifbreak;case MSG_GIMBAL_REPORT:
#if MOUNT == ENABLEDCHECK_PAYLOAD_SIZE(GIMBAL_REPORT);copter.camera_mount.send_gimbal_report(chan);
#endifbreak;case MSG_EKF_STATUS_REPORT:CHECK_PAYLOAD_SIZE(EKF_STATUS_REPORT);copter.ahrs.send_ekf_status_report(chan);break;case MSG_LIMITS_STATUS:case MSG_WIND:case MSG_POSITION_TARGET_GLOBAL_INT:case MSG_SERVO_OUT:case MSG_AOA_SSA:// unusedbreak;case MSG_PID_TUNING:CHECK_PAYLOAD_SIZE(PID_TUNING);copter.send_pid_tuning(chan);break;case MSG_VIBRATION:CHECK_PAYLOAD_SIZE(VIBRATION);send_vibration(copter.ins);break;case MSG_MISSION_ITEM_REACHED:CHECK_PAYLOAD_SIZE(MISSION_ITEM_REACHED);mavlink_msg_mission_item_reached_send(chan, mission_item_reached_index);break;case MSG_RETRY_DEFERRED:break; // just here to prevent a warningcase MSG_MAG_CAL_PROGRESS:copter.compass.send_mag_cal_progress(chan);break;case MSG_MAG_CAL_REPORT:copter.compass.send_mag_cal_report(chan);break;case MSG_ADSB_VEHICLE:CHECK_PAYLOAD_SIZE(ADSB_VEHICLE);copter.adsb.send_adsb_vehicle(chan);break;case MSG_BATTERY_STATUS:send_battery_status(copter.battery);break;}return true;
}

这里会有飞控所有相关的信息
我们看下关心的心跳包数据

NOINLINE void Copter::send_heartbeat(mavlink_channel_t chan)
{uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;uint8_t system_status = ap.land_complete ? MAV_STATE_STANDBY : MAV_STATE_ACTIVE;uint32_t custom_mode = control_mode;// set system as critical if any failsafe have triggeredif (failsafe.radio || failsafe.battery || failsafe.gcs || failsafe.ekf || failsafe.terrain || failsafe.adsb)  {system_status = MAV_STATE_CRITICAL;}// work out the base_mode. This value is not very useful// for APM, but we calculate it as best we can so a generic// MAVLink enabled ground station can work out something about// what the MAV is up to. The actual bit values are highly// ambiguous for most of the APM flight modes. In practice, you// only get useful information from the custom_mode, which maps to// the APM flight mode and has a well defined meaning in the// ArduPlane documentationbase_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;switch (control_mode) {case AUTO:case RTL:case LOITER:case AVOID_ADSB:case GUIDED:case CIRCLE:case POSHOLD:case BRAKE:base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;// note that MAV_MODE_FLAG_AUTO_ENABLED does not match what// APM does in any mode, as that is defined as "system finds its own goal// positions", which APM does not currently dobreak;default:break;}// all modes except INITIALISING have some form of manual// override if stick mixing is enabledbase_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;#if HIL_MODE != HIL_MODE_DISABLEDbase_mode |= MAV_MODE_FLAG_HIL_ENABLED;
#endif// we are armed if we are not initialisingif (motors->armed()) {base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;}// indicate we have set a custom modebase_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;gcs_chan[chan-MAVLINK_COMM_0].send_heartbeat(get_frame_mav_type(),base_mode,custom_mode,system_status);
}

void GCS_MAVLINK::send_heartbeat(uint8_t type, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status)
{mavlink_msg_heartbeat_send(chan,type,MAV_AUTOPILOT_ARDUPILOTMEGA,base_mode,custom_mode,system_status);
}

static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDSchar buf[MAVLINK_MSG_ID_HEARTBEAT_LEN];_mav_put_uint32_t(buf, 0, custom_mode);_mav_put_uint8_t(buf, 4, type);_mav_put_uint8_t(buf, 5, autopilot);_mav_put_uint8_t(buf, 6, base_mode);_mav_put_uint8_t(buf, 7, system_status);_mav_put_uint8_t(buf, 8, 3);_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
#elsemavlink_heartbeat_t packet;packet.custom_mode = custom_mode;packet.type = type;packet.autopilot = autopilot;packet.base_mode = base_mode;packet.system_status = system_status;packet.mavlink_version = 3;_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
#endif
}

MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint32_t msgid,const char *packet, uint8_t min_length, uint8_t length, uint8_t crc_extra)
{uint16_t checksum;uint8_t buf[MAVLINK_NUM_HEADER_BYTES];uint8_t ck[2];mavlink_status_t *status = mavlink_get_channel_status(chan);uint8_t header_len = MAVLINK_CORE_HEADER_LEN;uint8_t signature_len = 0;uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN];bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0;bool signing =  (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING);if (mavlink1) {length = min_length;if (msgid > 255) {// can't send 16 bit messages_mav_parse_error(status);return;}header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN;buf[0] = MAVLINK_STX_MAVLINK1;buf[1] = length;buf[2] = status->current_tx_seq;buf[3] = mavlink_system.sysid;buf[4] = mavlink_system.compid;buf[5] = msgid & 0xFF;} else {uint8_t incompat_flags = 0;if (signing) {incompat_flags |= MAVLINK_IFLAG_SIGNED;}length = _mav_trim_payload(packet, length);buf[0] = MAVLINK_STX;buf[1] = length;buf[2] = incompat_flags;buf[3] = 0; // compat_flagsbuf[4] = status->current_tx_seq;buf[5] = mavlink_system.sysid;buf[6] = mavlink_system.compid;buf[7] = msgid & 0xFF;buf[8] = (msgid >> 8) & 0xFF;buf[9] = (msgid >> 16) & 0xFF;}status->current_tx_seq++;checksum = crc_calculate((const uint8_t*)&buf[1], header_len);crc_accumulate_buffer(&checksum, packet, length);crc_accumulate(crc_extra, &checksum);ck[0] = (uint8_t)(checksum & 0xFF);ck[1] = (uint8_t)(checksum >> 8);if (signing) {// possibly add a signaturesignature_len = mavlink_sign_packet(status->signing, signature, buf, header_len+1,(const uint8_t *)packet, length, ck);}MAVLINK_START_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len);_mavlink_send_uart(chan, (const char *)buf, header_len+1);_mavlink_send_uart(chan, packet, length);_mavlink_send_uart(chan, (const char *)ck, 2);if (signature_len != 0) {_mavlink_send_uart(chan, (const char *)signature, signature_len);}MAVLINK_END_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len);
}

到这里我们可以看到所有的数据都是通过串口发出去

MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
{
#ifdef MAVLINK_SEND_UART_BYTES/* this is the more efficient approach, if the platformdefines it */MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len);
#else/* fallback to one byte at a time */uint16_t i;for (i = 0; i < len; i++) {comm_send_ch(chan, (uint8_t)buf[i]);}
#endif
}

（3） SCHED_TASK(gcs_send_deferred, 50, 550), //发送队列消息，statustext和data_stream

void Copter::gcs_send_deferred(void)
{gcs_send_message(MSG_RETRY_DEFERRED); //最后一条消息信息，队列结束gcs().service_statustext(); //发送特性ID
}

void GCS::service_statustext(void)
{// create bitmask of what mavlink ports we should send this text to.// note, if sending to all ports, we only need to store the bitmask for each and the string only once.// once we send over a link, clear the port but other busy ports bit may stay allowing for faster links// to clear the bit and send quickly but slower links to still store the string. Regardless of mixed// bitrates of ports, a maximum of _status_capacity strings can be buffered. Downside// is if you have a super slow link mixed with a faster port, if there are _status_capacity// strings in the slow queue then the next item can not be queued for the faster linkif (_statustext_queue.empty()) {// nothing to doreturn;}for (uint8_t idx=0; idx<_status_capacity; ) {statustext_t *statustext = _statustext_queue[idx];if (statustext == nullptr) {break;}// try and send to all active mavlink ports listed in the statustext.bitmaskfor (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {uint8_t chan_bit = (1U<<i);// logical AND (&) to mask them togetherif (statustext->bitmask & chan_bit) {// something is queued on a port and that's the port index we're looped atmavlink_channel_t chan_index = (mavlink_channel_t)(MAVLINK_COMM_0+i);if (HAVE_PAYLOAD_SPACE(chan_index, STATUSTEXT)) {// we have space so send then clear that channel bit on the maskmavlink_msg_statustext_send(chan_index, statustext->msg.severity, statustext->msg.text);statustext->bitmask &= ~chan_bit;}}}if (statustext->bitmask == 0) {_statustext_queue.remove(idx);} else {// move to next indexidx++;}}
}

这里还没分析，看的网上的：从_statustext_queue中查找是否有数据，如果有数据，从中取出数据。_statustext_queue 定义了一个模板，这个模板，是一个环数据结构。在通信过程中，经常用到。自己写传感器数据，或者对传感器数据进行滤波处理，经常用到ringbuffer.

（4） SCHED_TASK(gcs_data_stream_send, 50, 550), //发送数据流

void Copter::gcs_data_stream_send(void)
{for (uint8_t i=0; i<num_gcs; i++){if (gcs_chan[i].initialised){gcs_chan[i].data_stream_send(); //发送数据流}}
}

void GCS_MAVLINK_Copter::data_stream_send(void)
{if (waypoint_receiving){//不要干扰任务迁移-----don't interfere with mission transferreturn;}if (!copter.in_mavlink_delay && !copter.motors->armed()){handle_log_send(copter.DataFlash);}copter.gcs_out_of_time = false;send_queued_parameters();if (copter.gcs_out_of_time) return;if (copter.in_mavlink_delay) {// don't send any other stream types while in the delay callbackreturn;}if (stream_trigger(STREAM_RAW_SENSORS)){send_message(MSG_RAW_IMU1);  // RAW_IMU, SCALED_IMU2, SCALED_IMU3send_message(MSG_RAW_IMU2);  // SCALED_PRESSURE, SCALED_PRESSURE2, SCALED_PRESSURE3send_message(MSG_RAW_IMU3);  // SENSOR_OFFSETS}if (copter.gcs_out_of_time) return;if (stream_trigger(STREAM_EXTENDED_STATUS)){send_message(MSG_EXTENDED_STATUS1); // SYS_STATUS, POWER_STATUSsend_message(MSG_EXTENDED_STATUS2); // MEMINFOsend_message(MSG_CURRENT_WAYPOINT);send_message(MSG_GPS_RAW);send_message(MSG_NAV_CONTROLLER_OUTPUT);send_message(MSG_FENCE_STATUS);}if (copter.gcs_out_of_time) return;if (stream_trigger(STREAM_POSITION)){send_message(MSG_LOCATION);send_message(MSG_LOCAL_POSITION);}if (copter.gcs_out_of_time) return;if (stream_trigger(STREAM_RAW_CONTROLLER)){send_message(MSG_SERVO_OUT);}if (copter.gcs_out_of_time) return;if (stream_trigger(STREAM_RC_CHANNELS)){send_message(MSG_SERVO_OUTPUT_RAW);send_message(MSG_RADIO_IN);}if (copter.gcs_out_of_time) return;if (stream_trigger(STREAM_EXTRA1)){send_message(MSG_ATTITUDE);send_message(MSG_SIMSTATE); // SIMSTATE, AHRS2send_message(MSG_PID_TUNING);}if (copter.gcs_out_of_time) return;if (stream_trigger(STREAM_EXTRA2)){send_message(MSG_VFR_HUD);}if (copter.gcs_out_of_time) return;if (stream_trigger(STREAM_EXTRA3)){send_message(MSG_AHRS);send_message(MSG_HWSTATUS);send_message(MSG_SYSTEM_TIME);send_message(MSG_RANGEFINDER);
#if AP_TERRAIN_AVAILABLE && AC_TERRAINsend_message(MSG_TERRAIN);
#endifsend_message(MSG_BATTERY2);send_message(MSG_BATTERY_STATUS);send_message(MSG_MOUNT_STATUS);send_message(MSG_OPTICAL_FLOW);send_message(MSG_GIMBAL_REPORT);send_message(MSG_MAG_CAL_REPORT);send_message(MSG_MAG_CAL_PROGRESS);send_message(MSG_EKF_STATUS_REPORT);send_message(MSG_VIBRATION);send_message(MSG_RPM);}if (copter.gcs_out_of_time) return;if (stream_trigger(STREAM_ADSB)){send_message(MSG_ADSB_VEHICLE);}
}


void GCS_MAVLINK::send_message(enum ap_message id)
{uint8_t i, nextid;if (id == MSG_HEARTBEAT) //心跳{save_signing_timestamp(false); //定期保存时间信息}//看看我们是否可以发送延迟消息，如果有的话---- see if we can send the deferred messages, if anywhile (num_deferred_messages != 0){if (!try_send_message(deferred_messages[next_deferred_message])){break;}next_deferred_message++;if (next_deferred_message == MSG_RETRY_DEFERRED){next_deferred_message = 0;}num_deferred_messages--;}if (id == MSG_RETRY_DEFERRED){return;}//此消息ID可能已经被推迟。---- this message id might already be deferredfor (i=0, nextid = next_deferred_message; i < num_deferred_messages; i++){if (deferred_messages[nextid] == id) {// it's already deferred, discardreturn;}nextid++;if (nextid == MSG_RETRY_DEFERRED){nextid = 0;}}if (num_deferred_messages != 0 ||!try_send_message(id)) {// can't send it now, so defer itif (num_deferred_messages == MSG_RETRY_DEFERRED) {// the defer buffer is full, discardreturn;}nextid = next_deferred_message + num_deferred_messages;if (nextid >= MSG_RETRY_DEFERRED) {nextid -= MSG_RETRY_DEFERRED;}deferred_messages[nextid] = id;num_deferred_messages++;}
}

这篇关于Ardupilot飞控Mavlink代码学习的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！