[转]Mavlink协议

转自luckpl：Mavlink协议

一、Mavlink协议
MAVLink通讯协议是一个为微型飞行器设计的非常轻巧的、只由头文件构成的信息编组库。它可以通过串口非常高效地封装C结构数据，并将这些数据包发送至地面控制站。该协议被PX4, PIXHAWK, APM和Parrot AR.Drone平台所广泛测试并在以上的项目中作为MCU/IMU间以及Linux进程和地面站链路通信间的主干通信协议。

二、数据结构

Mavlink传输时基本单位是消息帧，消息帧的结构如下： 

 注意：校验码由crc16算法得到，算法从消息包的1~n+6字节（不包含STX），还要额外加上个MAVLINK_CRC_EXTRA进行计算得到一个16位的校验码。每个消息的头文件里都包含MAVLINK_CRC_EXTRA，这个 MAVLINK_CRC_EXTRA是由生成mavlink代码的xml文件生成的，加入这个额外的东西是为了当飞行器和地面站使用不同版本的mavlink协议时，双方计算得到的校验码会不同，这样不同版本间的mavlink协议就不会在一起正常工作，避免了由于不同版本间通讯时带来的重大潜在问题。

三、协议支持的数据类型

MAVLink协议支持固定大小的整形数据类型、IEEE754协议规定的单精度浮点型数据，或由以上数据构成的数组（如：char[10]），以及由协议自动添加的MAVLink版本号。具体如下： 

- char - Characters / strings 

- uint8_t - Unsigned 8 bit 

- int8_t - Signed 8 bit 

- uint16_t - Unsigned 16 bit 

- int16_t - Signed 16 bit 

- uint32_t - Unsigned 32 bit 

- int32_t - Signed 32 bit 

- uint64_t - Unsigned 64 bit 

- int64_t - Signed 64 bit 

- float - IEEE 754 single precision floating point number 

- double - IEEE 754 double precision floating point number 

- uint8_t_mavlink_version - 无符号的8位字符自动补充到当前的mavlink版本，它不能被写入，只是读按照8位字符的方式读取。

四、性能

Mavlink协议有两方面的特性：传输速度快和安全性高。协议允许检验消息内容，同样允许检测丢失的消息序列且最少只需要每个包中6个字节的开销来保证。传输实例：

五、航点协议

航点协议描述了航点是如何发送和读取的。其目标是在发送端和接收端确保建立一种连续稳定的状态，以达到每个使用MAVLINK的MAV能够和QGC通信并交换或更新它的航点。

使用航点协议时，除了应答消息之外的其他信息在被发送之后，发送端的部件就会启动一个定时器。如果在特定的时间内，没有收到响应则请求的消息将会被重新发送一次。该重传过程会重复数次，如果在最后一次的重传超时后仍然没有收到相应，则认定该项事务失败。该重传机制意味着所有的部件必须能够处理重复的消息。

1. 读取MAV的航点列表

读取过程如下：

QGC端先发送一个WAYPOINT_REQUEST_LIST消息给MAV；MAV则会回一个WAYPOINT_COUNT消息，该消息描述了MAV的航点列表中航点的数量

QGC端发送一个WAYPOINT_REQUEST(WP0)的消息请求得到第1个航点的信息；MAV则会回对应的包含航点数据的消息WAYPOINT(WP0)

重复步骤2得到第2、3、4…直到最后一个航点的数据

当全部接收完所有的航点消息后，QGC端会发送一个WAYPOINT_ACK消息给MAV，用来表明整个读取过程结束了

整个过程如下图所示：

 2. 写MAV的航点列表

写入过程为：

QGC先将航点列表中航点的数量以WAYPOINT_COUNT(N)消息发送给MAV；MAV则回应请求第1个航点的消息WAYPOINT_REQUEST(WP0)

QGC接收到第1个航点的请求消息后，将包含第1个航点的数据以WAYPOINT(WP0)消息发送给MAV；MAV收到后继续请求下一个航点的信息

当MAV接收到最后一个航点的信息后，MAV会向QGC发送一个WAYPOINT_ACK消息来表明整个事件的结束

整个过程如下图所示： 

3. 清除MAV的航点列表

4. 设置当前MAV航点

六、参数的读写

1. 读取参数列表

2. 读取单个参数

单个参数可以通过PARAM_REQUEST_READ消息来读取

3. 写参数

 七、增加新的mavlink消息

MAVLINK的消息是定义在XML文件中，再被转换成C/C++,C#或Python代码。以心跳消息为例对如何增加新的消息进行说明。

注意：心跳消息heartbeat是唯一必须被使用的消息，飞行器与地面站相连时，地面站通常是根据心跳消息来判断是否和飞行器失去了联系，所以飞行器端需要以一定频率发送这个心跳包（通常为1HZ）

1. XML文件中消息的定义

