devbox
从前慢现在也慢
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

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

作者：从前慢现在也慢 | 2024-06-08 07:05:21

赞

踩

mavlink java开发

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

        MAVLink官方地址

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

        1、安装MavLink环境；

        2、生成 MAVLink 库文件；

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

  1. <?xml version="1.0"?>
  2. <mavlink>
  3.  
  4.   <version>1</version>
  5.  <enums>
  6.     <enum name="SYS_STATE">
  7.       <description>System state.</description>
  8.       <entry value="0" name="SYS_STOP">
  9.         <description>System stop state</description>
  10.       </entry>
  11.       <entry value="1" name="SYS_RUN">
  12.         <description>System run state</description>
  13.       </entry>
  14.     </enum>
  15.   </enums>
  16.  
  17.   <messages>
  18.     <message id="150" name="SysInfo">
  19.       <description>System information.</description>
  20.       <field type="uint8_t" name="sysState" >System state.</field>
  21.       <field type="uint16_t" name="voltageBattery">Battery voltage</field>
  22.     </message>
  23. 	
  24.   </messages>
  25. 	
  26. </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()剥离出命令中的属性的两个方法：
  1. /* AUTO-GENERATED FILE.  DO NOT MODIFY.
  2.  *
  3.  * This class was automatically generated by the
  4.  * java mavlink generator tool. It should not be modified by hand.
  5.  */
  6.  
  7. package com.MAVLink.Messages;
  8.  
  9. import java.io.Serializable;
  10.  
  11. import com.MAVLink.MAVLinkPacket;
  12.  
  13. /**
  14.  * Common interface for all MAVLink messages
  15.  */
  16. public abstract class MAVLinkMessage implements Serializable {
  17.     private static final long serialVersionUID = -7754622750478538539L;
  18.     // The MAVLink message classes have been changed to implement Serializable, 
  19.     // this way is possible to pass a mavlink message through the Service-Acctivity interface
  20.  
  21.     public int sysid;
  22.  
  23.     public int compid;
  24.     public int msgid;
  25.     public boolean isMavlink2;
  26.  
  27.     public abstract MAVLinkPacket pack();
  28.     public abstract void unpack(MAVLinkPayload payload);
  29.     public abstract String toString();
  30.     public abstract String name();
  31. }
  • msg_sysinfo类继承了MAVLinkMessage ，属性voltageBattery、sysState对象的是XML中标签<field>的两个命令值。
  1. // MESSAGE SysInfo PACKING
  2. package com.MAVLink.Demo_heart;
  3. import com.MAVLink.MAVLinkPacket;
  4. import com.MAVLink.Messages.MAVLinkMessage;
  5. import com.MAVLink.Messages.MAVLinkPayload;
  6. import com.MAVLink.Messages.Units;
  7. import com.MAVLink.Messages.Description;
  8.  
  9.  
  10. public class msg_sysinfo extends MAVLinkMessage {
  11.  
  12.     public static final int MAVLINK_MSG_ID_SysInfo = 150;
  13.     public static final int MAVLINK_MSG_LENGTH = 3;
  14.     private static final long serialVersionUID = MAVLINK_MSG_ID_SysInfo;
  15.  
  16.     
  17.  
  18.     @Description("Battery voltage")
  19.     @Units("")
  20.     public int voltageBattery;
  21.     
  22.   
  23.     @Description("System state.")
  24.     @Units("")
  25.     public short sysState;
  26.     
  27.  
  28.  
  29.     @Override
  30.     public MAVLinkPacket pack() {
  31.         MAVLinkPacket packet = new MAVLinkPacket(MAVLINK_MSG_LENGTH,isMavlink2);
  32.         packet.sysid = sysid;
  33.         packet.compid = compid;
  34.         packet.msgid = MAVLINK_MSG_ID_SysInfo;
  35.  
  36.         packet.payload.putUnsignedShort(voltageBattery);
  37.         packet.payload.putUnsignedByte(sysState);
  38.         
  39.         if (isMavlink2) {
  40.             
  41.         }
  42.         return packet;
  43.     }
  44.  
  45.  
  46.     @Override
  47.     public void unpack(MAVLinkPayload payload) {
  48.         payload.resetIndex();
  49.  
  50.         this.voltageBattery = payload.getUnsignedShort();
  51.         this.sysState = payload.getUnsignedByte();
  52.         
  53.         if (isMavlink2) {
  54.             
  55.         }
  56.     }
  57.  
  58.  
  59.     public msg_sysinfo() {
  60.         this.msgid = MAVLINK_MSG_ID_SysInfo;
  61.     }
  62.  
  63.  
  64.     public msg_sysinfo( int voltageBattery, short sysState) {
  65.         this.msgid = MAVLINK_MSG_ID_SysInfo;
  66.  
  67.         this.voltageBattery = voltageBattery;
  68.         this.sysState = sysState;
  69.         
  70.     }
  71.  
  72.   
  73.     public msg_sysinfo( int voltageBattery, short sysState, int sysid, int compid, boolean isMavlink2) {
  74.         this.msgid = MAVLINK_MSG_ID_SysInfo;
  75.         this.sysid = sysid;
  76.         this.compid = compid;
  77.         this.isMavlink2 = isMavlink2;
  78.  
  79.         this.voltageBattery = voltageBattery;
  80.         this.sysState = sysState;
  81.         
  82.     }
  83.  
  84.  
  85.     public msg_sysinfo(MAVLinkPacket mavLinkPacket) {
  86.         this.msgid = MAVLINK_MSG_ID_SysInfo;
  87.  
  88.         this.sysid = mavLinkPacket.sysid;
  89.         this.compid = mavLinkPacket.compid;
  90.         this.isMavlink2 = mavLinkPacket.isMavlink2;
  91.         unpack(mavLinkPacket.payload);
  92.     }
  93.  
  94.         
  95.  
  96.     @Override
  97.     public String toString() {
  98.         return "MAVLINK_MSG_ID_SysInfo - sysid:"+sysid+" compid:"+compid+" voltageBattery:"+voltageBattery+" sysState:"+sysState+"";
  99.     }
  100.  
  101.  
  102.     @Override
  103.     public String name() {
  104.         return "MAVLINK_MSG_ID_SysInfo";
  105.     }
  106. }
  • MAVLinkPayload类对属性中的帧的有效负载payload做了数值的定义及拼装，例：在组帧pack()方法中对sysState的数据类型及消息命令顺序位置偏移。
  1. package com.MAVLink.Messages;
  2.  
  3. import java.nio.ByteBuffer;
  4.  
  5. public class MAVLinkPayload {
  6.  
  7.     private static final byte UNSIGNED_BYTE_MIN_VALUE = 0;
  8.     private static final short UNSIGNED_BYTE_MAX_VALUE = Byte.MAX_VALUE - Byte.MIN_VALUE;
  9.  
  10.     private static final short UNSIGNED_SHORT_MIN_VALUE = 0;
  11.     private static final int UNSIGNED_SHORT_MAX_VALUE = Short.MAX_VALUE - Short.MIN_VALUE;
  12.  
  13.     private static final int UNSIGNED_INT_MIN_VALUE = 0;
  14.     private static final long UNSIGNED_INT_MAX_VALUE = (long) Integer.MAX_VALUE - Integer.MIN_VALUE;
  15.  
  16.     private static final long UNSIGNED_LONG_MIN_VALUE = 0;
  17.  
  18.     public static final int MAX_PAYLOAD_SIZE = 255;
  19.     
  20.     public final ByteBuffer payload;
  21.     public int index;
  22.  
  23.     public MAVLinkPayload() {
  24.        // This has to be larger than the received payloadSize since MAVLINK V2 will truncate the payloads to the last non-zero value
  25.        payload = ByteBuffer.allocate(MAX_PAYLOAD_SIZE);
  26.     }
  27.  
  28.     public ByteBuffer getData() {
  29.         return payload;
  30.     }
  31.  
  32.     public int size() {
  33.         return payload.position();
  34.     }
  35.  
  36.     public void add(byte c) {
  37.         payload.put(c);
  38.     }
  39.  
  40.     public void resetIndex() {
  41.         index = 0;
  42.     }
  43.  
  44.     public byte getByte() {
  45.         byte result = 0;
  46.         result |= (payload.get(index + 0) & 0xFF);
  47.         index += 1;
  48.         return result;
  49.     }
  50.  
  51.     public short getUnsignedByte() {
  52.         short result = 0;
  53.         result |= payload.get(index + 0) & 0xFF;
  54.         index += 1;
  55.         return result; 
  56.     }
  57.  
  58.     public short getShort() {
  59.         short result = 0;
  60.         result |= (payload.get(index + 1) & 0xFF) << 8;
  61.         result |= (payload.get(index + 0) & 0xFF);
  62.         index += 2;
  63.         return result;
  64.     }
  65.  
  66.     public int getUnsignedShort() {
  67.         int result = 0;
  68.         result |= (payload.get(index + 1) & 0xFF) << 8;
  69.         result |= (payload.get(index + 0) & 0xFF);
  70.         index += 2;
  71.         return result;
  72.     }
  73.  
  74.     public int getInt() {
  75.         int result = 0;
  76.         result |= (payload.get(index + 3) & 0xFF) << 24;
  77.         result |= (payload.get(index + 2) & 0xFF) << 16;
  78.         result |= (payload.get(index + 1) & 0xFF) << 8;
  79.         result |= (payload.get(index + 0) & 0xFF);
  80.         index += 4;
  81.         return result;
  82.     }
  83.  
  84.     public long getUnsignedInt() {
  85.         long result = 0;
  86.         result |= (payload.get(index + 3) & 0xFFL) << 24;
  87.         result |= (payload.get(index + 2) & 0xFFL) << 16;
  88.         result |= (payload.get(index + 1) & 0xFFL) << 8;
  89.         result |= (payload.get(index + 0) & 0xFFL);
  90.         index += 4;
  91.         return result;
  92.     }
  93.  
  94.     public long getLong() {
  95.         long result = 0;
  96.         result |= (payload.get(index + 7) & 0xFFL) << 56;
  97.         result |= (payload.get(index + 6) & 0xFFL) << 48;
  98.         result |= (payload.get(index + 5) & 0xFFL) << 40;
  99.         result |= (payload.get(index + 4) & 0xFFL) << 32;
  100.         result |= (payload.get(index + 3) & 0xFFL) << 24;
  101.         result |= (payload.get(index + 2) & 0xFFL) << 16;
  102.         result |= (payload.get(index + 1) & 0xFFL) << 8;
  103.         result |= (payload.get(index + 0) & 0xFFL);
  104.         index += 8;
  105.         return result;
  106.     }
  107.  
  108.     public long getUnsignedLong(){
  109.         return getLong();
  110.     }
  111.     
  112.     public long getLongReverse() {
  113.         long result = 0;
  114.         result |= (payload.get(index + 0) & 0xFFL) << 56;
  115.         result |= (payload.get(index + 1) & 0xFFL) << 48;
  116.         result |= (payload.get(index + 2) & 0xFFL) << 40;
  117.         result |= (payload.get(index + 3) & 0xFFL) << 32;
  118.         result |= (payload.get(index + 4) & 0xFFL) << 24;
  119.         result |= (payload.get(index + 5) & 0xFFL) << 16;
  120.         result |= (payload.get(index + 6) & 0xFFL) << 8;
  121.         result |= (payload.get(index + 7) & 0xFFL);
  122.         index += 8;
  123.         return result;
  124.     }
  125.  
  126.     public float getFloat() {
  127.         return Float.intBitsToFloat(getInt());
  128.     }
  129.  
  130.     public double getDouble() {
  131.         return Double.longBitsToDouble(getLong());
  132.     }   
  133.     
  134.     public void putByte(byte data) {
  135.         add(data);
  136.     }
  137.  
  138.     public void putUnsignedByte(short data) {
  139.         if (data < UNSIGNED_BYTE_MIN_VALUE || data > UNSIGNED_BYTE_MAX_VALUE) {
  140.             throw new IllegalArgumentException("Value is outside of the range of an unsigned byte: " + data);
  141.         }
  142.  
  143.         putByte((byte) data);
  144.     }
  145.  
  146.     public void putShort(short data) {
  147.         add((byte) (data >> 0));
  148.         add((byte) (data >> 8));
  149.     }
  150.  
  151.     public void putUnsignedShort(int data) {
  152.         if (data < UNSIGNED_SHORT_MIN_VALUE || data > UNSIGNED_SHORT_MAX_VALUE) {
  153.             throw new IllegalArgumentException("Value is outside of the range of an unsigned short: " + data);
  154.         }
  155.  
  156.         putShort((short) data);
  157.     }
  158.  
  159.     public void putInt(int data) {
  160.         add((byte) (data >> 0));
  161.         add((byte) (data >> 8));
  162.         add((byte) (data >> 16));
  163.         add((byte) (data >> 24));
  164.     }
  165.  
  166.     public void putUnsignedInt(long data) {
  167.         if (data < UNSIGNED_INT_MIN_VALUE || data > UNSIGNED_INT_MAX_VALUE) {
  168.             throw new IllegalArgumentException("Value is outside of the range of an unsigned int: " + data);
  169.         }
  170.  
  171.         putInt((int) data);
  172.     }
  173.  
  174.     public void putLong(long data) {
  175.         add((byte) (data >> 0));
  176.         add((byte) (data >> 8));
  177.         add((byte) (data >> 16));
  178.         add((byte) (data >> 24));
  179.         add((byte) (data >> 32));
  180.         add((byte) (data >> 40));
  181.         add((byte) (data >> 48));
  182.         add((byte) (data >> 56));
  183.     }
  184.  
  185.     public void putUnsignedLong(long data) {
  186.         if (data < UNSIGNED_LONG_MIN_VALUE) {
  187.             throw new IllegalArgumentException("Value is outside of the range of an unsigned long: " + data);
  188.         }
  189.  
  190.         putLong(data);
  191.     }
  192.  
  193.     public void putFloat(float data) {
  194.         putInt(Float.floatToIntBits(data));
  195.     }
  196.  
  197.     public void putDouble(double data) {
  198.         putLong(Double.doubleToLongBits(data));
  199.     }
  200.  
  201. }
  • Parser类对来自Drone端的字节数据流解析成一个完整的帧，mavlink_parse_char(int c)方法把Drone端组装成mavlink帧即MAVLinkPacket：
  1. package com.MAVLink;
  2. import com.MAVLink.MAVLinkPacket;
  3. import com.MAVLink.Messages.MAVLinkStats;
  4.  
  5. public class Parser {
  6.     static final String TAG = Parser.class.getSimpleName();
  7.     static final boolean V = false;
  8.  
  9.     static void logv(String tag, String msg) {
  10.         if(V) System.out.println(String.format("%s: %s", tag, msg));
  11.     }
  12.  
  13.  
  14.     enum MAV_states {
  15.         MAVLINK_PARSE_STATE_UNINIT,
  16.         MAVLINK_PARSE_STATE_IDLE,
  17.         MAVLINK_PARSE_STATE_GOT_STX,
  18.         MAVLINK_PARSE_STATE_GOT_LENGTH,
  19.         MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS, // MAVLink 2
  20.         MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS, // MAVLink 2
  21.         MAVLINK_PARSE_STATE_GOT_SEQ,
  22.         MAVLINK_PARSE_STATE_GOT_SYSID,
  23.         MAVLINK_PARSE_STATE_GOT_COMPID,
  24.         MAVLINK_PARSE_STATE_GOT_MSGID1,
  25.         MAVLINK_PARSE_STATE_GOT_MSGID2, // MAVLink 2
  26.         MAVLINK_PARSE_STATE_GOT_MSGID3, // MAVLink 2
  27.         MAVLINK_PARSE_STATE_GOT_CRC1,
  28.         MAVLINK_PARSE_STATE_GOT_CRC2, // MAVLink 2
  29.         MAVLINK_PARSE_STATE_GOT_PAYLOAD,
  30.         MAVLINK_PARSE_STATE_GOT_SIGNATURE, // MAVLink 2
  31.     }
  32.  
  33.     private MAV_states state = MAV_states.MAVLINK_PARSE_STATE_UNINIT;
  34.  
  35.     public MAVLinkStats stats;
  36.     private MAVLinkPacket m;
  37.     private boolean isMavlink2;
  38.  
  39.     public Parser() {
  40.         this(false);
  41.     }
  42.  
  43.     public Parser(boolean ignoreRadioPacketStats) {
  44.         stats = new MAVLinkStats(ignoreRadioPacketStats);
  45.         isMavlink2 = false;
  46.     }
  47.  
  48.     public MAVLinkPacket mavlink_parse_char(int c) {
  49.  
  50.         // force to 8 bits
  51.         c &= 0xFF;
  52.  
  53.         switch (state) {
  54.             case MAVLINK_PARSE_STATE_UNINIT:
  55.             case MAVLINK_PARSE_STATE_IDLE:
  56.                 // MAVLink 1 and 2
  57.                 if (c == MAVLinkPacket.MAVLINK_STX_MAVLINK2) {
  58.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_STX;
  59.                     if (!isMavlink2) {
  60.                         isMavlink2 = true;
  61.                         logv(TAG, "Turning mavlink2 ON");
  62.                     }
  63.                 } else if (c == MAVLinkPacket.MAVLINK_STX_MAVLINK1) {
  64.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_STX;
  65.                     if (isMavlink2) {
  66.                         isMavlink2 = false;
  67.                         logv(TAG, "Turning mavlink2 OFF");
  68.                     }
  69.                 }
  70.                 break;
  71.  
  72.             case MAVLINK_PARSE_STATE_GOT_STX:
  73.                 // MAVLink 1 and 2
  74.                 m = new MAVLinkPacket(c, isMavlink2);
  75.                 if (isMavlink2) {
  76.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_LENGTH;
  77.                 } else {
  78.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
  79.                 }
  80.                 break;
  81.  
  82.             case MAVLINK_PARSE_STATE_GOT_LENGTH:
  83.                 // MAVLink 1 and 2
  84.                 m.incompatFlags = c;
  85.                 if (c != 0 && c != 1) {
  86.                     // message includes an incompatible feature flag
  87.                     state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
  88.                     break;
  89.                 }
  90.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS;
  91.                 break;
  92.  
  93.             case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS:
  94.                 // MAVLink 2 only
  95.                 m.compatFlags = c;
  96.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
  97.                 break;
  98.  
  99.             case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS:
  100.                 m.seq = c;
  101.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_SEQ;
  102.                 break;
  103.  
  104.             case MAVLINK_PARSE_STATE_GOT_SEQ:
  105.                 // back to MAVLink 1 and 2
  106.                 m.sysid = c;
  107.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_SYSID;
  108.                 break;
  109.  
  110.             case MAVLINK_PARSE_STATE_GOT_SYSID:
  111.                 // MAVLink 1 and 2
  112.                 m.compid = c;
  113.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_COMPID;
  114.                 break;
  115.  
  116.             case MAVLINK_PARSE_STATE_GOT_COMPID:
  117.                 // MAVLink 1 and 2
  118.                 m.msgid = c;
  119.                 if (isMavlink2) {
  120.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_MSGID1;
  121.                 } else if (m.len > 0) {
  122.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_MSGID3;
  123.                 } else {
  124.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;
  125.                 }
  126.                 break;
  127.  
  128.             case MAVLINK_PARSE_STATE_GOT_MSGID1:
  129.                 // MAVLink 2 only
  130.                 m.msgid |= c << 8;
  131.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_MSGID2;
  132.                 break;
  133.  
  134.             case MAVLINK_PARSE_STATE_GOT_MSGID2:
  135.                 // MAVLink 2 only
  136.                 m.msgid |= c << 16;
  137.                 if (m.len > 0) {
  138.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_MSGID3;
  139.                 } else {
  140.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;
  141.                 }
  142.                 break;
  143.  
  144.             case MAVLINK_PARSE_STATE_GOT_MSGID3:
  145.                 // back to MAVLink 1 and 2
  146.                 m.payload.add((byte) c);
  147.                 if (m.payloadIsFilled()) {
  148.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;
  149.                 }
  150.                 break;
  151.  
  152.             case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
  153.                 boolean crcGen = m.generateCRC(m.payload.size());
  154.                 // Check first checksum byte and verify the CRC was successfully generated (msg extra exists)
  155.                 if (c != m.crc.getLSB() || !crcGen) {
  156.                     state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
  157.                     stats.crcError();
  158.                 } else {
  159.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_CRC1;
  160.                 }
  161.                 break;
  162.  
  163.             case MAVLINK_PARSE_STATE_GOT_CRC1:
  164.                 // Check second checksum byte
  165.                 if (c != m.crc.getMSB()) {
  166.                     state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
  167.                     stats.crcError();
  168.                 } else { // crc is good
  169.                     stats.newPacket(m);
  170.                     
  171.                     if (!isMavlink2 || (m.incompatFlags != 0x01)) {
  172.                         // If no signature, then return the message.
  173.                         state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
  174.                         return m;
  175.                     } else {
  176.                         // TODO: MAVLink 2 - signed
  177.                         state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
  178.                         stats.crcError();
  179.                     }
  180.                 }
  181.                 break;
  182.                 
  183.             case MAVLINK_PARSE_STATE_GOT_CRC2:
  184.                 // TODO: implement signature parsing and validation
  185.                 state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
  186.                 stats.crcError();
  187.                 break;
  188.         } // switch
  189.         return null;
  190.     }
  191. }
  • MAVLinkPacket类是一个包装类包含了消息协议的组帧和解帧，encodePacket()会把mavlink帧的STX、payloadSize、flags、sysid、compid、msgid、payload、crc都拼接起来，unpack会根据msgid分离出不同类型的消息命令如msg_sysinfo：
  1.  
  2. package com.MAVLink;
  3.  
  4. import java.io.Serializable;
  5. import com.MAVLink.Messages.MAVLinkPayload;
  6. import com.MAVLink.Messages.MAVLinkMessage;
  7. import com.MAVLink.Demo_heart.CRC;
  8.  
  9. import com.MAVLink.Demo_heart.*;
  10.  
  11. public class MAVLinkPacket implements Serializable {
  12.     private static final long serialVersionUID = 2095947771227815314L;
  13.  
  14.     public static final int MAVLINK_STX_MAVLINK1 = 0xFE; // 254
  15.     public static final int MAVLINK_STX_MAVLINK2 = 0xFD; // 253
  16.     public static final int MAVLINK1_HEADER_LEN = 6;
  17.     public static final int MAVLINK2_HEADER_LEN = 10;
  18.     public static final int MAVLINK1_NONPAYLOAD_LEN = MAVLINK1_HEADER_LEN + 2;
  19.     public static final int MAVLINK2_NONPAYLOAD_LEN = MAVLINK2_HEADER_LEN + 2;
  20.  
  21.     static final boolean V = false;
  22.     static void logv(String str) {
  23.         if(V) System.out.println(String.format("MAVLinkPacket: %s", str));
  24.     }
  25.  
  26.     public final int len;
  27.  
  28.  
  29.     public int seq;
  30.  
  31.  
  32.     public int sysid;
  33.  
  34.  
  35.     public int compid;
  36.  
  37.    
  38.     public int msgid;
  39.  
  40.    
  41.     public MAVLinkPayload payload;
  42.  
  43.  
  44.     public CRC crc;
  45.  
  46.  
  47.  
  48.     public boolean isMavlink2;
  49.  
  50.  
  51.     public int incompatFlags;
  52.  
  53.   
  54.     public int compatFlags;
  55.  
  56.     public MAVLinkPacket(int payloadLength) {
  57.         this(payloadLength, false);
  58.     }
  59.  
  60.     public MAVLinkPacket(final int payloadLength, final boolean isMavlink2) {
  61.         len = payloadLength;
  62.         payload = new MAVLinkPayload();
  63.         this.isMavlink2 = isMavlink2;
  64.     }
  65.  
  66.     public boolean payloadIsFilled() {
  67.         return payload.size() >= len;
  68.     }
  69.  
  70.  
  71.     public boolean generateCRC(final int payloadSize) {
  72.         if (crc == null) {
  73.             crc = new CRC();
  74.         } else {
  75.             crc.start_checksum();
  76.         }
  77.  
  78.         if (isMavlink2) {
  79.             crc.update_checksum(payloadSize);
  80.             crc.update_checksum(incompatFlags);
  81.             crc.update_checksum(compatFlags);
  82.             crc.update_checksum(seq);
  83.             crc.update_checksum(sysid);
  84.             crc.update_checksum(compid);
  85.             crc.update_checksum(msgid);
  86.             crc.update_checksum(msgid >>> 8);
  87.             crc.update_checksum(msgid >>> 16);
  88.         } else {
  89.             crc.update_checksum(payloadSize);
  90.             crc.update_checksum(seq);
  91.             crc.update_checksum(sysid);
  92.             crc.update_checksum(compid);
  93.             crc.update_checksum(msgid);
  94.         }
  95.  
  96.         payload.resetIndex();
  97.  
  98.         for (int i = 0; i < payloadSize; i++) {
  99.             crc.update_checksum(payload.getByte());
  100.         }
  101.         return crc.finish_checksum(msgid);
  102.     }
  103.  
  104.     
  105.     private int mavTrimPayload(final byte[] payload)
  106.     {
  107.         int length = payload.length;
  108.         while (length > 1 && payload[length-1] == 0) {
  109.             length--;
  110.         }
  111.         return length;
  112.     }
  113.  
  114.   
  115.     public byte[] encodePacket() {
  116.         final int bufLen;
  117.         final int payloadSize;
  118.  
  119.         if (isMavlink2) {
  120.             payloadSize = mavTrimPayload(payload.payload.array());
  121.             bufLen = MAVLINK2_HEADER_LEN + payloadSize + 2;
  122.         } else {
  123.             payloadSize = payload.size();
  124.             bufLen = MAVLINK1_HEADER_LEN + payloadSize + 2;
  125.  
  126.         }
  127.         byte[] buffer = new byte[bufLen];
  128.  
  129.         int i = 0;
  130.         if (isMavlink2) {
  131.             buffer[i++] = (byte) MAVLINK_STX_MAVLINK2;
  132.             buffer[i++] = (byte) payloadSize;
  133.             buffer[i++] = (byte) incompatFlags;
  134.             buffer[i++] = (byte) compatFlags;
  135.             buffer[i++] = (byte) seq;
  136.             buffer[i++] = (byte) sysid;
  137.             buffer[i++] = (byte) compid;
  138.             buffer[i++] = (byte) (msgid & 0XFF);
  139.             buffer[i++] = (byte) ((msgid >>> 8) & 0XFF);
  140.             buffer[i++] = (byte) ((msgid >>> 16) & 0XFF);
  141.         } else {
  142.             buffer[i++] = (byte) MAVLINK_STX_MAVLINK1;
  143.             buffer[i++] = (byte) payloadSize;
  144.             buffer[i++] = (byte) seq;
  145.             buffer[i++] = (byte) sysid;
  146.             buffer[i++] = (byte) compid;
  147.             buffer[i++] = (byte) msgid;
  148.         }
  149.  
  150.         for (int j = 0; j < payloadSize; ++j) {
  151.             buffer[i++] = payload.payload.get(j);
  152.         }
  153.  
  154.         generateCRC(payloadSize);
  155.         buffer[i++] = (byte) (crc.getLSB());
  156.         buffer[i++] = (byte) (crc.getMSB());
  157.  
  158.         logv(String.format("encode: isMavlink2=%s msgid=%d", isMavlink2, msgid));
  159.  
  160.         return buffer;
  161.     }
  162.         
  163.     
  164.     public MAVLinkMessage unpack() {
  165.         switch (msgid) {
  166.                     case msg_sysinfo.MAVLINK_MSG_ID_SysInfo:
  167.                 return  new msg_sysinfo(this);
  168.             
  169.             default:
  170.                 return null;
  171.         }
  172.     }
  173.  
  174. }

        Drone端C/C++代码分析：

        mavlink_msg_sysinfo.hpp封装了有效的命令类比Java代码的msg_sysinfo类、mavlink_msg_sysinfo.hpp继承Message是相对于msg_sysinfo继承了MAVLinkMessage。工具类mavlink_helpers.h包含了组帧和解帧的具体操作及接收数据、发送数据的操作。

  • mavlink_msg_sysinfo的组帧的调用：
  1. static inline uint16_t
  2. mavlink_msg_sysinfo_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t *msg,
  3.                            const mavlink_sysinfo_t *sysinfo) {
  4.     return mavlink_msg_sysinfo_pack(system_id, component_id, msg, sysinfo->sysState,
  5.                                     sysinfo->voltageBattery);
  6. }
  •  mavlink_helpers.h以下函数对传进来的数据进行组帧：
  1. MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
  2. 						      mavlink_status_t* status, uint8_t min_length, uint8_t length, uint8_t crc_extra)
  3. {
  4. 	bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0;
  5. #ifndef MAVLINK_NO_SIGN_PACKET
  6. 	bool signing = 	(!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING);
  7. #else
  8. 	bool signing = false;
  9. #endif
  10. 	uint8_t signature_len = signing? MAVLINK_SIGNATURE_BLOCK_LEN : 0;
  11.         uint8_t header_len = MAVLINK_CORE_HEADER_LEN+1;
  12. 	uint8_t buf[MAVLINK_CORE_HEADER_LEN+1];
  13. 	if (mavlink1) {
  14. 		msg->magic = MAVLINK_STX_MAVLINK1;
  15. 		header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN+1;
  16. 	} else {
  17. 		msg->magic = MAVLINK_STX;
  18. 	}
  19. 	msg->len = mavlink1?min_length:_mav_trim_payload(_MAV_PAYLOAD(msg), length);
  20. 	msg->sysid = system_id;
  21. 	msg->compid = component_id;
  22. 	msg->incompat_flags = 0;
  23. 	if (signing) {
  24. 		msg->incompat_flags |= MAVLINK_IFLAG_SIGNED;
  25. 	}
  26. 	msg->compat_flags = 0;
  27. 	msg->seq = status->current_tx_seq;
  28. 	status->current_tx_seq = status->current_tx_seq + 1;
  29.  
  30. 	// form the header as a byte array for the crc
  31. 	buf[0] = msg->magic;
  32. 	buf[1] = msg->len;
  33. 	if (mavlink1) {
  34. 		buf[2] = msg->seq;
  35. 		buf[3] = msg->sysid;
  36. 		buf[4] = msg->compid;
  37. 		buf[5] = msg->msgid & 0xFF;
  38. 	} else {
  39. 		buf[2] = msg->incompat_flags;
  40. 		buf[3] = msg->compat_flags;
  41. 		buf[4] = msg->seq;
  42. 		buf[5] = msg->sysid;
  43. 		buf[6] = msg->compid;
  44. 		buf[7] = msg->msgid & 0xFF;
  45. 		buf[8] = (msg->msgid >> 8) & 0xFF;
  46. 		buf[9] = (msg->msgid >> 16) & 0xFF;
  47. 	}
  48. 	
  49. 	uint16_t checksum = crc_calculate(&buf[1], header_len-1);
  50. 	crc_accumulate_buffer(&checksum, _MAV_PAYLOAD(msg), msg->len);
  51. 	crc_accumulate(crc_extra, &checksum);
  52. 	mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF);
  53. 	mavlink_ck_b(msg) = (uint8_t)(checksum >> 8);
  54.  
  55. 	msg->checksum = checksum;
  56.  
  57. #ifndef MAVLINK_NO_SIGN_PACKET
  58. 	if (signing) {
  59. 		mavlink_sign_packet(status->signing,
  60. 				    msg->signature,
  61. 				    (const uint8_t *)buf, header_len,
  62. 				    (const uint8_t *)_MAV_PAYLOAD(msg), msg->len,
  63. 				    (const uint8_t *)_MAV_PAYLOAD(msg)+(uint16_t)msg->len);
  64. 	}
  65. #endif
  66.  
  67. 	return msg->len + header_len + 2 + signature_len;
  68. }
  • mavlink_helpers.h实现了几种不同数据流的发送函数：_mav_finalize_message_chan_sen()、_mavlink_resend_uart()、mavlink_msg_to_send_buffer()。以下以mavlink_msg_to_send_buffer()为例:
  1. /**
  2.  * @brief Pack a message to send it over a serial byte stream
  3.  */
  4. MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buf, const mavlink_message_t *msg)
  5. {
  6. 	uint8_t signature_len, header_len;
  7. 	uint8_t *ck;
  8.         uint8_t length = msg->len;
  9.         
  10. 	if (msg->magic == MAVLINK_STX_MAVLINK1) {
  11. 		signature_len = 0;
  12. 		header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN;
  13. 		buf[0] = msg->magic;
  14. 		buf[1] = length;
  15. 		buf[2] = msg->seq;
  16. 		buf[3] = msg->sysid;
  17. 		buf[4] = msg->compid;
  18. 		buf[5] = msg->msgid & 0xFF;
  19. 		memcpy(&buf[6], _MAV_PAYLOAD(msg), msg->len);
  20. 		ck = buf + header_len + 1 + (uint16_t)msg->len;
  21. 	} else {
  22. 		length = _mav_trim_payload(_MAV_PAYLOAD(msg), length);
  23. 		header_len = MAVLINK_CORE_HEADER_LEN;
  24. 		buf[0] = msg->magic;
  25. 		buf[1] = length;
  26. 		buf[2] = msg->incompat_flags;
  27. 		buf[3] = msg->compat_flags;
  28. 		buf[4] = msg->seq;
  29. 		buf[5] = msg->sysid;
  30. 		buf[6] = msg->compid;
  31. 		buf[7] = msg->msgid & 0xFF;
  32. 		buf[8] = (msg->msgid >> 8) & 0xFF;
  33. 		buf[9] = (msg->msgid >> 16) & 0xFF;
  34. 		memcpy(&buf[10], _MAV_PAYLOAD(msg), length);
  35. 		ck = buf + header_len + 1 + (uint16_t)length;
  36. 		signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0;
  37. 	}
  38. 	ck[0] = (uint8_t)(msg->checksum & 0xFF);
  39. 	ck[1] = (uint8_t)(msg->checksum >> 8);
  40. 	if (signature_len > 0) {
  41. 		memcpy(&ck[2], msg->signature, signature_len);
  42. 	}
  43.  
  44. 	return header_len + 1 + 2 + (uint16_t)length + (uint16_t)signature_len;
  45. }
  • mavlink_helpers.h的mavlink_frame_char_buffer()从接收的数据组成帧：
  1. /**
  2.  * This is a variant of mavlink_frame_char() but with caller supplied
  3.  * parsing buffers. It is useful when you want to create a MAVLink
  4.  * parser in a library that doesn't use any global variables
  5.  *
  6.  * @param rxmsg    parsing message buffer
  7.  * @param status   parsing status buffer
  8.  * @param c        The char to parse
  9.  *
  10.  * @param r_message NULL if no message could be decoded, otherwise the message data
  11.  * @param r_mavlink_status if a message was decoded, this is filled with the channel's stats
  12.  * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
  13.  *
  14.  */
  15. MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, 
  16.                                                  mavlink_status_t* status,
  17.                                                  uint8_t c, 
  18.                                                  mavlink_message_t* r_message, 
  19.                                                  mavlink_status_t* r_mavlink_status)
  20. {
  21.  
  22. 	status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
  23.  
  24. 	switch (status->parse_state)
  25. 	{
  26. 	case MAVLINK_PARSE_STATE_UNINIT:
  27. 	case MAVLINK_PARSE_STATE_IDLE:
  28. 		if (c == MAVLINK_STX)
  29. 		{
  30. 			status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
  31. 			rxmsg->len = 0;
  32. 			rxmsg->magic = c;
  33.                         status->flags &= ~MAVLINK_STATUS_FLAG_IN_MAVLINK1;
  34. 			mavlink_start_checksum(rxmsg);
  35. 		} else if (c == MAVLINK_STX_MAVLINK1)
  36. 		{
  37. 			status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
  38. 			rxmsg->len = 0;
  39. 			rxmsg->magic = c;
  40.                         status->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1;
  41. 			mavlink_start_checksum(rxmsg);
  42. 		}
  43. 		break;
  44.  
  45. 	case MAVLINK_PARSE_STATE_GOT_STX:
  46. 			if (status->msg_received 
  47. /* Support shorter buffers than the
  48.    default maximum packet size */
  49. #if (MAVLINK_MAX_PAYLOAD_LEN < 255)
  50. 				|| c > MAVLINK_MAX_PAYLOAD_LEN
  51. #endif
  52. 				)
  53. 		{
  54. 			status->buffer_overrun++;
  55. 			_mav_parse_error(status);
  56. 			status->msg_received = 0;
  57. 			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
  58. 		}
  59. 		else
  60. 		{
  61. 			// NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
  62. 			rxmsg->len = c;
  63. 			status->packet_idx = 0;
  64. 			mavlink_update_checksum(rxmsg, c);
  65.                         if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
  66.                             rxmsg->incompat_flags = 0;
  67.                             rxmsg->compat_flags = 0;
  68.                             status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
  69.                         } else {
  70.                             status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
  71.                         }
  72. 		}
  73. 		break;
  74.  
  75. 	case MAVLINK_PARSE_STATE_GOT_LENGTH:
  76. 		rxmsg->incompat_flags = c;
  77. 		if ((rxmsg->incompat_flags & ~MAVLINK_IFLAG_MASK) != 0) {
  78. 			// message includes an incompatible feature flag
  79. 			_mav_parse_error(status);
  80. 			status->msg_received = 0;
  81. 			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
  82. 			break;
  83. 		}
  84. 		mavlink_update_checksum(rxmsg, c);
  85. 		status->parse_state = MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS;
  86. 		break;
  87.  
  88. 	case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS:
  89. 		rxmsg->compat_flags = c;
  90. 		mavlink_update_checksum(rxmsg, c);
  91. 		status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
  92. 		break;
  93.  
  94. 	case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS:
  95. 		rxmsg->seq = c;
  96. 		mavlink_update_checksum(rxmsg, c);
  97. 		status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
  98. 		break;
  99.                 
  100. 	case MAVLINK_PARSE_STATE_GOT_SEQ:
  101. 		rxmsg->sysid = c;
  102. 		mavlink_update_checksum(rxmsg, c);
  103. 		status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
  104. 		break;
  105.  
  106. 	case MAVLINK_PARSE_STATE_GOT_SYSID:
  107. 		rxmsg->compid = c;
  108. 		mavlink_update_checksum(rxmsg, c);
  109.                 status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
  110. 		break;
  111.  
  112. 	case MAVLINK_PARSE_STATE_GOT_COMPID:
  113. 		rxmsg->msgid = c;
  114. 		mavlink_update_checksum(rxmsg, c);
  115. 		if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
  116. 			if(rxmsg->len > 0) {
  117. 				status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
  118. 			} else {
  119. 				status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
  120. 			}
  121. #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
  122. 			if (rxmsg->len < mavlink_min_message_length(rxmsg) ||
  123. 				rxmsg->len > mavlink_max_message_length(rxmsg)) {
  124. 				_mav_parse_error(status);
  125. 				status->parse_state = MAVLINK_PARSE_STATE_IDLE;
  126. 				break;
  127. 			}
  128. #endif
  129. 		} else {
  130. 			status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID1;
  131. 		}
  132. 		break;
  133.  
  134. 	case MAVLINK_PARSE_STATE_GOT_MSGID1:
  135. 		rxmsg->msgid |= ((uint32_t)c)<<8;
  136. 		mavlink_update_checksum(rxmsg, c);
  137. 		status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID2;
  138. 		break;
  139.  
  140. 	case MAVLINK_PARSE_STATE_GOT_MSGID2:
  141. 		rxmsg->msgid |= ((uint32_t)c)<<16;
  142. 		mavlink_update_checksum(rxmsg, c);
  143. 		if(rxmsg->len > 0){
  144. 			status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
  145. 		} else {
  146. 			status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
  147. 		}
  148. #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
  149.         if (rxmsg->len < mavlink_min_message_length(rxmsg) ||
  150.             rxmsg->len > mavlink_max_message_length(rxmsg))
  151.         {
  152. 			_mav_parse_error(status);
  153. 			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
  154. 			break;
  155.         }
  156. #endif
  157. 		break;
  158.                 
  159. 	case MAVLINK_PARSE_STATE_GOT_MSGID3:
  160. 		_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
  161. 		mavlink_update_checksum(rxmsg, c);
  162. 		if (status->packet_idx == rxmsg->len)
  163. 		{
  164. 			status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
  165. 		}
  166. 		break;
  167.  
  168. 	case MAVLINK_PARSE_STATE_GOT_PAYLOAD: {
  169. 		const mavlink_msg_entry_t *e = mavlink_get_msg_entry(rxmsg->msgid);
  170. 		uint8_t crc_extra = e?e->crc_extra:0;
  171. 		mavlink_update_checksum(rxmsg, crc_extra);
  172. 		if (c != (rxmsg->checksum & 0xFF)) {
  173. 			status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1;
  174. 		} else {
  175. 			status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
  176. 		}
  177.                 rxmsg->ck[0] = c;
  178.  
  179. 		// zero-fill the packet to cope with short incoming packets
  180.                 if (e && status->packet_idx < e->max_msg_len) {
  181.                         memset(&_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx], 0, e->max_msg_len - status->packet_idx);
  182. 		}
  183. 		break;
  184.         }
  185.  
  186. 	case MAVLINK_PARSE_STATE_GOT_CRC1:
  187. 	case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:
  188. 		if (status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c != (rxmsg->checksum >> 8)) {
  189. 			// got a bad CRC message
  190. 			status->msg_received = MAVLINK_FRAMING_BAD_CRC;
  191. 		} else {
  192. 			// Successfully got message
  193. 			status->msg_received = MAVLINK_FRAMING_OK;
  194. 		}
  195. 		rxmsg->ck[1] = c;
  196.  
  197. 		if (rxmsg->incompat_flags & MAVLINK_IFLAG_SIGNED) {
  198. 			status->parse_state = MAVLINK_PARSE_STATE_SIGNATURE_WAIT;
  199. 			status->signature_wait = MAVLINK_SIGNATURE_BLOCK_LEN;
  200.  
  201. 			// If the CRC is already wrong, don't overwrite msg_received,
  202. 			// otherwise we can end up with garbage flagged as valid.
  203. 			if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) {
  204. 				status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
  205. 			}
  206. 		} else {
  207. 			if (status->signing &&
  208. 			   	(status->signing->accept_unsigned_callback == NULL ||
  209. 			   	 !status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {
  210.  
  211. 				// If the CRC is already wrong, don't overwrite msg_received.
  212. 				if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) {
  213. 					status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;
  214. 				}
  215. 			}
  216. 			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
  217. 			if (r_message != NULL) {
  218. 				memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
  219. 			}
  220. 		}
  221. 		break;
  222. 	case MAVLINK_PARSE_STATE_SIGNATURE_WAIT:
  223. 		rxmsg->signature[MAVLINK_SIGNATURE_BLOCK_LEN-status->signature_wait] = c;
  224. 		status->signature_wait--;
  225. 		if (status->signature_wait == 0) {
  226. 			// we have the whole signature, check it is OK
  227. #ifndef MAVLINK_NO_SIGNATURE_CHECK
  228. 			bool sig_ok = mavlink_signature_check(status->signing, status->signing_streams, rxmsg);
  229. #else
  230. 			bool sig_ok = true;
  231. #endif
  232. 			if (!sig_ok &&
  233. 			   	(status->signing->accept_unsigned_callback &&
  234. 			   	 status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {
  235. 				// accepted via application level override
  236. 				sig_ok = true;
  237. 			}
  238. 			if (sig_ok) {
  239. 				status->msg_received = MAVLINK_FRAMING_OK;
  240. 			} else {
  241. 				status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;
  242. 			}
  243. 			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
  244. 			if (r_message !=NULL) {
  245. 				memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
  246. 			}
  247. 		}
  248. 		break;
  249. 	}
  250.  
  251. 	// If a message has been successfully decoded, check index
  252. 	if (status->msg_received == MAVLINK_FRAMING_OK)
  253. 	{
  254. 		//while(status->current_seq != rxmsg->seq)
  255. 		//{
  256. 		//	status->packet_rx_drop_count++;
  257. 		//               status->current_seq++;
  258. 		//}
  259. 		status->current_rx_seq = rxmsg->seq;
  260. 		// Initial condition: If no packet has been received so far, drop count is undefined
  261. 		if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
  262. 		// Count this packet as received
  263. 		status->packet_rx_success_count++;
  264. 	}
  265.  
  266.        if (r_message != NULL) {
  267.            r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg
  268.        }
  269.        if (r_mavlink_status != NULL) {	
  270.            r_mavlink_status->parse_state = status->parse_state;
  271.            r_mavlink_status->packet_idx = status->packet_idx;
  272.            r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
  273.            r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
  274.            r_mavlink_status->packet_rx_drop_count = status->parse_error;
  275.            r_mavlink_status->flags = status->flags;
  276.        }
  277.        status->parse_error = 0;
  278.  
  279. 	if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) {
  280. 		/*
  281. 		  the CRC came out wrong. We now need to overwrite the
  282. 		  msg CRC with the one on the wire so that if the
  283. 		  caller decides to forward the message anyway that
  284. 		  mavlink_msg_to_send_buffer() won't overwrite the
  285. 		  checksum
  286. 		 */
  287.             if (r_message != NULL) {
  288.                 r_message->checksum = rxmsg->ck[0] | (rxmsg->ck[1]<<8);
  289.             }
  290. 	}
  291.  
  292. 	return status->msg_received;
  293. }

