[置顶] Android地面站-MavLink解析部分源码

MavLink是轻量级的通讯协议,主要应用于终端与小型无人载具间的通讯。由于它的通用性,MavLink可以被翻译成各种语言的代码应用于各种不同的环境。具体如何通过工具来生成对应的MavLink代码请访问:

http://www.qgroundcontrol.org/mavlink/create_new_mavlink_message

MavLink协议所定义的消息,大致分为两类,一类是通用消息,另外一种是自定义消息。通用消息和自定义消息的数据结构相同,差异只体现在数据本身。我取MavLink中最常使用的心跳消息作为例子:

<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>

这里的心跳和push中的心跳是一个意思。由于网络环境的不确定性,加入心跳来保证长连接的可靠性。MavLink的消息定义以通用的XML格式为基准,并且根节点是<message>节点。消息ID从0~255,同时为了区分自定义消息。name属性定义了此消息的名称,description节点记录了此消息的用途。field节点用于记录消息中的域。uint8_t中的数字代表此域占用多少个bit。即uint8_t占用8个bit一个字节,而uint32_t占用32个bit,4个字节。各个域分别所代表的含义是:

1. type: 代表小型无人交通工具的类型,可能是直升机,汽车,多旋翼等等

2.autopilot: 代表此操作系统平台,平台的类型由MAV_TYPE类:

public class MAV_TYPE {
   public static final int MAV_TYPE_GENERIC = 0; /* Generic micro air vehicle. | */
   public static final int MAV_TYPE_FIXED_WING = 1; /* Fixed wing aircraft. | */
   public static final int MAV_TYPE_QUADROTOR = 2; /* Quadrotor | */
   public static final int MAV_TYPE_COAXIAL = 3; /* Coaxial helicopter | */
   public static final int MAV_TYPE_HELICOPTER = 4; /* Normal helicopter with tail rotor. | */
   public static final int MAV_TYPE_ANTENNA_TRACKER = 5; /* Ground installation | */
   public static final int MAV_TYPE_GCS = 6; /* Operator control unit / ground control station | */
   public static final int MAV_TYPE_AIRSHIP = 7; /* Airship, controlled | */
   public static final int MAV_TYPE_FREE_BALLOON = 8; /* Free balloon, uncontrolled | */
   public static final int MAV_TYPE_ROCKET = 9; /* Rocket | */
   public static final int MAV_TYPE_GROUND_ROVER = 10; /* Ground rover | */
   public static final int MAV_TYPE_SURFACE_BOAT = 11; /* Surface vessel, boat, ship | */
   public static final int MAV_TYPE_SUBMARINE = 12; /* Submarine | */
   public static final int MAV_TYPE_HEXAROTOR = 13; /* Hexarotor | */
   public static final int MAV_TYPE_OCTOROTOR = 14; /* Octorotor | */
   public static final int MAV_TYPE_TRICOPTER = 15; /* Octorotor | */
   public static final int MAV_TYPE_FLAPPING_WING = 16; /* Flapping wing | */
   public static final int MAV_TYPE_KITE = 17; /* Flapping wing | */
   public static final int MAV_TYPE_ONBOARD_CONTROLLER = 18; /* Onboard companion controller | */
   public static final int MAV_TYPE_VTOL_DUOROTOR = 19; /* Two-rotor VTOL using control surfaces in vertical operation in addition. Tailsitter. | */
   public static final int MAV_TYPE_VTOL_QUADROTOR = 20; /* Quad-rotor VTOL using a V-shaped quad config in vertical operation. Tailsitter. | */
   public static final int MAV_TYPE_VTOL_TILTROTOR = 21; /* Tiltrotor VTOL | */
   public static final int MAV_TYPE_VTOL_RESERVED2 = 22; /* VTOL reserved 2 | */
   public static final int MAV_TYPE_VTOL_RESERVED3 = 23; /* VTOL reserved 3 | */
   public static final int MAV_TYPE_VTOL_RESERVED4 = 24; /* VTOL reserved 4 | */
   public static final int MAV_TYPE_VTOL_RESERVED5 = 25; /* VTOL reserved 5 | */
   public static final int MAV_TYPE_GIMBAL = 26; /* Onboard gimbal | */
   public static final int MAV_TYPE_ADSB = 27; /* Onboard ADSB peripheral | */
   public static final int MAV_TYPE_ENUM_END = 28; /*  | */
}

