MAVLink分析及封装自定义的类MavLink通信协议

        MAVLink是一种非常轻量级的消息传输协议, 用于地面控制终端（地面站）与无人机之间 (以及机载无人机组件之间) 进行通信。

        MAVLink官方地址

  一、自动生成mavlink协议代码：

        1、安装MavLink环境；

        2、生成 MAVLink 库文件；

        3、装好环境后，利用mavgenerate把以下的demo.xml生成对应的mavlink JAVA/C/C++代码

<?xml version="1.0"?>
<mavlink>

  <version>1</version>
 <enums>
    <enum name="SYS_STATE">
      <description>System state.</description>
      <entry value="0" name="SYS_STOP">
        <description>System stop state</description>
      </entry>
      <entry value="1" name="SYS_RUN">
        <description>System run state</description>
      </entry>
    </enum>
  </enums>

  <messages>
    <message id="150" name="SysInfo">
      <description>System information.</description>
      <field type="uint8_t" name="sysState" >System state.</field>
      <field type="uint16_t" name="voltageBattery">Battery voltage</field>
    </message>
	
  </messages>
	
</mavlink>

        生成的代码在项目MyMavlinkDemoModulede的heartbeat中。

        二、MavLink2帧格式：

        

         以上是MavLink的一帧格式，整个协议组包和解包过程都是围绕着这帧进行的

        三、自动生成mavlink的Java/C/C++代码分析：

                协议握手图：

        

        我们以Java相关的代码为GCS端，以C/C++相关代码为Drone端为例。

        GCS端Java代码分析：

        工具类MAVLinkMessage、MAVLinkPayload、MAVLinkPacket、Paser对象协议的封装起到至关重要的作用，而msg_sysinfo是具体到每条有效的命令。

•  MAVLinkMessage 类定义了 sysid、compid、msgid的帧头的属性，提供了pack()对MAVLinkPacket的组装和unpack()剥离出命令中的属性的两个方法：

/* AUTO-GENERATED FILE.  DO NOT MODIFY.
 *
 * This class was automatically generated by the
 * java mavlink generator tool. It should not be modified by hand.
 */

package com.MAVLink.Messages;

import java.io.Serializable;

import com.MAVLink.MAVLinkPacket;

/**
 * Common interface for all MAVLink messages
 */
public abstract class MAVLinkMessage implements Serializable {
    private static final long serialVersionUID = -7754622750478538539L;
    // The MAVLink message classes have been changed to implement Serializable, 
    // this way is possible to pass a mavlink message through the Service-Acctivity interface

    public int sysid;

    public int compid;
    public int msgid;
    public boolean isMavlink2;

    public abstract MAVLinkPacket pack();
    public abstract void unpack(MAVLinkPayload payload);
    public abstract String toString();
    public abstract String name();
}

• msg_sysinfo类继承了MAVLinkMessage ，属性voltageBattery、sysState对象的是XML中标签<field>的两个命令值。

// MESSAGE SysInfo PACKING
package com.MAVLink.Demo_heart;
import com.MAVLink.MAVLinkPacket;
import com.MAVLink.Messages.MAVLinkMessage;
import com.MAVLink.Messages.MAVLinkPayload;
import com.MAVLink.Messages.Units;
import com.MAVLink.Messages.Description;


public class msg_sysinfo extends MAVLinkMessage {

    public static final int MAVLINK_MSG_ID_SysInfo = 150;
    public static final int MAVLINK_MSG_LENGTH = 3;
    private static final long serialVersionUID = MAVLINK_MSG_ID_SysInfo;

    
 
    @Description("Battery voltage")
    @Units("")
    public int voltageBattery;
    
  
    @Description("System state.")
    @Units("")
    public short sysState;
    


    @Override
    public MAVLinkPacket pack() {
        MAVLinkPacket packet = new MAVLinkPacket(MAVLINK_MSG_LENGTH,isMavlink2);
        packet.sysid = sysid;
        packet.compid = compid;
        packet.msgid = MAVLINK_MSG_ID_SysInfo;

        packet.payload.putUnsignedShort(voltageBattery);
        packet.payload.putUnsignedByte(sysState);
        
        if (isMavlink2) {
            
        }
        return packet;
    }

 
    @Override
    public void unpack(MAVLinkPayload payload) {
        payload.resetIndex();

        this.voltageBattery = payload.getUnsignedShort();
        this.sysState = payload.getUnsignedByte();
        
        if (isMavlink2) {
            
        }
    }

 
    public msg_sysinfo() {
        this.msgid = MAVLINK_MSG_ID_SysInfo;
    }


    public msg_sysinfo( int voltageBattery, short sysState) {
        this.msgid = MAVLINK_MSG_ID_SysInfo;

        this.voltageBattery = voltageBattery;
        this.sysState = sysState;
        
    }

  
    public msg_sysinfo( int voltageBattery, short sysState, int sysid, int compid, boolean isMavlink2) {
        this.msgid = MAVLINK_MSG_ID_SysInfo;
        this.sysid = sysid;
        this.compid = compid;
        this.isMavlink2 = isMavlink2;

        this.voltageBattery = voltageBattery;
        this.sysState = sysState;
        
    }


    public msg_sysinfo(MAVLinkPacket mavLinkPacket) {
        this.msgid = MAVLINK_MSG_ID_SysInfo;

        this.sysid = mavLinkPacket.sysid;
        this.compid = mavLinkPacket.compid;
        this.isMavlink2 = mavLinkPacket.isMavlink2;
        unpack(mavLinkPacket.payload);
    }

        

    @Override
    public String toString() {
        return "MAVLINK_MSG_ID_SysInfo - sysid:"+sysid+" compid:"+compid+" voltageBattery:"+voltageBattery+" sysState:"+sysState+"";
    }

 
    @Override
    public String name() {
        return "MAVLINK_MSG_ID_SysInfo";
    }
}
        

• MAVLinkPayload类对属性中的帧的有效负载payload做了数值的定义及拼装，例：在组帧pack()方法中对sysState的数据类型及消息命令顺序位置偏移。

package com.MAVLink.Messages;

import java.nio.ByteBuffer;

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() {
       // This has to be larger than the received payloadSize since MAVLINK V2 will truncate the payloads to the last non-zero value
       payload = ByteBuffer.allocate(MAX_PAYLOAD_SIZE);
    }

    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) & 0xFFL) << 24;
        result |= (payload.get(index + 2) & 0xFFL) << 16;
        result |= (payload.get(index + 1) & 0xFFL) << 8;
        result |= (payload.get(index + 0) & 0xFFL);
        index += 4;
        return result;
    }

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

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