四、封装自定义的类MavLink通信协议：

        通过自动生成mavlink的协议代码，我们可以此基础上进行自定义的拓展。形成自己通信协议。

协议的帧的设计：

        下图是mavlink2中一帧的结构，在我们自己的项目也可以类似的加入项目中需要的字段定义，更加丰富灵活的用于不同需求的项目中。

        介于设备有多种模块组成，比如无人机有相机模块（MODULE_CAMERA）、中继遥控器模块（MODULE_REPEATER_RC）、中继无人机模块（MODULE_REPEATER_VEHICLE）、遥控器模块（MODULE_RC）、视觉识别模块（MODULE_CV）、云台模块（MODULE_GIMBAL）、电芯模块（MODULE_BATTERY）、超声波模块（MODULE_ULTRASONIC）、地面站模块（MODULE_GCS）等等多种模块组成，这些模块间要互相通信。

        模块之间会进行互相的通信，交叉的信息交换这样就要明确发送端和接收端。这样的话可以在帧头加上两个标识SrcId和destId来区分源端和目的端，如从GCS发送至Drone对应的SrcId=MODULE_GCS/DestId=MODULE_REPEATER_VEHICLE。

        鉴于通信的安全也可在帧头位置加入对应的加解密算法的类型标识，总之按照项目自身的需求加入对应的帧头标识。

        现在基于MavLink2的基础上添加SrcId和destId两个帧头标识为例进行代码编程的自定义类MavLink2项目。     

         自定义类MavLink2 GCS端Java代码分析：

  ··首先我们把协议通信命令区分为发送和接收：AckMsg和CmdMsg两类，分别用于接收和发送。

  •      AckSysInfo类会解析出playload的属性值：
  1. public class AckSysInfo extends BaseAckMsg {
  2.     public static final int MAVLINK_MSG_ID_SysInfo = 150;
  3.     public static final int DEST_ID_SysInfo = 3;
  4.     public static final int SRC_ID_SysInfo = 2;
  5.  
  6.  
  7.     public static final int MAVLINK_MSG_LENGTH = 3;
  8.     private static final long serialVersionUID = MAVLINK_MSG_ID_SysInfo;
  9.  
  10.     public int voltageBattery;
  11.     public short sysState;
  12.  
  13.     public AckSysInfo() {
  14.     }
  15.  
  16.     public AckSysInfo(MyLinkPacket myLinkPacket) {
  17.         this.sysid = myLinkPacket.sysid;
  18.         desId = myLinkPacket.destId;
  19.         srcId = myLinkPacket.srcId;
  20.         this.compid = myLinkPacket.compid;
  21.         this.msgid = MAVLINK_MSG_ID_SysInfo;
  22.         unpack(myLinkPacket);
  23.     }
  24.  
  25.  
  26.     //解析出payload包含的实体数值
  27.     private void unpack(MyLinkPacket myLinkPacket) {
  28.         super.decrypt(myLinkPacket);
  29.         sysState = myLinkPacket.payload.getShort();
  30.         voltageBattery = myLinkPacket.payload.getInt();
  31.     }
  32.  
  33.  
  34.     @Override
  35.     public String toString() {
  36.         return "AckSysInfo{" +
  37.                 "voltageBattery=" + voltageBattery +
  38.                 ", sysState=" + sysState +
  39.                 '}';
  40.     }
  41.  
  42. }
  •  CmdSysInfo会包playload的属性值包装成一个MyLinkPacket：
  1. public class CmdSysInfo extends BaseCmd {
  2.     public static final int MAVLINK_MSG_ID_SysInfo = 150;
  3.     public static final int MAVLINK_MSG_LENGTH = 3;
  4.     public static final int DEST_ID_SysInfo = 3;
  5.     public static final int SRC_ID_SysInfo = 2;
  6.     private static final long serialVersionUID = MAVLINK_MSG_ID_SysInfo;
  7.  
  8.     public int voltageBattery;
  9.     public short sysState;
  10.  
  11.     public CmdSysInfo(int voltageBattery, short sysState) {
  12.         this.voltageBattery = voltageBattery;
  13.         this.sysState = sysState;
  14.  
  15.     }
  16.     @Override
  17.     public MyLinkPacket pack() {
  18.         MyLinkPacket myLinkPacket = new MyLinkPacket(MAVLINK_MSG_LENGTH);
  19.         //设置帧头的sysid、destId、srcId、compid、msgid
  20.         myLinkPacket.sysid = 1;
  21.         myLinkPacket.destId = DEST_ID_SysInfo;
  22.         myLinkPacket.srcId = SRC_ID_SysInfo;
  23.         myLinkPacket.compid = 3;
  24.         myLinkPacket.msgid = 3;
  25.         //添加命令playload实际的值
  26.         myLinkPacket.payload.putUnsignedShort(voltageBattery);
  27.         myLinkPacket.payload.putUnsignedByte(sysState);
  28.         return null;
  29.     }
  30.     
  31.     @Override
  32.     public String toString() {
  33.         return "AckSysInfo{" +
  34.                 "voltageBattery=" + voltageBattery +
  35.                 ", sysState=" + sysState +
  36.                 '}';
  37.     }
  38.  
  39.    
  40. }

     接收ACK类：

  • ReceiveThread是一个接收处理类，如接收UDP的数据或者是AOA串口的数据这个因项目而异。作用是在于接收的数据处理成对应的ACK（如：AckSysInfo）的数据。
  • MyParser如MavLink2中Parse功能一样，这里增加了对SrcId和destId的解析处理。
  1. public class MyParser {
  2.     static final String TAG = MyParser.class.getSimpleName();
  3.  
  4.     enum MAV_states {
  5.         MAVLINK_PARSE_STATE_UNINIT,
  6.         MAVLINK_PARSE_STATE_IDLE,
  7.         MAVLINK_PARSE_STATE_GOT_STX,
  8.         MAVLINK_PARSE_STATE_GOT_LENGTH,
  9.         MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS, // MAVLink 2
  10.         MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS, // MAVLink 2
  11.         MAVLINK_PARSE_STATE_GOT_SEQ,
  12.         MAVLINK_PARSE_STATE_GOT_SYSID,
  13.  
  14.         MAVLINK_PARSE_STATE_GOT_DESTID,
  15.         MAVLINK_PARSE_STATE_GOT_SRCID,
  16.  
  17.         MAVLINK_PARSE_STATE_GOT_COMPID,
  18.         MAVLINK_PARSE_STATE_GOT_MSGID1,
  19.         MAVLINK_PARSE_STATE_GOT_MSGID2,
  20.         MAVLINK_PARSE_STATE_GOT_MSGID3,
  21.         MAVLINK_PARSE_STATE_GOT_CRC1,
  22.         MAVLINK_PARSE_STATE_GOT_CRC2,
  23.         MAVLINK_PARSE_STATE_GOT_PAYLOAD,
  24.         MAVLINK_PARSE_STATE_GOT_SIGNATURE,
  25.     }
  26.  
  27.     private MAV_states state = MAV_states.MAVLINK_PARSE_STATE_UNINIT;
  28.  
  29.     public MyLinkStats stats;
  30.     private MyLinkPacket m;
  31.  
  32.     public MyParser() {
  33.         this(false);
  34.     }
  35.  
  36.     public MyParser(boolean ignoreRadioPacketStats) {
  37.         stats = new MyLinkStats(ignoreRadioPacketStats);
  38.     }
  39.  
  40.     public MyLinkPacket mavlink_parse_char(int c) {
  41.  
  42.         // force to 8 bits
  43.         c &= 0xFF;
  44.  
  45.         switch (state) {
  46.             case MAVLINK_PARSE_STATE_UNINIT:
  47.             case MAVLINK_PARSE_STATE_IDLE:
  48.                 // MAVLink 1 and 2
  49.                 if (c == MyLinkPacket.MAVLINK_STX) {
  50.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_STX;
  51.                 }
  52.                 break;
  53.  
  54.             case MAVLINK_PARSE_STATE_GOT_STX:
  55.                 // MAVLink 1 and 2
  56.                 m = new MyLinkPacket(c);
  57.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_LENGTH;
  58.                 break;
  59.  
  60.             case MAVLINK_PARSE_STATE_GOT_LENGTH:
  61.                 m.incompatFlags = c;
  62.                 if (c != 0 && c != 1) {
  63.                     state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
  64.                     break;
  65.                 }
  66.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS;
  67.                 break;
  68.  
  69.             case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS:
  70.                 m.compatFlags = c;
  71.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
  72.                 break;
  73.  
  74.             case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS:
  75.                 m.seq = c;
  76.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_SEQ;
  77.                 break;
  78.  
  79.             case MAVLINK_PARSE_STATE_GOT_SEQ:
  80.                 // back to MAVLink 1 and 2
  81.                 m.sysid = c;
  82.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_SYSID;
  83.                 break;
  84.  
  85.             case MAVLINK_PARSE_STATE_GOT_SYSID:
  86.                 m.compid = c;
  87.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_DESTID;
  88.                 break;
  89.             case MAVLINK_PARSE_STATE_GOT_DESTID:
  90.                 m.compid = c;
  91.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_SRCID;
  92.                 break;
  93.  
  94.             case MAVLINK_PARSE_STATE_GOT_SRCID:
  95.                 m.compid = c;
  96.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_COMPID;
  97.                 break;
  98.  
  99.             case MAVLINK_PARSE_STATE_GOT_COMPID:
  100.                 // MAVLink 1 and 2
  101.                 m.msgid = c;
  102.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_MSGID1;
  103.                 if (m.len > 0) {
  104.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_MSGID3;
  105.                 } else {
  106.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;
  107.                 }
  108.                 break;
  109.  
  110.             case MAVLINK_PARSE_STATE_GOT_MSGID1:
  111.                 m.msgid |= c << 8;
  112.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_MSGID2;
  113.                 break;
  114.  
  115.             case MAVLINK_PARSE_STATE_GOT_MSGID2:
  116.                 m.msgid |= c << 16;
  117.                 if (m.len > 0) {
  118.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_MSGID3;
  119.                 } else {
  120.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;
  121.                 }
  122.                 break;
  123.  
  124.             case MAVLINK_PARSE_STATE_GOT_MSGID3:
  125.                 //将payload的数据加入
  126.                 m.payload.add((byte) c);
  127.                 if (m.payloadIsFilled()) {
  128.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;
  129.                 }
  130.                 break;
  131.  
  132.             case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
  133.                 boolean crcGen = m.generateCRC(m.payload.size());
  134.                 // Check first checksum byte and verify the CRC was successfully generated (msg extra exists)
  135.                 if (c != m.crc.getLSB() || !crcGen) {
  136.                     state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
  137.                     stats.crcError();
  138.                 } else {
  139.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_CRC1;
  140.                 }
  141.                 break;
  142.  
  143.             case MAVLINK_PARSE_STATE_GOT_CRC1:
  144.                 // Check second checksum byte
  145.                 if (c != m.crc.getMSB()) {
  146.                     state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
  147.                     stats.crcError();
  148.                 } else { // crc is good
  149. //                    stats.newPacket(m);
  150.                     state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
  151.                     stats.crcError();
  152.                 }
  153.                 break;
  154.  
  155.             case MAVLINK_PARSE_STATE_GOT_CRC2:
  156.                 state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
  157.                 stats.crcError();
  158.                 break;
  159.         } // switch
  160.         return null;
  161.     }
  162. }

        发送Cmd类 

  •  SendMav是把CmdSysInfo拼装一个MyLinkPacket装成字节流的形式；
  • MyLinkPacket编码成一个类MavLink协议帧：
  1. public class MyLinkPacket implements Serializable {
  2.     private static final long serialVersionUID = 2095947771227815314L;
  3.     public static final int MAVLINK_STX = 0xFD; // 253
  4.     public static final int HEADER_LEN = 12;
  5.     public final int len;
  6.     public int seq;
  7.     public int sysid;
  8.     public int destId;
  9.     public int srcId;
  10.     public int compid;
  11.     public int msgid;
  12.     public MyLinkPayload payload;
  13.     public CRC crc;
  14.     public int incompatFlags;
  15.     public int compatFlags;
  16.  
  17.     public MyLinkPacket(int payloadLength) {
  18.         len = payloadLength;
  19.         payload = new MyLinkPayload();
  20.     }
  21.  
  22.     public boolean payloadIsFilled() {
  23.         return payload.size() >= len;
  24.     }
  25.  
  26.     public boolean generateCRC(final int payloadSize) {
  27.         if (crc == null) {
  28.             crc = new CRC();
  29.         } else {
  30.             crc.start_checksum();
  31.         }
  32.         crc.update_checksum(payloadSize);
  33.         crc.update_checksum(incompatFlags);
  34.         crc.update_checksum(compatFlags);
  35.         crc.update_checksum(seq);
  36.         crc.update_checksum(sysid);
  37.         crc.update_checksum(destId);
  38.         crc.update_checksum(srcId);
  39.         crc.update_checksum(compid);
  40.         crc.update_checksum(msgid);
  41.         crc.update_checksum(msgid >>> 8);
  42.         crc.update_checksum(msgid >>> 16);
  43.         payload.resetIndex();
  44.  
  45.         for (int i = 0; i < payloadSize; i++) {
  46.             crc.update_checksum(payload.getByte());
  47.         }
  48.         return crc.finish_checksum(msgid);
  49.     }
  50.  
  51.     private int mavTrimPayload(final byte[] payload) {
  52.         int length = payload.length;
  53.         while (length > 1 && payload[length - 1] == 0) {
  54.             length--;
  55.         }
  56.         return length;
  57.     }
  58.  
  59.     public byte[] encodePacket() {
  60.         final int bufLen;
  61.         final int payloadSize;
  62.  
  63.         payloadSize = mavTrimPayload(payload.payload.array());
  64.         bufLen = HEADER_LEN + payloadSize + 2;
  65.         byte[] buffer = new byte[bufLen];
  66.         int i = 0;
  67.         buffer[i++] = (byte) MAVLINK_STX;
  68.         buffer[i++] = (byte) payloadSize;
  69.         buffer[i++] = (byte) incompatFlags;
  70.         buffer[i++] = (byte) compatFlags;
  71.         buffer[i++] = (byte) seq;
  72.         buffer[i++] = (byte) sysid;
  73.         buffer[i++] = (byte) destId;
  74.         buffer[i++] = (byte) srcId;
  75.         buffer[i++] = (byte) compid;
  76.         buffer[i++] = (byte) (msgid & 0XFF);
  77.         buffer[i++] = (byte) ((msgid >>> 8) & 0XFF);
  78.         buffer[i++] = (byte) ((msgid >>> 16) & 0XFF);
  79.         for (int j = 0; j < payloadSize; ++j) {
  80.             buffer[i++] = payload.payload.get(j);
  81.         }
  82.  
  83.         generateCRC(payloadSize);
  84.         buffer[i++] = (byte) (crc.getLSB());
  85.         buffer[i++] = (byte) (crc.getMSB());
  86.  
  87.         return buffer;
  88.     }
  89.  
  90. }

自定义类MavLink1 Drone端C/C++代码分析：

        

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/从前慢现在也慢/article/detail/688863
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号