3.base_mode:记录小型交通工具的基本模式

4.custom_mode:记录小型交工具的特征模式

5.mavlink_version:mavlink协议的版本号

大家可能好奇为什么有了个基本模式还有有个特征模式,原因是因为MavLink是要兼顾多种类型的小型交通工具的协议,这样的话,不能保证所有的基本模式覆盖到所有的交通器。

接下来,我们通过网站上的mavlink-generator 去生成一套java代码,用在我们的android程序中。

生成的代码移植性很好,我们可以无缝的直接copy到我们的android工程中。我们来看下生成的代码的分包:

common包: 放一些常用的MavLink消息和CRC校验工具

ardupilotmega包:存放针对mega板子特有的消息

Messages包:提供消息基本类和一些缓存处理类

enums包:存放一些常量

MAVLinkPacket类:用来记录原始报文

Parser类:用于解析信道中传递过来的数据,生成MAVLinkPacket格式的报文。

由于本篇的主题是MavLink消息在Android地面站的解析,因此我们不过分的关注于信道和业务本身。我们看上面的分包我们会发现,其实对于解析来说,最重要的就是Parser类。在我们开始解析前,通过一张图再回忆一下心跳消息的数据结构,因为我们将以它为样本作为例子:

我们收到的心跳完整报文是一个byte数组,因此我们需要对它进行解析,解析出我们自己的对象模型,就需要调用mavlink_parse_char(int c)方法。这就有个问题,我们明明读取到的是byte数组,但是方法中要我们传递一个int。这个原因我们不妨来看一下Parser这个类:

public class Parser {

    /**
     * States from the parsing state machine
     */
    enum MAV_states {
        MAVLINK_PARSE_STATE_UNINIT, MAVLINK_PARSE_STATE_IDLE, MAVLINK_PARSE_STATE_GOT_STX, MAVLINK_PARSE_STATE_GOT_LENGTH, MAVLINK_PARSE_STATE_GOT_SEQ, MAVLINK_PARSE_STATE_GOT_SYSID, MAVLINK_PARSE_STATE_GOT_COMPID, MAVLINK_PARSE_STATE_GOT_MSGID, MAVLINK_PARSE_STATE_GOT_CRC1, MAVLINK_PARSE_STATE_GOT_PAYLOAD
    }

    MAV_states state = MAV_states.MAVLINK_PARSE_STATE_UNINIT;

    private boolean msg_received;

    public MAVLinkStats stats = new MAVLinkStats();
    private MAVLinkPacket m;

