Ardupilot飞控Mavlink代码学习
目录
-
-
- 目录
-
- 摘要
- 1.Ardupilot怎么实现Mavlink初始化
- 2.Mavlink消息通信过程
摘要
本节主要记录自己学习Ardupilot的Mavlink协议的过程，欢迎一起交流分析！
1.Ardupilot怎么实现Mavlink初始化
（1）初始化USB端口，进行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;
}
}
该函数大体的作用：判断有没有串口，已经串口通道，设置串口波特率，初始化串口协议，判断是否使用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_stream
SCHED_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 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<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 message
if (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 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》我们重点看下几个函数：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;
}**这是一个处理完整的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 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;
}
}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);
}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);
}
}我们重点看这个函数：
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 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); 这些代码就不讲解，后续继续研究
（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; iif (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 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++;
}
}
到这里有个比较重要的函数，那就是：
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;
}这里会有飞控所有相关的信息
我们看下关心的心跳包数据
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);
}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_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
}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);
}到这里我们可以看到所有的数据都是通过串口发出去
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
}（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 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; i1U<// logical AND (&) to mask them together
if (statustext->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++;
}
}
} 这里还没分析，看的网上的：从_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; iif (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 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++;
}
}