无人机开发-通讯协议MavLink详解
1. MAVLink简介
MAVLink（Micro Air Vehicle Link，微型空中飞行器链路通讯协议）是无人飞行器与地面站（Ground Control Station ，GCS）之间通讯，以及无人飞行器之间通讯最常用的协议。它已经在PX4、APM、PIXHAWK和Parrot AR.Drone飞控平台上进行了大量测试。
2.发明者Lorenz Meier简介
MAVLink的最初开发于2009年，由Lorenz Meier完成。Lorenz Meier的LinkedIn主页是：https://www.linkedin.com/in/meierlorenz，个人主页是：https://www.inf.ethz.ch/personal/lomeier/。
根据官网和个人主页，Lorenz Meie的个人经历如下：
2004年~2008年在德国康斯坦茨大学（Universität Konstanz）就读信息工程专业；
2008 年~2011年在苏黎世联邦理工学院（德语：Eidgenössische Technische Hochschule Zürich，简称 ETH Zürich或ETHZ）就读视觉计算方向研究生；
2011~至今在ETHZ攻读博士后，研究方向是：Research on Drones and mobile phones focused on obstacle mapping, path planning and control.
2011年到现在，Lorenz Meier一直是开源无人机项目Autopilot的建立者和维护者。关于Autopilot，我会另辟章节介绍。
从介绍来看，Lorenz Meier的研究方向包括了无人机避障、基于智能手机或无人机的3D重建、无人机通讯协议等有趣又前言的内容。
这个页面是Lorenz Meier发表的几篇文章：https://www.researchgate.net/profile/Lorenz_Meier3
3. MAVLink相关资料
维基百科：https://en.wikipedia.org/wiki/MAVLink
MavLink官方网站：http://qgroundcontrol.org/mavlink/start
Python写的用于生成C、Java等语言的MavLink生成器软件：https://github.com/mavlink/mavlink
4.协议构成
下面内容引自官网。 
• The checksum is the same as used in ITU X.25 and SAE AS-4 standards (CRC-16-CCITT), documented in SAE AS5669A. Please see the MAVLink source code for a documented C-implementation of it. LINK TO CHECKSUM
• The minimum packet length is 8 bytes for acknowledgement packets without payload
• The maximum packet length is 263 bytes for full payload
MavLink的长度是固定的，即 17byte= 6 bytes header + 9 bytes payload + 2 bytes checksum。
5.封包过程
由用户生成的部分包括PlayLoad本身、消息包的STX、COMP、MSG，其他部分自动生成。下图来自于博客http://blog.csdn.net/u013983741/article/details/48053235，侵删。 
6.3DR Service实现MavLink协议
3DR Service是Autopilot提供的Android端的app服务，用于做SDK，提供与无人机通讯，以AIDL的方式为上层的App提供服务。基于3DRService，开发者可以不用处理复杂的MavLink通讯，只根据AIDL接口调用服务即可。
这里下载了3DRService用于分析，地址为：https://github.com/ne0fhyk/3DRServices。3DRService将MavLink的协议部分作为单独的包，即项目中的dependencyLibs文件夹。
6.1. UML图
绘制dependencyLibs的UML图。该包主要提供了MavLink的所有类型的封包类和解析类。 
例如，对于MAVLinkPacket类，其核心部分为封包过程。
/**
* Encode this packet for transmission.
*
* @return Array with bytes to be transmitted
*/
public byte[] encodePacket() {
byte[] buffer = new byte[6 + len + 2];
int i = 0;
buffer[i++] = (byte) MAVLINK_STX;
buffer[i++] = (byte) len;
buffer[i++] = (byte) seq;
buffer[i++] = (byte) sysid;
buffer[i++] = (byte) compid;
buffer[i++] = (byte) msgid;
final int payloadSize = payload.size();
for (int j = 0; j < payloadSize; j++) {
buffer[i++] = payload.payload.get(j);
}
generateCRC();
buffer[i++] = (byte) (crc.getLSB());
buffer[i++] = (byte) (crc.getMSB());
return buffer;
}解包的核心部分在Parser类中：
/**
* This is a convenience function which handles the complete MAVLink
* parsing. the function will parse one byte at a time and return the
* complete packet once it could be successfully decoded. Checksum and other
* failures will be silently ignored.
*
* @param c
* The char to parse
*/
public MAVLinkPacket mavlink_parse_char(int c) {
msg_received = false;
switch (state) {
case MAVLINK_PARSE_STATE_UNINIT:
case MAVLINK_PARSE_STATE_IDLE:
if (c == MAVLinkPacket.MAVLINK_STX) {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_STX;
}
break;
case MAVLINK_PARSE_STATE_GOT_STX:
if (msg_received) {
msg_received = false;
state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
} else {
m = new MAVLinkPacket(c);
state = MAV_states.MAVLINK_PARSE_STATE_GOT_LENGTH;
}
break;
case MAVLINK_PARSE_STATE_GOT_LENGTH:
m.seq = c;
state = MAV_states.MAVLINK_PARSE_STATE_GOT_SEQ;
break;
case MAVLINK_PARSE_STATE_GOT_SEQ:
m.sysid = c;
state = MAV_states.MAVLINK_PARSE_STATE_GOT_SYSID;
break;
case MAVLINK_PARSE_STATE_GOT_SYSID:
m.compid = c;
state = MAV_states.MAVLINK_PARSE_STATE_GOT_COMPID;
break;
case MAVLINK_PARSE_STATE_GOT_COMPID:
m.msgid = c;
if (m.len == 0) {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;
} else {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_MSGID;
}
break;
case MAVLINK_PARSE_STATE_GOT_MSGID:
m.payload.add((byte) c);
if (m.payloadIsFilled()) {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;
}
break;
case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
m.generateCRC();
// Check first checksum byte
if (c != m.crc.getLSB()) {
msg_received = false;
state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
if (c == MAVLinkPacket.MAVLINK_STX) {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_STX;
m.crc.start_checksum();
}
stats.crcError();
} else {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_CRC1;
}
break;
case MAVLINK_PARSE_STATE_GOT_CRC1:
// Check second checksum byte
if (c != m.crc.getMSB()) {
msg_received = false;
state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
if (c == MAVLinkPacket.MAVLINK_STX) {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_STX;
m.crc.start_checksum();
}
stats.crcError();
} else { // Successfully received the message
stats.newPacket(m);
msg_received = true;
state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
}
break;
}
if (msg_received) {
return m;
} else {
return null;
}
}
这个类的使用是逐个直接解析，解析完毕后返回完整的包，例如，对字节数组packet，解析过程如下：
for(int i = 0; i < packet.length - 1; i++){
parser.mavlink_parse_char(packet[i] & 0xFF);//每次解析1位
}
MAVLinkPacket m = parser.mavlink_parse_char(packet[packet.length - 1] & 0xFF);//最后1位即可返回6.2 MavLink包测试
dependencyLibs提供了测试实例。以msg_altitude为例，判断生成的包和解析的包是否相同，即可判断该类是否正确。
/**
* The current system altitude.
*/
public class msg_altitude_test{
public static final int MAVLINK_MSG_ID_ALTITUDE = 141;
public static final int MAVLINK_MSG_LENGTH = 24;
private static final long serialVersionUID = MAVLINK_MSG_ID_ALTITUDE;
private Parser parser = new Parser();//1位解析类
public CRC generateCRC(byte[] packet){
CRC crc = new CRC();
for (int i = 1; i < packet.length - 2; i++) {
crc.update_checksum(packet[i] & 0xFF);
}
crc.finish_checksum(MAVLINK_MSG_ID_ALTITUDE);
return crc;
}
public byte[] generateTestPacket(){
ByteBuffer payload = ByteBuffer.allocate(6 + MAVLINK_MSG_LENGTH + 2);
payload.put((byte)MAVLinkPacket.MAVLINK_STX); //stx
payload.put((byte)MAVLINK_MSG_LENGTH); //len
payload.put((byte)0); //seq
payload.put((byte)255); //sysid
payload.put((byte)190); //comp id
payload.put((byte)MAVLINK_MSG_ID_ALTITUDE); //msg id
payload.putFloat((float)17.0); //altitude_monotonic
payload.putFloat((float)45.0); //altitude_amsl
payload.putFloat((float)73.0); //altitude_local
payload.putFloat((float)101.0); //altitude_relative
payload.putFloat((float)129.0); //altitude_terrain
payload.putFloat((float)157.0); //bottom_clearance
CRC crc = generateCRC(payload.array());
payload.put((byte)crc.getLSB());
payload.put((byte)crc.getMSB());
return payload.array();
}
@Test
public void test(){
byte[] packet = generateTestPacket();
for(int i = 0; i < packet.length - 1; i++){
parser.mavlink_parse_char(packet[i] & 0xFF);//每次解析1位
}
MAVLinkPacket m = parser.mavlink_parse_char(packet[packet.length - 1] & 0xFF);//最后1位即可返回
byte[] processedPacket = m.encodePacket();//解析
assertArrayEquals("msg_altitude", processedPacket, packet);
}
}介绍mavlink所发送的数据结构。Mavlink传输时的基本单位是消息帧。
如图所示，每个消息帧都是上述的结构，除了灰色外，其他的格子都代表了一个字节的数据。
红色的是起始标志位（stx），在v1.0版本中以“FE”作为起始标志。这个标志位在mavlink消息帧接收端进行消息解码时有用处。
第二个格子代表的是灰色部分（payload，称作有效载荷，要用的数据在有效载荷里面）的字节长度（len），范围从0到255之间。在mavlink消息帧接收端可以用它和实际收到的有效载荷的长度比较，以验证有效载荷的长度是否正确。
第三个格子代表的是本次消息帧的序号（seq），每次发完一个消息，这个字节的内容会加1，加到255后会从0重新开始。这个序号用于mavlink消息帧接收端计算消息丢失比例用的，相当于是信号强度。
第四个格子代表了发送本条消息帧的设备的系统编号（sys），使用PIXHAWK刷PX4固件时默认的系统编号为1，用于mavlink消息帧接收端识别是哪个设备发来的消息。
第五个格子代表了发送本条消息帧的设备的单元编号（comp），使用PIXHAWK刷PX4固件时默认的单元编号为50，用于mavlink消息帧接收端识别是设备的哪个单元发来的消息（暂时没什么用） 。
第六个格子代表了有效载荷中消息包的编号（msg），注意它和序号是不同的，这个字节很重要，mavlink消息帧接收端要根据这个编号来确定有效载荷里到底放了什么消息包并根据编号选择对应的方式来处理有效载荷里的信息包。
最后两个字节是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协议。下节主要介绍mavlink里消息的种类和如何看懂开始时提到的那个官方的mavlink消息介绍。
参考
[1]http://mrsxm.mfzgi5lqnfwg65bomnxw2.erenta.ru/wp-content/uploads/sites/6/2015/05/MAVLINK_FOR_DUMMIESPart1_v.1.1.pdf
[2]http://blog.csdn.net/u013983741/article/details/48053235
可参考原文：http://blog.csdn.net/brillianteagle/article/details/50823346