mavlink（五）C库源码分析
1. C 库源码分析
以下分析的源码基于mavlink生成器 1.0.12版本，mavlink协议为v2。
1.1. 编解码流程
以下按common.xml 内定义的heartbeat消息示例，分析heartbeat消息的编码流程，其他消息雷同。
首先，生成器mavgenerate将定义在xml内的每个message专门生成一个文件，形如mavlink_msg_xxx.h，它提供了消息xxx的id、payload、crc_extra、len以及编解码函数定义。
1.1.1. 编码
编码的入口为mavlink_msg_heartbeat_encode函数，其函数栈调用关系如下：
#0 mavlink_finalize_message_buffer
#1 mavlink_finalize_message_chan
#2 mavlink_finalize_message
#3 mavlink_msg_heartbeat_pack
#4 mavlink_msg_heartbeat_encode
mavlink_msg_heartbeat_encode 直接调用了 mavlink_msg_heartbeat_pack，实现如下：
static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
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);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN);
#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;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HEARTBEAT_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_HEARTBEAT;
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
}
MAVLINK_ALIGNED_FIELDS 在mavlink.h中被定义为1，而MAVLINK_NEED_BYTE_SWAP则在protocol.h中定义为如下：
/*
If you want MAVLink on a system that is native big-endian,
you need to define NATIVE_BIG_ENDIAN
*/
#ifdef NATIVE_BIG_ENDIAN
# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN == MAVLINK_LITTLE_ENDIAN)
#else
# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN != MAVLINK_LITTLE_ENDIAN)
#endif
其中
// 定义在mavlink_type.h
#define MAVLINK_BIG_ENDIAN 0
#define MAVLINK_LITTLE_ENDIAN 1
// 定义在mavlink.h
#ifndef MAVLINK_ENDIAN
#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN
#endif
当前默认是小端序机器，NATIVE_BIG_ENDIAN未做定义，MAVLINK_NEED_BYTE_SWAP则为false，mavlink_msg_heartbeat_pack对buffer进行编码时不需要转换字节序。反之，若当前为大端序，需要自行定义NATIVE_BIG_ENDIAN，同时在做编码的时候需要调用_mav_put_uint32_t这类接口对结构体字段转换字节序。其定义如下，其他类型的接口雷同。
// 定义在protocol.h
#if MAVLINK_NEED_BYTE_SWAP
#define _mav_put_uint32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
#elif !MAVLINK_ALIGNED_FIELDS
#define _mav_put_uint32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b)
#else
#define _mav_put_uint32_t(buf, wire_offset, b) *(uint32_t *)&buf[wire_offset] = b
#endif
#if MAVLINK_NEED_BYTE_SWAP
static inline void byte_swap_4(char *dst, const char *src)
{
dst[0] = src[3];
dst[1] = src[2];
dst[2] = src[1];
dst[3] = src[0];
}
#elif !MAVLINK_ALIGNED_FIELDS
static inline void byte_copy_4(char *dst, const char *src)
{
dst[0] = src[0];
dst[1] = src[1];
dst[2] = src[2];
dst[3] = src[3];
}
#endif
mavlink_msg_heartbeat_pack接口先对结构体部分，即载荷部分做了编码处理，而其消息头及校验和、签名字段等的编码则在mavlink_finalize_message实现。
mavlink_finalize_message直接调用了mavlink_finalize_message_chan接口，其使用了默认通道 MAVLINK_COMM_0。
而mavlink_finalize_message_chan接口如下：
// 定义在mavlink_helper.h
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra)
{
mavlink_status_t *status = mavlink_get_channel_status(chan);
return mavlink_finalize_message_buffer(msg, system_id, component_id, status, min_length, length, crc_extra);
}
// 定义在mavlink_helper.h
MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
{
#ifdef MAVLINK_EXTERNAL_RX_STATUS
// No m_mavlink_status array defined in function,
// has to be defined externally
#else
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
#endif
return &m_mavlink_status[chan];
}
// 定义在mavlink_type.h
#ifndef MAVLINK_COMM_NUM_BUFFERS
#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32)
# define MAVLINK_COMM_NUM_BUFFERS 16
#else
# define MAVLINK_COMM_NUM_BUFFERS 4
#endif
#endif
mavlink 静态定义了一个mavlink_status_t 数组，数组的大小即为通道数，在linux或win32平台，其为16个。每个通道独享一个mavlink_status_t用于存储统计信息及解码状态机，因此，每个传输通道的编解码过程互不干扰。
mavlink_finalize_message_buffer具体实现了头部和尾部校验和、签名字段的编码，如下：
MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
mavlink_status_t* status, uint8_t min_length, uint8_t length, uint8_t crc_extra)
{
bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0;
#ifndef MAVLINK_NO_SIGN_PACKET
bool signing = (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING);
#else
bool signing = false;
#endif
uint8_t signature_len = signing? MAVLINK_SIGNATURE_BLOCK_LEN : 0;
uint8_t header_len = MAVLINK_CORE_HEADER_LEN+1;
uint8_t buf[MAVLINK_CORE_HEADER_LEN+1];
if (mavlink1) {
msg->magic = MAVLINK_STX_MAVLINK1;
header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN+1;
} else {
msg->magic = MAVLINK_STX;
}
msg->len = mavlink1?min_length:_mav_trim_payload(_MAV_PAYLOAD(msg), length);
msg->sysid = system_id;
msg->compid = component_id;
msg->incompat_flags = 0;
if (signing) {
msg->incompat_flags |= MAVLINK_IFLAG_SIGNED;
}
msg->compat_flags = 0;
msg->seq = status->current_tx_seq;
status->current_tx_seq = status->current_tx_seq + 1;
// form the header as a byte array for the crc
buf[0] = msg->magic;
buf[1] = msg->len;
if (mavlink1) {
buf[2] = msg->seq;
buf[3] = msg->sysid;
buf[4] = msg->compid;
buf[5] = msg->msgid & 0xFF;
} else {
buf[2] = msg->incompat_flags;
buf[3] = msg->compat_flags;
buf[4] = msg->seq;
buf[5] = msg->sysid;
buf[6] = msg->compid;
buf[7] = msg->msgid & 0xFF;
buf[8] = (msg->msgid >> 8) & 0xFF;
buf[9] = (msg->msgid >> 16) & 0xFF;
}
uint16_t checksum = crc_calculate(&buf[1], header_len-1);
crc_accumulate_buffer(&checksum, _MAV_PAYLOAD(msg), msg->len);
crc_accumulate(crc_extra, &checksum);
mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF);
mavlink_ck_b(msg) = (uint8_t)(checksum >> 8);
msg->checksum = checksum;
#ifndef MAVLINK_NO_SIGN_PACKET
if (signing) {
mavlink_sign_packet(status->signing,
msg->signature,
(const uint8_t *)buf, header_len,
(const uint8_t *)_MAV_PAYLOAD(msg), msg->len,
(const uint8_t *)_MAV_PAYLOAD(msg)+(uint16_t)msg->len);
}
#endif
return msg->len + header_len + 2 + signature_len;
}
由以上可以提取几点细节：
- mavlink v1版本不包含签名字段，不提供数据包签名功能（line 6体现）；
- mavlink v2版本对载荷末尾0字节部分做了截断，而v1版本不做此处理（line 19体现）；
- 签名的数据包，其incompat_flags 字段 MAVLINK_IFLAG_SIGNED 置位（line 23~25体现）；
- 对于每个通道的每次编码，其数据包的序号是累加的（line 27~28体现）；
- mavlink v2 msgid 字段占3个字节，小端序，低字节存储在低地址，高字节存储在高地址（line 44~46体现）；
- 校验和从len字段到payload字段计算，magic字段不参与，在payload后需额外加一个crc_extra参与计算（line 49~53体现）；
其中，截断载荷末尾0字节的实现如下：
MAVLINK_HELPER uint8_t _mav_trim_payload(const char *payload, uint8_t length)
{
while (length > 1 && payload[length-1] == 0) {
length--;
}
return length;
}
length 至少为1，即使载荷都为0，依旧会保留1个字节的载荷。
其中，签名字段编码实现如下：
MAVLINK_HELPER uint8_t mavlink_sign_packet(mavlink_signing_t *signing,
uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN],
const uint8_t *header, uint8_t header_len,
const uint8_t *packet, uint8_t packet_len,
const uint8_t crc[2])
{
mavlink_sha256_ctx ctx;
union {
uint64_t t64;
uint8_t t8[8];
} tstamp;
if (signing == NULL || !(signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING)) {
return 0;
}
signature[0] = signing->link_id;
tstamp.t64 = signing->timestamp;
memcpy(&signature[1], tstamp.t8, 6);
signing->timestamp++;
mavlink_sha256_init(&ctx);
mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key));
mavlink_sha256_update(&ctx, header, header_len);
mavlink_sha256_update(&ctx, packet, packet_len);
mavlink_sha256_update(&ctx, crc, 2);
mavlink_sha256_update(&ctx, signature, 7);
mavlink_sha256_final_48(&ctx, &signature[7]);
return MAVLINK_SIGNATURE_BLOCK_LEN;
}
可以看出，签名字段的计算采用sha256-48算法, 由secret_key + header + payload + crc + link-id + timestamp, 作为上下文参与计算，算法在此不做深究。
至此，消息头、载荷、校验和、签名都已经做好计算和编码处理，并存储在mavlink_message_t结构中，接下来需要对其进行序列化操作，才可以发送。
// 定义在mavlink_helper.h
MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buf, const mavlink_message_t *msg)
{
uint8_t signature_len, header_len;
uint8_t *ck;
uint8_t length = msg->len;
if (msg->magic == MAVLINK_STX_MAVLINK1) {
signature_len = 0;
header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN;
buf[0] = msg->magic;
buf[1] = length;
buf[2] = msg->seq;
buf[3] = msg->sysid;
buf[4] = msg->compid;
buf[5] = msg->msgid & 0xFF;
memcpy(&buf[6], _MAV_PAYLOAD(msg), msg->len);
ck = buf + header_len + 1 + (uint16_t)msg->len;
} else {
length = _mav_trim_payload(_MAV_PAYLOAD(msg), length);
header_len = MAVLINK_CORE_HEADER_LEN;
buf[0] = msg->magic;
buf[1] = length;
buf[2] = msg->incompat_flags;
buf[3] = msg->compat_flags;
buf[4] = msg->seq;
buf[5] = msg->sysid;
buf[6] = msg->compid;
buf[7] = msg->msgid & 0xFF;
buf[8] = (msg->msgid >> 8) & 0xFF;
buf[9] = (msg->msgid >> 16) & 0xFF;
memcpy(&buf[10], _MAV_PAYLOAD(msg), length);
ck = buf + header_len + 1 + (uint16_t)length;
signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0;
}
ck[0] = (uint8_t)(msg->checksum & 0xFF);
ck[1] = (uint8_t)(msg->checksum >> 8);
if (signature_len > 0) {
memcpy(&ck[2], msg->signature, signature_len);
}
return header_len + 1 + 2 + (uint16_t)length + (uint16_t)signature_len;
}
序列化的过程即逐一赋值或拷贝，最终存储在buf中，并返回buf的有效长度。
总结
整个编码的过程，数据传递为 mavlink_heartbeat_t --> mavlink_message_t --> uint8_t buf[]。而mavlink_status_t则参与编码过程中计算、状态量的存取；
1.1.2. 解码
解码的函数入口为mavlink_parse_char，其调用堆栈如下：
#0 mavlink_frame_char_buffer
#1 mavlink_frame_char
#2 mavlink_parse_char
先来看mavlink_frame_char_buffer
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_frame_char的返回值msg_received来判断解析状态，当解析状态值为MAVLINK_FRAMING_BAD_CRC或MAVLINK_FRAMING_BAD_SIGNATURE时，表示字段异常（CRC校验出错或签名出错），更新相应的状态值，并返回0提示解码失败，同时如果此时传入的字符为消息头MAVLINK_STX，开始新一轮解析（line 12~17）。
msg_received的取值枚举为下：
typedef enum {
MAVLINK_FRAMING_INCOMPLETE=0,
MAVLINK_FRAMING_OK=1,
MAVLINK_FRAMING_BAD_CRC=2,
MAVLINK_FRAMING_BAD_SIGNATURE=3
} mavlink_framing_t;
其可以表示4种解析状态：帧解析中、帧解析成功、帧解析CRC出错、帧解析签名出错；
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);
}
mavlink_get_channel_buffer和mavlink_get_channel_status分别根据通道号取到mavlink_message_t和mavlink_status_t结构，其存储了解析过程的帧结构和相关状态量（包括解析状态机）。这两个结构是声明为static的，其生命周期贯穿程序运行始终。
另一方面，解析是按通道来的，每个通道解析数据独立，内部维护的状态值也各自独立，互不干扰；
MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
{
#ifdef MAVLINK_EXTERNAL_RX_BUFFER
// No m_mavlink_buffer array defined in function,
// has to be defined externally
#else
static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
#endif
return &m_mavlink_buffer[chan];
}
MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
{
#ifdef MAVLINK_EXTERNAL_RX_STATUS
// No m_mavlink_status array defined in function,
// has to be defined externally
#else
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
#endif
return &m_mavlink_status[chan];
}
然后是mavlink_frame_char_buffer，其实现了解析的具体过程。
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)
{
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) {
if(rxmsg->len > 0) {
status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
} else {
status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
}
#ifdef MAVLINK_CHECK_MESSAGE_LENGTH
if (rxmsg->len < mavlink_min_message_length(rxmsg) ||
rxmsg->len > mavlink_max_message_length(rxmsg)) {
_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 |= ((uint32_t)c)<<16;
mavlink_update_checksum(rxmsg, c);
if(rxmsg->len > 0){
status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
} else {
status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
}
#ifdef MAVLINK_CHECK_MESSAGE_LENGTH
if (rxmsg->len < mavlink_min_message_length(rxmsg) ||
rxmsg->len > mavlink_max_message_length(rxmsg))
{
_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->max_msg_len) {
memset(&_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx], 0, e->max_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;
if (r_message != NULL) {
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
#ifndef MAVLINK_NO_SIGNATURE_CHECK
bool sig_ok = mavlink_signature_check(status->signing, status->signing_streams, rxmsg);
#else
bool sig_ok = true;
#endif
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;
if (r_message !=NULL) {
memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
}
}
break;
}
bufferIndex++;
// If a message has been successfully 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++;
}
if (r_message != NULL) {
r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg
}
if (r_mavlink_status != NULL) {
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
*/
if (r_message != NULL) {
r_message->checksum = rxmsg->ck[0] | (rxmsg->ck[1]<<8);
}
}
return status->msg_received;
}
整个解析的过程是按照状态机来实现的，根据收到的每个字符对应于帧结构的每个字段，区分状态机。若数据流符合帧字段，则状态机按序迁移并完成解析，否则（字段出错，如crc或签名等），丢弃前面缓存的数据流，状态机重置，直到收到帧头字段再开始新一轮的解析。
整个解析过程，兼容了mavlink v1和mavlink v2两个版本。因为帧结构的差异，v2比v1多了几个中间状态。
解析成功后，将结果赋值给r_message和r_mavlink_status，并不直接返回静态存储的m_mavlink_buffer[chan]和m_mavlink_status[chan]。
解析成功后，返回值为status->msg_received = MAVLINK_FRAMING_OK，值为1，所以用户层调用mavlink_parse_char时可判断当返回值为1时即表示解析到完整一帧了。
解析过程中，每次收到符合预期的字符，都会调用mavlink_update_checksum计算校验和，当收到校验和字段时会进行校验（line 176）。
解析过程中，会记录和统计相关状态量（成功次数、失败次数等），可通过r_mavlink_status获得。
整个解析过程的状态迁移图如下：
mavlink_parse_char只是解析数据流，反序列化，其结果存储在mavlink_message_t中，其中payload部分是没有解析的，需要根据msgid调用具体消息的decode函数。
如下为heartbeat消息的decode函数：
static inline void mavlink_msg_heartbeat_decode(const mavlink_message_t* msg, mavlink_heartbeat_t* heartbeat)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
heartbeat->custom_mode = mavlink_msg_heartbeat_get_custom_mode(msg);
heartbeat->type = mavlink_msg_heartbeat_get_type(msg);
heartbeat->autopilot = mavlink_msg_heartbeat_get_autopilot(msg);
heartbeat->base_mode = mavlink_msg_heartbeat_get_base_mode(msg);
heartbeat->system_status = mavlink_msg_heartbeat_get_system_status(msg);
heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg);
#else
uint8_t len = msg->len < MAVLINK_MSG_ID_HEARTBEAT_LEN? msg->len : MAVLINK_MSG_ID_HEARTBEAT_LEN;
memset(heartbeat, 0, MAVLINK_MSG_ID_HEARTBEAT_LEN);
memcpy(heartbeat, _MAV_PAYLOAD(msg), len);
#endif
}
当宏定义MAVLINK_NEED_BYTE_SWAP为true或MAVLINK_ALIGNED_FIELDS为false时，需要考虑交换字节序或字节对齐。否则将msg->payload直接拷贝到具体消息的结构体即可（mavlink_heartbeat_t）。
_MAV_PAYLOAD实现如下：
#define _MAV_PAYLOAD(msg) ((const char *)(&((msg)->payload64[0])))
mavlink_msg_heartbeat_get_xxx(msg) 等接口实现了消息字段的字节序交换或字节对齐问题。
根据字段类型，其具体调用如下系列的函数，其中wire_offset为字段在结构体中的偏移。
// 定义在protocol.h
_MAV_RETURN_char(msg, wire_offset)
_MAV_RETURN_int8_t(msg, wire_offset)
_MAV_RETURN_uint8_t(msg, wire_offset)
_MAV_RETURN_uint16_t(msg, wire_offset)
_MAV_RETURN_uint32_t(msg, wire_offset)
_MAV_RETURN_uint64_t(msg, wire_offset)
_MAV_RETURN_int16_t(msg, wire_offset)
_MAV_RETURN_int32_t(msg, wire_offset)
_MAV_RETURN_int64_t(msg, wire_offset)
_MAV_RETURN_float(msg, wire_offset)
_MAV_RETURN_double(msg, wire_offset)
// 对于数组类型，则是调用如下函数，xxx为类型，同上
_MAV_RETURN_xxx_array(msg, field_name, array_length, wire_offset);
其中单字节类型不需要考虑字节序交换或字节对齐
#define _MAV_RETURN_char(msg, wire_offset) (char)_MAV_PAYLOAD(msg)[wire_offset]
#define _MAV_RETURN_int8_t(msg, wire_offset) (int8_t)_MAV_PAYLOAD(msg)[wire_offset]
#define _MAV_RETURN_uint8_t(msg, wire_offset) (uint8_t)_MAV_PAYLOAD(msg)[wire_offset]
其他根据不同宏定义实现不同，对于非数组字段
#if MAVLINK_NEED_BYTE_SWAP
#define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
{ TYPE r; byte_swap_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
_MAV_MSG_RETURN_TYPE(uint16_t, 2)
_MAV_MSG_RETURN_TYPE(int16_t, 2)
_MAV_MSG_RETURN_TYPE(uint32_t, 4)
_MAV_MSG_RETURN_TYPE(int32_t, 4)
_MAV_MSG_RETURN_TYPE(uint64_t, 8)
_MAV_MSG_RETURN_TYPE(int64_t, 8)
_MAV_MSG_RETURN_TYPE(float, 4)
_MAV_MSG_RETURN_TYPE(double, 8)
#elif !MAVLINK_ALIGNED_FIELDS
#define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
{ TYPE r; byte_copy_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
_MAV_MSG_RETURN_TYPE(uint16_t, 2)
_MAV_MSG_RETURN_TYPE(int16_t, 2)
_MAV_MSG_RETURN_TYPE(uint32_t, 4)
_MAV_MSG_RETURN_TYPE(int32_t, 4)
_MAV_MSG_RETURN_TYPE(uint64_t, 8)
_MAV_MSG_RETURN_TYPE(int64_t, 8)
_MAV_MSG_RETURN_TYPE(float, 4)
_MAV_MSG_RETURN_TYPE(double, 8)
#else // nicely aligned, no swap
#define _MAV_MSG_RETURN_TYPE(TYPE) \
static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
{ return *(const TYPE *)(&_MAV_PAYLOAD(msg)[ofs]);}
_MAV_MSG_RETURN_TYPE(uint16_t)
_MAV_MSG_RETURN_TYPE(int16_t)
_MAV_MSG_RETURN_TYPE(uint32_t)
_MAV_MSG_RETURN_TYPE(int32_t)
_MAV_MSG_RETURN_TYPE(uint64_t)
_MAV_MSG_RETURN_TYPE(int64_t)
_MAV_MSG_RETURN_TYPE(float)
_MAV_MSG_RETURN_TYPE(double)
#endif // MAVLINK_NEED_BYTE_SWAP
对于MAVLINK_NEED_BYTE_SWAP，则是调用 byte_swap_xxx系列函数实现字节序交换；
对于!MAVLINK_ALIGNED_FIELDS，则是调用 byte_copy_xxx系列函数实现字节对齐；
对于数组字段
static inline uint16_t _MAV_RETURN_char_array(const mavlink_message_t *msg, char *value,
uint8_t array_length, uint8_t wire_offset)
{
memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
return array_length;
}
static inline uint16_t _MAV_RETURN_uint8_t_array(const mavlink_message_t *msg, uint8_t *value,
uint8_t array_length, uint8_t wire_offset)
{
memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
return array_length;
}
static inline uint16_t _MAV_RETURN_int8_t_array(const mavlink_message_t *msg, int8_t *value,
uint8_t array_length, uint8_t wire_offset)
{
memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
return array_length;
}
#if MAVLINK_NEED_BYTE_SWAP
#define _MAV_RETURN_ARRAY(TYPE, V) \
static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
uint8_t array_length, uint8_t wire_offset) \
{ \
uint16_t i; \
for (i=0; i<array_length; i++) { \
value[i] = _MAV_RETURN_## TYPE (msg, wire_offset+(i*sizeof(value[0]))); \
} \
return array_length*sizeof(value[0]); \
}
#else
#define _MAV_RETURN_ARRAY(TYPE, V) \
static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
uint8_t array_length, uint8_t wire_offset) \
{ \
memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length*sizeof(TYPE)); \
return array_length*sizeof(TYPE); \
}
#endif
_MAV_RETURN_ARRAY(uint16_t, u16)
_MAV_RETURN_ARRAY(uint32_t, u32)
_MAV_RETURN_ARRAY(uint64_t, u64)
_MAV_RETURN_ARRAY(int16_t, i16)
_MAV_RETURN_ARRAY(int32_t, i32)
_MAV_RETURN_ARRAY(int64_t, i64)
_MAV_RETURN_ARRAY(float, f)
_MAV_RETURN_ARRAY(double, d)
对于uint8_t/int8_t/char等单字节类型的数组字段，直接拷贝即可，不需考虑字节序交换；
对于多字节类型的数组字段，当MAVLINK_NEED_BYTE_SWAP为true，需要对数组每一项进行字节序交换，最终调用的是byte_swap_xxx；
本文来自博客园，作者：流翎，转载请注明原文链接：https://www.cnblogs.com/hjx168/p/17737335.html