<message id="0" name="HEARTBEAT">
  <description>The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot).</description>
  <field type="uint8_t" name="type">Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)</field>
  <field type="uint8_t" name="autopilot">Autopilot type / class. defined in MAV_CLASS ENUM</field>
  <field type="uint8_t" name="base_mode">System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h</field>
  <field type="uint32_t" name="custom_mode">Navigation mode bitfield, see MAV_AUTOPILOT_CUSTOM_MODE ENUM for some examples. This field is autopilot-specific.</field>
  <field type="uint8_t" name="system_status">System status flag, see MAV_STATUS ENUM</field>
  <field type="uint8_t_mavlink_version" name="mavlink_version">MAVLink version</field>
</message>

代码解读：

每条消息都是在<message></message>之间定义的

id="0"表示这条消息的ID或索引是数字0。消息ID的有效数字是从0~255，其中150~240是预留给用户来自定义消息的。

name="HEARTBEAT"是一个易读的消息名称，只用在定义代码中，消息传输时不会发送这个名字，消息本身只依赖于消息ID。

定义中的<description></description>表示消息的描述，消息的描述很重要，但不是必须的。消息的描述可以让用户明确该消息的含义和用途。

定义中的<field></field>表示消息的字段，类似于C中结构体的变量。消息的字段可以是有无符号的8位、16位、32位、64位的整型，也可以是单双精度的IEEE754浮点型。

type="uint8_t"表示该字段为无符号的8位整型。如果你想定义一个数组可以这样定义：type="uint8_t[5]" ,uint8_t_mavlink_version是一个特殊的类型，它是一个无符号的8位整型，用来表示当前使用的mavlink协议的版本号，该字段是只读的，当消息发送时会自动被填充。

2. 建立消息定义文件

如果你想建立一个单独的消息定义文件，就像commn.xml或其它位于message_definitions文件夹下的XML文件一样。你可以像下面代码一样定义，注意一定要包含版本号;如果这个XML文件和commn.xml位于同一个路径下，那么最终生成的MAVLINK代码中会包含commn.xml的内容。

<?xml version="1.0"?>
<mavlink>
        <include>common.xml</include>
        <!-- NOTE: If the included file already contains a version tag, remove the version tag here, else uncomment to enable. -->
    <!--<version>3</version>-->
    <enums>
    </enums>
    <messages>
        <message id="150" name="RUDDER_RAW">
            <description>This message encodes all of the raw rudder sensor data from the USV.</description>
            <field type="uint16_t" name="position">The raw data from the position sensor, generally a potentiometer.</field>
            <field type="uint8_t" name="port_limit">Status of the rudder limit sensor, port side. 0 indicates off and 1 indicates that the limit is hit. If this sensor is inactive set to 0xFF.</field>
            <field type="uint8_t" name="center_limit">Status of the rudder limit sensor, port side. 0 indicates off and 1 indicates that the limit is hit. If this sensor is inactive set to 0xFF.</field>
            <field type="uint8_t" name="starboard_limit">Status of the rudder limit sensor, starboard side. 0 indicates off and 1 indicates that the limit is hit. If this sensor is inactive set to 0xFF.</field>
        </message>
    </messages>
</mavlink>

3. 编绎XML文件

在完成消息定义文件后，它可被编绎成C代码，编绎步骤如下：

先下载一个代码生成器：https://github.com/mavlink/mavlink

然后运行里面的mavgenerate.py，运行这个文件需要先安装Python2.7+: 

http://rj.baidu.com/soft/detail/17016.html?ald

装载mavlink/message_definitions下的XML文件

选择输出路径，如mavlink/include

点击“Generate”

编绎完成后，以心跳消息为例，你会在相应的头文件中得到如下C结构体代码：

#define MAVLINK_MSG_ID_HEARTBEAT 0

typedef struct __mavlink_heartbeat_t
{
 uint32_t custom_mode; ///< Navigation mode bitfield, see MAV_AUTOPILOT_CUSTOM_MODE ENUM for some examples. This field is autopilot-specific.
 uint8_t type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
 uint8_t autopilot; ///< Autopilot type / class. defined in MAV_CLASS ENUM
 uint8_t base_mode; ///< System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
 uint8_t system_status; ///< System status flag, see MAV_STATUS ENUM
 uint8_t mavlink_version; ///< MAVLink version
} mavlink_heartbeat_t;

也会自动生成相应的打包函数

/**
   * @brief Pack a heartbeat message
 * @param system_id ID of this system
 * @param component_id ID of this component (e.g. 200 for IMU)
 * @param msg The MAVLink message to compress the data into
 *
 * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
 * @param autopilot Autopilot type / class. defined in MAV_CLASS ENUM
 * @param base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
 * @param custom_mode Navigation mode bitfield, see MAV_AUTOPILOT_CUSTOM_MODE ENUM for some examples. This field is autopilot-specific.
 * @param system_status System status flag, see MAV_STATUS ENUM
 * @return length of the message in bytes (excluding serial stream start sign)
 */
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)


/**
 * @brief Encode a heartbeat struct into a message
 *
 * @param system_id ID of this system
 * @param component_id ID of this component (e.g. 200 for IMU)
 * @param msg The MAVLink message to compress the data into
 * @param heartbeat C-struct to read the message contents from
 */
