Ardupilot飞控Mavlink代码学习
本文共 93185 字，大约阅读时间需要 310 分钟。
摘自：
Ardupilot飞控Mavlink代码学习
2018-07-05 23:36:33 4302
分类专栏：
版权
目录
摘要
本节主要记录自己学习Ardupilot的Mavlink协议的过程，欢迎一起交流分析！
1.Ardupilot怎么实现Mavlink初始化
可以看出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 port AP_HAL::UARTDriver *uart; uart = serial_manager.find_serial(protocol, instance); //寻找有没有串口 if (uart == nullptr) { //如果没有找到串口立即返回--------return immediately if not found return; } //获取Mavlink协议通道---------get associated mavlink channel mavlink_channel_t mav_chan; if (!serial_manager.get_mavlink_channel(protocol, instance, mav_chan)) { //没找到就返回--------return immediately in unlikely case mavlink channel cannot be found return; } /* Now try to cope with SiK radios that may be stuck in bootloader mode because CTS was held while powering on. This tells the bootloader to wait for a firmware. It affects any SiK radio with CTS connected that is externally powered. To cope we send 0x30 0x20 at 115200 on startup, which tells the bootloader to reset and boot normally 现在试着处理可能在Bootloader中插入的SIK收音机。模式，因为CTS在供电时被保持。这告诉了等待加载固件的Bootloader。它影响任何SIK收音机 CTS连接的是外部供电。为了应付我们发送0x3 0x20在启动时115200，它告诉引导加载程序重置正常启动 */ uart->begin(115200); //设置波特率115200 AP_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 baudrate uart->begin(serial_manager.find_baudrate(protocol, instance)); //串口初始化and init the gcs instance init(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 key load_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 capable hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MAVLINK2); } else if (status) { // user has asked to only send MAVLink1 status->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 MAVLink2 status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; }} - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
该函数大体的作用：判断有没有串口，已经串口通道，设置串口波特率，初始化串口协议，判断是否使用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"); }} - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
在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;} - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
这个函数是：一个处理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);}
- 1
- 2
- 3
- 4
到这里初始化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_stream SCHED_TASK(gcs_data_stream_send, 50, 550), //发送数据流
- 1
- 2
- 3
- 4
（1）首先分析SCHED_TASK(gcs_check_input, 400, 180), //检查串口数据，对串口数据解析，解包操作
void GCS_MAVLINK::update(run_cli_fn run_cli, uint32_t max_time_us){ // receive new packets mavlink_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 bytes uint16_t nbytes = comm_get_available(chan); //接收串口数据 for (uint16_t i=0; i 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 messages if (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 timeout if (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
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
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 failure mavlink_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;} - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
**这是一个处理完整的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);} - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
继续看代码:
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 transmission medium is prone to missing (or extra) characters (e.g. a radio that fades in and out). Only use if the channel will only contain messages types listed in the headers. */#ifdef MAVLINK_CHECK_MESSAGE_LENGTH#ifndef MAVLINK_MESSAGE_LENGTH static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;#define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid]#endif#endif int 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 the default 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 CRC2 rxmsg->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_LENGTH if (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_LENGTH if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(rxmsg->msgid)) { _mav_parse_error(status); status->parse_state = MAVLINK_PARSE_STATE_IDLE; break; }#endif break; 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 packets if (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 message status->msg_received = MAVLINK_FRAMING_BAD_CRC; } else { // Successfully got message status->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 OK bool 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 override sig_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 index if (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 undefined if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; // Count this packet as received status->packet_rx_success_count++; } r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg r_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 the msg CRC with the one on the wire so that if the caller decides to forward the message anyway that mavlink_msg_to_send_buffer() won't overwrite the checksum */ 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 messages if (AP_HAL::micros() - tstart_us > max_time_us) { break; } } - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
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 source copter.avoidance_adsb.handle_msg(msg); } GCS_MAVLINK::packetReceived(status, msg);} - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
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 radio if (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 // MAVLink2 mavlink_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 it if (msg_snoop != nullptr) { msg_snoop(&msg); } if (routing.check_and_forward(chan, &msg) && accept_packet(status, msg)) { handleMessage(&msg); }} - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
我们重点看这个函数：
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);} - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
我们在来看下用户信息包处理：
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 required switch (msg->msgid) { case MAVLINK_MSG_ID_HEARTBEAT: // MAV ID: 0 { // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes if(msg->sysid != 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_FAILSAFE if (!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 failsafe mavlink_msg_command_ack_send_buf(msg, chan, MAVLINK_MSG_ID_SET_MODE, MAV_RESULT_FAILED); }#else handle_set_mode(msg, FUNCTOR_BIND(&copter, &Copter::gcs_set_mode, bool, uint8_t));#endif break; } 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 bootup if (!copter.ap.initialised_params && !AP_BoardConfig::in_sensor_config_error()) { break; } // mark the firmware version in the tlog 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);#endif GCS_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 == ENABLED copter.camera_mount.handle_param_value(msg);#endif break; } 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 EEPROM case 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 GCS case 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 individually case 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 message case 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 location if (!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 == ENABLED handle_gimbal_report(copter.camera_mount, msg);#endif break; } 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 gcs mavlink_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 groundstation copter.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 purposes copter.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 gcs mavlink_manual_control_t packet; mavlink_msg_manual_control_decode(msg, &packet); if (packet.z < 0) { // Copter doesn't do negative thrust break; } 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 groundstation copter.failsafe.rc_override_active = override_active; // a manual control message is considered to be a 'heartbeat' from the ground station for failsafe purposes copter.failsafe.last_heartbeat_ms = AP_HAL::millis(); break; } case MAVLINK_MSG_ID_COMMAND_INT: { // decode packet mavlink_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 location if (!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 NAK mavlink_msg_command_ack_send_buf(msg, chan, packet.command, result); break; } // Pre-Flight calibration requests case MAVLINK_MSG_ID_COMMAND_LONG: // MAV ID: 76 { // decode packet mavlink_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 cm if(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 : unused if (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 failure if(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 location if (!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 location if (!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 == ENABLED case 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 == ENABLED case MAV_CMD_DO_MOUNT_CONTROL:#if MOUNT == ENABLED if(!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;#endif break; 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 armed if (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 calibration copter.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 calibration result = 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 gyros if (!copter.calibrate_gyros()) { result = MAV_RESULT_FAILED; break; } // accel trim float trim_roll, trim_pitch; if(copter.ins.calibrate_trim(trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine copter.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 calibration result = 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 failure if (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 deprecated copter.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 bootloader hal.scheduler->reboot(is_equal(packet.param1,3.0f)); result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_FENCE_ENABLE:#if AC_FENCE == ENABLED result = 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 failure result = MAV_RESULT_FAILED;#endif break;#if PARACHUTE == ENABLED case MAV_CMD_DO_PARACHUTE: // configure or release parachute result = 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 altitude copter.parachute_manual_release(); break; default: result = MAV_RESULT_FAILED; break; } break;#endif case 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 == ENABLED case 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;#endif case 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); #endif GCS_MAVLINK::send_statustext_chan(MAV_SEVERITY_INFO, chan, "Frame: %s", copter.get_frame_string()); // send system ID if we can char 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 AltHold if (!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 motors copter.init_arm_motors(true); } else if (copter.ap.land_complete) { // if armed and landed, takeoff if (copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) { copter.do_user_takeoff(packet.param1*100, true); } } else { // if flying, land copter.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 motors copter.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 mode bool 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 NAK mavlink_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 packet mavlink_set_attitude_target_t packet; mavlink_msg_set_attitude_target_decode(msg, &packet); // exit if vehicle is not in Guided mode or Auto-Guided mode if ((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 thrust if ((packet.type_mask & ((1<<7)|(1<<6))) != 0) { break; } // convert thrust to climb rate packet.thrust = constrain_float(packet.thrust, 0.0f, 1.0f); float climb_rate_cms = 0.0f; if (is_equal(packet.thrust, 0.5f)) { climb_rate_cms = 0.0f; } else if (packet.thrust > 0.5f) { // climb at up to WPNAV_SPEED_UP climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * copter.wp_nav->get_speed_up(); } else { // descend at up to WPNAV_SPEED_DN climb_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 rate bool 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 packet mavlink_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 mode if ((copter.control_mode != GUIDED) && !(copter.control_mode == AUTO && copter.auto_mode == Auto_NavGuided)) { break; } // check for supported coordinate frames if (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 position Vector3f pos_vector; if (!pos_ignore) { // convert to cm pos_vector = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f); // rotate to body-frame if necessary if (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 necessary if (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-origin pos_vector.z = copter.pv_alt_above_origin(pos_vector.z); } } // prepare velocity Vector3f vel_vector; if (!vel_ignore) { // convert to cm vel_vector = Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f); // rotate to body-frame if necessary if (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 yaw float 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 request if (!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 packet mavlink_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 mode if ((copter.control_mode != GUIDED) && !(copter.control_mode == AUTO && copter.auto_mode == Auto_NavGuided)) { break; } // check for supported coordinate frames if (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_INT packet.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 location if (!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_INT case 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 controller loc.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 yaw float 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 == ENABLED copter.g2.proximity.handle_msg(msg);#endif break; } 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_DISABLED case MAVLINK_MSG_ID_HIL_STATE: // MAV ID: 90 { mavlink_hil_state_t packet; mavlink_msg_hil_state_decode(msg, &packet); // sanity check location if (!check_latlng(packet.lat, packet.lon)) { break; } // set gps hil sensor Location 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/sec Vector3f gyros; gyros.x = packet.rollspeed; gyros.y = packet.pitchspeed; gyros.z = packet.yawspeed; // m/s/s Vector3f 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_DISABLED case 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 == ENABLED case 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 GCS case MAVLINK_MSG_ID_FENCE_POINT: // MAV ID: 160 case 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_CONFIGURE case MAVLINK_MSG_ID_DIGICAM_CONFIGURE: // MAV ID: 202 break; //deprecated. Use MAV_CMD_DO_DIGICAM_CONTROL case 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_CONFIGURE case MAVLINK_MSG_ID_MOUNT_CONFIGURE: // MAV ID: 204 copter.camera_mount.configure_msg(msg); break; //deprecated. Use MAV_CMD_DO_MOUNT_CONTROL case 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 == ENABLED case MAVLINK_MSG_ID_TERRAIN_DATA: case MAVLINK_MSG_ID_TERRAIN_CHECK:#if AP_TERRAIN_AVAILABLE && AC_TERRAIN copter.terrain.handle_data(chan, msg);#endif break;#if AC_RALLY == ENABLED // receive a rally point from GCS and store in EEPROM case 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 location if (!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 GCS case 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 == ENABLED case 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 Notify AP_Notify::handle_led_control(msg); break; case MAVLINK_MSG_ID_PLAY_TUNE: // send message to Notify AP_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 location if (!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 == ENABLED copter.adsb.handle_message(chan, msg);#endif break; case MAVLINK_MSG_ID_VISION_POSITION_DELTA:#if VISUAL_ODOMETRY_ENABLED == ENABLED copter.g2.visual_odom.handle_msg(msg);#endif break; 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 timeout if (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
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
这些代码就不讲解，后续继续研究
（2）SCHED_TASK(gcs_send_heartbeat, 1, 110), //发送心跳包
void Copter::gcs_send_heartbeat(void){ gcs_send_message(MSG_HEARTBEAT); //心跳包} - 1
- 2
- 3
- 4
void Copter::gcs_send_message(enum ap_message id){ for (uint8_t i=0; i - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
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 any while (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 deferred for (i=0, nextid = next_deferred_message; i < num_deferred_messages; i++) { if (deferred_messages[nextid] == id) { // it's already deferred, discard return; } nextid++; if (nextid == MSG_RETRY_DEFERRED) { nextid = 0; } } if (num_deferred_messages != 0 || !try_send_message(id)) { // can't send it now, so defer it if (num_deferred_messages == MSG_RETRY_DEFERRED) { // the defer buffer is full, discard return; } nextid = next_deferred_message + num_deferred_messages; if (nextid >= MSG_RETRY_DEFERRED) { nextid -= MSG_RETRY_DEFERRED; } deferred_messages[nextid] = id; num_deferred_messages++; }} - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
到这里有个比较重要的函数，那就是：
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; }#endif switch(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 user if (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 == ENABLED CHECK_PAYLOAD_SIZE(RANGEFINDER); copter.send_rangefinder(chan);#endif CHECK_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_TERRAIN CHECK_PAYLOAD_SIZE(TERRAIN_REQUEST); copter.terrain.send_request(chan);#endif break; case MSG_CAMERA_FEEDBACK:#if CAMERA == ENABLED CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK); copter.camera.send_feedback(chan, copter.gps, copter.ahrs, copter.current_loc);#endif break; case MSG_STATUSTEXT: // depreciated, use GCS_MAVLINK::send_statustext* return false; case MSG_FENCE_STATUS:#if AC_FENCE == ENABLED CHECK_PAYLOAD_SIZE(FENCE_STATUS); copter.send_fence_status(chan);#endif break; case MSG_AHRS: CHECK_PAYLOAD_SIZE(AHRS); send_ahrs(copter.ahrs); break; case MSG_SIMSTATE:#if CONFIG_HAL_BOARD == HAL_BOARD_SITL CHECK_PAYLOAD_SIZE(SIMSTATE); copter.send_simstate(chan);#endif CHECK_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 == ENABLED CHECK_PAYLOAD_SIZE(MOUNT_STATUS); copter.camera_mount.status_msg(chan);#endif // MOUNT == ENABLED break; case MSG_BATTERY2: CHECK_PAYLOAD_SIZE(BATTERY2); send_battery2(copter.battery); break; case MSG_OPTICAL_FLOW:#if OPTFLOW == ENABLED CHECK_PAYLOAD_SIZE(OPTICAL_FLOW); send_opticalflow(copter.ahrs, copter.optflow);#endif break; case MSG_GIMBAL_REPORT:#if MOUNT == ENABLED CHECK_PAYLOAD_SIZE(GIMBAL_REPORT); copter.camera_mount.send_gimbal_report(chan);#endif break; 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: // unused break; 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 warning case 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;} - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
- 104
- 105
- 106
- 107
- 108
- 109
- 110
- 111
- 112
- 113
- 114
- 115
- 116
- 117
- 118
- 119
- 120
- 121
- 122
- 123
- 124
- 125
- 126
- 127
- 128
- 129
- 130
- 131
- 132
- 133
- 134
- 135
- 136
- 137
- 138
- 139
- 140
- 141
- 142
- 143
- 144
- 145
- 146
- 147
- 148
- 149
- 150
- 151
- 152
- 153
- 154
- 155
- 156
- 157
- 158
- 159
- 160
- 161
- 162
- 163
- 164
- 165
- 166
- 167
- 168
- 169
- 170
- 171
- 172
- 173
- 174
- 175
- 176
- 177
- 178
- 179
- 180
- 181
- 182
- 183
- 184
- 185
- 186
- 187
- 188
- 189
- 190
- 191
- 192
- 193
- 194
- 195
- 196
- 197
- 198
- 199
- 200
- 201
- 202
- 203
- 204
- 205
- 206
- 207
- 208
- 209
- 210
- 211
- 212
- 213
- 214
- 215
- 216
- 217
- 218
- 219
- 220
- 221
- 222
- 223
- 224
- 225
- 226
- 227
- 228
- 229
- 230
- 231
- 232
- 233
- 234
- 235
- 236
- 237
- 238
- 239
- 240
- 241
- 242
- 243
- 244
- 245
- 246
- 247
- 248
- 249
- 250
- 251
- 252
- 253
- 254
- 255
这里会有飞控所有相关的信息
我们看下关心的心跳包数据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 triggered if (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 documentation base_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 do break; default: break; } // all modes except INITIALISING have some form of manual // override if stick mixing is enabled base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;#if HIL_MODE != HIL_MODE_DISABLED base_mode |= MAV_MODE_FLAG_HIL_ENABLED;#endif // we are armed if we are not initialising if (motors->armed()) { base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } // indicate we have set a custom mode base_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);} - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
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);} - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
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_FIELDS char 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);#else mavlink_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} - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
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_flags buf[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 signature signature_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);} - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
到这里我们可以看到所有的数据都是通过串口发出去
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 platform defines 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} - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
（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} - 1
- 2
- 3
- 4
- 5
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 link if (_statustext_queue.empty()) { // nothing to do return; } 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.bitmask for (uint8_t i=0; i bitmask & chan_bit) { // something is queued on a port and that's the port index we're looped at mavlink_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 mask mavlink_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 index idx++; } }} - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
这里还没分析，看的网上的：从_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 - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
void GCS_MAVLINK_Copter::data_stream_send(void){ if (waypoint_receiving) { //不要干扰任务迁移-----don't interfere with mission transfer return; } 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 callback return; } if (stream_trigger(STREAM_RAW_SENSORS)) { send_message(MSG_RAW_IMU1); // RAW_IMU, SCALED_IMU2, SCALED_IMU3 send_message(MSG_RAW_IMU2); // SCALED_PRESSURE, SCALED_PRESSURE2, SCALED_PRESSURE3 send_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_STATUS send_message(MSG_EXTENDED_STATUS2); // MEMINFO send_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, AHRS2 send_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_TERRAIN send_message(MSG_TERRAIN);#endif send_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 any while (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 deferred for (i=0, nextid = next_deferred_message; i < num_deferred_messages; i++) { if (deferred_messages[nextid] == id) { // it's already deferred, discard return; } nextid++; if (nextid == MSG_RETRY_DEFERRED) { nextid = 0; } } if (num_deferred_messages != 0 || !try_send_message(id)) { // can't send it now, so defer it if (num_deferred_messages == MSG_RETRY_DEFERRED) { // the defer buffer is full, discard return; } nextid = next_deferred_message + num_deferred_messages; if (nextid >= MSG_RETRY_DEFERRED) { nextid -= MSG_RETRY_DEFERRED; } deferred_messages[nextid] = id; num_deferred_messages++; }}
你可能感兴趣的文章
缓存篇-Redis缓存失效以及解决方案
查看>>
缓存篇-使用Redis进行分布式锁应用
查看>>
缓存篇-Redisson的使用
查看>>
phpquery抓取网站内容简单介绍
查看>>
找工作准备的方向（4月22日写的）
查看>>
关于fwrite写入文件后打开查看是乱码的问题
查看>>
用结构体指针前必须要用malloc，不然会出现段错误
查看>>
Linux系统中的美
查看>>
一些实战项目（linux应用层编程，多线程编程，网络编程）
查看>>
我觉得专注于去学东西就好了，与世无争。
查看>>
原来k8s docker是用go语言写的，和现在所讲的go是一个东西！
查看>>
STM32CubeMX 真的不要太好用
查看>>
STM32CubeMX介绍、下载与安装
查看>>
电机和桨叶要搭配选择
查看>>
不要买铝合金机架的无人机，不耐摔，易变形弯曲。
查看>>
ACfly也是基于FreeRTOS的
查看>>
F330装GPS的位置
查看>>
我想先用三个或者五个激光测距做无人机的室内定位和避障
查看>>
pixhawk也可以用Airsim仿真
查看>>
《无人机电机与电调技术》可以看看
查看>>