【无人机开发】通讯协议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;
        }
}

`



 
 
  • 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();
}

@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);
}
}

参考

[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


评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值