static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat)

当然也会生成解析函数

/**
 * @brief Decode a heartbeat message into a struct
 *
 * @param msg The message to decode
 * @param heartbeat C-struct to decode the message contents into
 */
static inline void mavlink_msg_heartbeat_decode(const mavlink_message_t* msg, mavlink_heartbeat_t* heartbeat)

八、消息的发送与解析

以心跳包为例，对如何发送消息和解析消息进行说明，首先打开mavlink_msg_heartbeat.h头文件，在这个头文件中包含了结构体的定义，打包函数，发送函数，接收函数和解析函数。我们通常要使用的是发送函数和接收函数。以下为发送函数

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 //采用大端模式，则需要进行byte swap
    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);

#if MAVLINK_CRC_EXTRA
    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
#else
    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN);
#endif
#else                                               // 小端模式（mavlink默认的模式）
    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;

#if MAVLINK_CRC_EXTRA
    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);// 将消息完整打包并发送
#else
    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_HEARTBEAT_LEN);
#endif
#endif
}

接下来进入打包发送函数，该函数位于mavlink_helper.h中

#if MAVLINK_CRC_EXTRA//默认
MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length, uint8_t crc_extra)
#else
MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length)
#endif
{
    uint16_t checksum;
    uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
    uint8_t ck[2];
    mavlink_status_t *status = mavlink_get_channel_status(chan);
    buf[0] = MAVLINK_STX;                   // 加入STX
    buf[1] = length;                        // 加入LEN
    buf[2] = status->current_tx_seq;        // 加入SEQ
    buf[3] = mavlink_system.sysid;          // 加入SYS
    buf[4] = mavlink_system.compid;         // 加入COMP
    buf[5] = msgid;                         // 加入MSG
    status->current_tx_seq++;               // 消息序列号加1
    checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);//计算非载荷的CRC校验码（不包含STX）
    crc_accumulate_buffer(&checksum, packet, length);//计算载荷的CRC校验码
#if MAVLINK_CRC_EXTRA
    crc_accumulate(crc_extra, &checksum);// 计算crc_extra的CRC校验码
#endif
    ck[0] = (uint8_t)(checksum & 0xFF);// 低8位CRC校验码
    ck[1] = (uint8_t)(checksum >> 8);  // 高8位CRC校验码

    MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);// 空宏，无作用
    _mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES);//向串口发送载荷之前的部分
    _mavlink_send_uart(chan, packet, length);//发送载荷部分
    _mavlink_send_uart(chan, (const char *)ck, 2);// 发送CRC校验码
    MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);// 空宏，无作用
}

到此，一个消息就发送完毕，接下来进入接收端的解析过程，消息的解析函数也位于mavlink_helper.h中

MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)

注意：解析时，会对从串口收到的每一个字节进行识别，如果为STX，则开始进行消息解码，将其以及此后的每个字节都存入一个消息结构体中，并计算对应的CRC校验码。如果最后的CRC校验码能匹配上，则将整个消息的数据存入r_message结构体中，如果解析完成一个消息，则会返回一个真值；然后继续检验字节是否为STX，循环解析。

解析完成后，可以对r_messsage结构体中的信息进行识别，r_message结构体就是相当于一个消息帧，包含了消息帧的所有信息：

typedef struct __mavlink_message {
    uint16_t checksum; ///< sent at end of packet
    uint8_t magic;   ///< protocol magic marker     是STX
    uint8_t len;     ///< Length of payload
    uint8_t seq;     ///< Sequence of packet
    uint8_t sysid;   ///< ID of message sender system/aircraft
    uint8_t compid;  ///< ID of the message sender component
    uint8_t msgid;   ///< ID of message in payload
    uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];// 除以8是因为uint64是8个字节的
} mavlink_message_t;

如果r_message中的msgid == MAVLINK_MSG_ID_HEARTBEAT,则可调用mavlink_msg_heartbeat.h头文件中的接收函数去得到相应要得到的变量，如

static inline uint8_t mavlink_msg_heartbeat_get_system_status(const mavlink_message_t* msg)
{
    return _MAV_RETURN_uint8_t(msg,  7);
}

调用该函数即可得到system_status,要得到其它变量的信息直接调用相关函数即可，至此，所有的发送和接收完成。

九、通用的Mavlink消息

可在这里查看：Common MAVLink Message Documentation 
在该网页中，包含了mavlink type的枚举和mavlink message，所有的消息编号也即ID，是以蓝色#加数字来表示的

 

参考资料

MavLink官方网站：http://qgroundcontrol.org/mavlink/start

Mavlink协议理解Pixhawk APM http://blog.csdn.net/super_mice/article/details/44836585

Mavlink-最强大的微型飞行器通信协议 http://bbs.loveuav.com/thread-36-1-1.html

维基百科：https://en.wikipedia.org/wiki/MAVLink

Mavlink微型飞行器的通信协议 http://wenku.baidu.com/view/44b9d4dd50e2524de5187eab.html?from=search

原文链接：https://blog.csdn.net/luckpl/article/details/52608868