【无人机开发】通讯协议MavLink详解

来源：互联网发布：优惠券网站php源码编辑：程序博客网时间：2024/05/17 23:42

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;        }}`1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108

这个类的使用是逐个直接解析，解析完毕后返回完整的包，例如，对字节数组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();}@Testpublic 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);}}

参考

[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

0 0