    /**
     * 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;
        }
    }

}

我们发现,Parser类必须要线性的解析报文,也就是说,在同一个周期内,只能有一条消息在Parser类中处理。并且Parser类的方法结构本质上是一个状态机。外部代码需要遍历传入byte中的数据用于生成报文:

private void handleData(Parser parser, int bufferSize, byte[] buffer) {
            if (bufferSize < 1) {
                return;
            }

            for (int i = 0; i < bufferSize; i++) {
                int code = buffer[i] & 0x00ff;
                MAVLinkPacket receivedPacket = parser.mavlink_parse_char(code);
                if (receivedPacket != null) {
                    //test(receivedPacket);
                }
            }
        }

Parser类所具有的状态列表:

 enum MAV_states {
        MAVLINK_PARSE_STATE_UNINIT, MAVLINK_PARSE_STATE_IDLE, MAVLINK_PARSE_STATE_GOT_STX, MAVLINK_PARSE_STATE_GOT_LENGTH, MAVLINK_PARSE_STATE_GOT_SEQ, MAVLINK_PARSE_STATE_GOT_SYSID, MAVLINK_PARSE_STATE_GOT_COMPID, MAVLINK_PARSE_STATE_GOT_MSGID, MAVLINK_PARSE_STATE_GOT_CRC1, MAVLINK_PARSE_STATE_GOT_PAYLOAD
    }

前面说了Parser类本质上是一个状态机,初始的状态是:MAV_PARSE_STATE_UNINIT。一旦解析成功或者失败,状态将进入到IDLE。

Parser类的状态机基本可以使用上面的图片表示,基本上没有什么复杂的内容,主要的在与刚开始的数据长度的记录。如果你的数据长度大于零的话,解析器会将你的数据缓存在一个叫做payload的数据结构中。

 case MAVLINK_PARSE_STATE_GOT_MSGID:
            m.payload.add((byte) c);
            if (m.payloadIsFilled()) {
                state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;
            }
            break;

PayLoad对应的类是MAVLinkPayLoad类,他是数据的缓存器和转换器,就是将无意义的byte数组,组织成为有意义的平台数据类型。

public class MAVLinkPayload {

    private static final byte UNSIGNED_BYTE_MIN_VALUE = 0;
    private static final short UNSIGNED_BYTE_MAX_VALUE = Byte.MAX_VALUE - Byte.MIN_VALUE;

    private static final short UNSIGNED_SHORT_MIN_VALUE = 0;
    private static final int UNSIGNED_SHORT_MAX_VALUE = Short.MAX_VALUE - Short.MIN_VALUE;

    private static final int UNSIGNED_INT_MIN_VALUE = 0;
    private static final long UNSIGNED_INT_MAX_VALUE = (long) Integer.MAX_VALUE - Integer.MIN_VALUE;

    private static final long UNSIGNED_LONG_MIN_VALUE = 0;

    public static final int MAX_PAYLOAD_SIZE = 255;
    
    public final ByteBuffer payload;
    public int index;

    public MAVLinkPayload(int payloadSize) {
       if(payloadSize > MAX_PAYLOAD_SIZE) {
            payload = ByteBuffer.allocate(MAX_PAYLOAD_SIZE);
        } else {
            payload = ByteBuffer.allocate(payloadSize);
        }
    }

    public ByteBuffer getData() {
        return payload;
    }

    public int size() {
        return payload.position();
    }

    public void add(byte c) {
        payload.put(c);
    }

    public void resetIndex() {
        index = 0;
    }

    public byte getByte() {
        byte result = 0;
        result |= (payload.get(index + 0) & 0xFF);
        index += 1;
        return result;
    }

    public short getUnsignedByte(){
        short result = 0;
        result |= payload.get(index + 0) & 0xFF;
        index+= 1;
        return result; 
    }

    public short getShort() {
        short result = 0;
        result |= (payload.get(index + 1) & 0xFF) << 8;
        result |= (payload.get(index + 0) & 0xFF);
        index += 2;
        return result;
    }

    public int getUnsignedShort(){
        int result = 0;
        result |= (payload.get(index + 1) & 0xFF) << 8;
        result |= (payload.get(index + 0) & 0xFF);
        index += 2;
        return result;
    }

    public int getInt() {
        int result = 0;
        result |= (payload.get(index + 3) & 0xFF) << 24;
        result |= (payload.get(index + 2) & 0xFF) << 16;
        result |= (payload.get(index + 1) & 0xFF) << 8;
        result |= (payload.get(index + 0) & 0xFF);
        index += 4;
        return result;
    }

    public long getUnsignedInt(){
        long result = 0;
        result |= (payload.get(index + 3) & 0xFFFFL) << 24;
        result |= (payload.get(index + 2) & 0xFFFFL) << 16;
        result |= (payload.get(index + 1) & 0xFFFFL) << 8;
        result |= (payload.get(index + 0) & 0xFFFFL);
        index += 4;
        return result;
    }

    public long getLong() {
        long result = 0;
        result |= (payload.get(index + 7) & 0xFFFFL) << 56;
        result |= (payload.get(index + 6) & 0xFFFFL) << 48;
        result |= (payload.get(index + 5) & 0xFFFFL) << 40;
        result |= (payload.get(index + 4) & 0xFFFFL) << 32;
        result |= (payload.get(index + 3) & 0xFFFFL) << 24;
        result |= (payload.get(index + 2) & 0xFFFFL) << 16;
        result |= (payload.get(index + 1) & 0xFFFFL) << 8;
        result |= (payload.get(index + 0) & 0xFFFFL);
        index += 8;
        return result;
    }

    public long getUnsignedLong(){
        return getLong();
    }
    
    public long getLongReverse() {
        long result = 0;
        result |= (payload.get(index + 0) & 0xFFFFL) << 56;
        result |= (payload.get(index + 1) & 0xFFFFL) << 48;
        result |= (payload.get(index + 2) & 0xFFFFL) << 40;
        result |= (payload.get(index + 3) & 0xFFFFL) << 32;
        result |= (payload.get(index + 4) & 0xFFFFL) << 24;
        result |= (payload.get(index + 5) & 0xFFFFL) << 16;
        result |= (payload.get(index + 6) & 0xFFFFL) << 8;
        result |= (payload.get(index + 7) & 0xFFFFL);
        index += 8;
        return result;
    }

    public float getFloat() {
        return Float.intBitsToFloat(getInt());
    }
    
    public void putByte(byte data) {
        add(data);
    }

    public void putUnsignedByte(short data){
        if(data < UNSIGNED_BYTE_MIN_VALUE || data > UNSIGNED_BYTE_MAX_VALUE){
            throw new IllegalArgumentException("Value is outside of the range of an unsigned byte: " + data);
        }

        putByte((byte) data);
    }

    public void putShort(short data) {
        add((byte) (data >> 0));
        add((byte) (data >> 8));
    }

    public void putUnsignedShort(int data){
        if(data < UNSIGNED_SHORT_MIN_VALUE || data > UNSIGNED_SHORT_MAX_VALUE){
            throw new IllegalArgumentException("Value is outside of the range of an unsigned short: " + data);
        }

        putShort((short) data);
    }

    public void putInt(int data) {
        add((byte) (data >> 0));
        add((byte) (data >> 8));
        add((byte) (data >> 16));
        add((byte) (data >> 24));
    }

    public void putUnsignedInt(long data){
        if(data < UNSIGNED_INT_MIN_VALUE || data > UNSIGNED_INT_MAX_VALUE){
            throw new IllegalArgumentException("Value is outside of the range of an unsigned int: " + data);
        }

        putInt((int) data);
    }

    public void putLong(long data) {
        add((byte) (data >> 0));
        add((byte) (data >> 8));
        add((byte) (data >> 16));
        add((byte) (data >> 24));
        add((byte) (data >> 32));
        add((byte) (data >> 40));
        add((byte) (data >> 48));
        add((byte) (data >> 56));
    }

    public void putUnsignedLong(long data){
        if(data < UNSIGNED_LONG_MIN_VALUE){
            throw new IllegalArgumentException("Value is outside of the range of an unsigned long: " + data);
        }

        putLong(data);
    }

    public void putFloat(float data) {
        putInt(Float.floatToIntBits(data));
    }

}

这个类的复用性很高,我们在很多解析器里面都可以用到它,希望大家以后如果写自己的解析器的话可以想到它。好的,我们现在有了数据Payload我们怎么解析出消息呢？

我们回到我们的Packet类,Packet用了一个很典型的命名unpack来用来解包:

 public MAVLinkMessage unpack() {
        switch (msgid) {
                         
            case msg_sensor_offsets.MAVLINK_MSG_ID_SENSOR_OFFSETS:
                return  new msg_sensor_offsets(this);
                 
            case msg_set_mag_offsets.MAVLINK_MSG_ID_SET_MAG_OFFSETS:
                return  new msg_set_mag_offsets(this);
                 
            case msg_meminfo.MAVLINK_MSG_ID_MEMINFO:
                return  new msg_meminfo(this);
......
}

如果你自定义了一种MavLink协议类型的话,代码生成器会自动帮你生成一个case和一个消息类,而在这里,我们找到我们所需要的心跳类case:

case msg_heartbeat.MAVLINK_MSG_ID_HEARTBEAT:
                return  new msg_heartbeat(this);

在心跳消息的构造器里,具体消息类型会对报文的具体内容做真正的解包:

public void unpack(MAVLinkPayload payload) {
        payload.resetIndex();
              
        this.custom_mode = payload.getUnsignedInt();
              
        this.type = payload.getUnsignedByte();
              
        this.autopilot = payload.getUnsignedByte();
              
        this.base_mode = payload.getUnsignedByte();
              
        this.system_status = payload.getUnsignedByte();
              
        this.mavlink_version = payload.getUnsignedByte();
        
    }

这样,记录在payload中杂乱的数据就被记录在msg_heartbeat类的变量中啦~

大家如果感兴趣,就自己去生成和阅读它的代码,代码量很少很好读懂,且通用性很好很好调试。