天天看點
傳回

無人機開發-Android地面站MavLink解析部分源碼

MavLink是輕量級的通訊協定,主要應用于終端與小型無人載具間的通訊。由于它的通用性,MavLink可以被翻譯成各種語言的代碼應用于各種不同的環境。具體如何通過工具來生成對應的MavLink代碼請通路:

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

MavLink協定所定義的消息,大緻分為兩類,一類是通用消息,另外一種是自定義消息。通用消息和自定義消息的資料結構相同,差異隻展現在資料本身。我取MavLink中最常使用的心跳消息作為例子:

[html]  view plain  copy

  1. <message id="0" name="HEARTBEAT">  
  2.   <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>  
  3.   <field type="uint8_t" name="type">Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)</field>  
  4.   <field type="uint8_t" name="autopilot">Autopilot type / class. defined in MAV_CLASS ENUM</field>  
  5.   <field type="uint8_t" name="base_mode">System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h</field>  
  6.   <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>  
  7.   <field type="uint8_t" name="system_status">System status flag, see MAV_STATUS ENUM</field>  
  8.   <field type="uint8_t_mavlink_version" name="mavlink_version">MAVLink version</field>  
  9. </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類:

[java]  view plain  copy

  1. public class MAV_TYPE {  
  2.    public static final int MAV_TYPE_GENERIC = 0;   
  3.    public static final int MAV_TYPE_FIXED_WING = 1;   
  4.    public static final int MAV_TYPE_QUADROTOR = 2;   
  5.    public static final int MAV_TYPE_COAXIAL = 3;   
  6.    public static final int MAV_TYPE_HELICOPTER = 4;   
  7.    public static final int MAV_TYPE_ANTENNA_TRACKER = 5;   
  8.    public static final int MAV_TYPE_GCS = 6;   
  9.    public static final int MAV_TYPE_AIRSHIP = 7;   
  10.    public static final int MAV_TYPE_FREE_BALLOON = 8;   
  11.    public static final int MAV_TYPE_ROCKET = 9;   
  12.    public static final int MAV_TYPE_GROUND_ROVER = 10;   
  13.    public static final int MAV_TYPE_SURFACE_BOAT = 11;   
  14.    public static final int MAV_TYPE_SUBMARINE = 12;   
  15.    public static final int MAV_TYPE_HEXAROTOR = 13;   
  16.    public static final int MAV_TYPE_OCTOROTOR = 14;   
  17.    public static final int MAV_TYPE_TRICOPTER = 15;   
  18.    public static final int MAV_TYPE_FLAPPING_WING = 16;   
  19.    public static final int MAV_TYPE_KITE = 17;   
  20.    public static final int MAV_TYPE_ONBOARD_CONTROLLER = 18;   
  21.    public static final int MAV_TYPE_VTOL_DUOROTOR = 19;   
  22.    public static final int MAV_TYPE_VTOL_QUADROTOR = 20;   
  23.    public static final int MAV_TYPE_VTOL_TILTROTOR = 21;   
  24.    public static final int MAV_TYPE_VTOL_RESERVED2 = 22;   
  25.    public static final int MAV_TYPE_VTOL_RESERVED3 = 23;   
  26.    public static final int MAV_TYPE_VTOL_RESERVED4 = 24;   
  27.    public static final int MAV_TYPE_VTOL_RESERVED5 = 25;   
  28.    public static final int MAV_TYPE_GIMBAL = 26;   
  29.    public static final int MAV_TYPE_ADSB = 27;   
  30.    public static final int MAV_TYPE_ENUM_END = 28;   
  31. }  

3.base_mode:記錄小型交通工具的基本模式

4.custom_mode:記錄小型交工具的特征模式

5.mavlink_version:mavlink協定的版本号

大家可能好奇為什麼有了個基本模式還有有個特征模式,原因是因為MavLink是要兼顧多種類型的小型交通工具的協定,這樣的話,不能保證所有的基本模式覆寫到所有的交通器。

接下來,我們通過網站上的mavlink-generator 去生成一套java代碼,用在我們的android程式中。

生成的代碼移植性很好,我們可以無縫的直接copy到我們的android工程中。我們來看下生成的代碼的分包:

無人機開發-Android地面站MavLink解析部分源碼

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

ardupilotmega包:存放針對mega闆子特有的消息

Messages包:提供消息基本類和一些緩存處理類

enums包:存放一些常量

MAVLinkPacket類:用來記錄原始封包

Parser類:用于解析信道中傳遞過來的資料,生成MAVLinkPacket格式的封包。

由于本篇的主題是MavLink消息在Android地面站的解析,是以我們不過分的關注于信道和業務本身。我們看上面的分包我們會發現,其實對于解析來說,最重要的就是Parser類。在我們開始解析前,通過一張圖再回憶一下心跳消息的資料結構,因為我們将以它為樣本作為例子:

無人機開發-Android地面站MavLink解析部分源碼

我們收到的心跳完整封包是一個byte數組,是以我們需要對它進行解析,解析出我們自己的對象模型,就需要調用mavlink_parse_char(int c)方法。這就有個問題,我們明明讀取到的是byte數組,但是方法中要我們傳遞一個int。這個原因我們不妨來看一下Parser這個類:

[java]  view plain  copy

  1. public class Parser {  
  2.     enum MAV_states {  
  3.         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  
  4.     }  
  5.     MAV_states state = MAV_states.MAVLINK_PARSE_STATE_UNINIT;  
  6.     private boolean msg_received;  
  7.     public MAVLinkStats stats = new MAVLinkStats();  
  8.     private MAVLinkPacket m;  
  9.     public MAVLinkPacket mavlink_parse_char(int c) {  
  10.         msg_received = false;  
  11.         switch (state) {  
  12.         case MAVLINK_PARSE_STATE_UNINIT:  
  13.         case MAVLINK_PARSE_STATE_IDLE:  
  14.             if (c == MAVLinkPacket.MAVLINK_STX) {  
  15.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_STX;  
  16.             }  
  17.             break;  
  18.         case MAVLINK_PARSE_STATE_GOT_STX:  
  19.             if (msg_received) {  
  20.                 msg_received = false;  
  21.                 state = MAV_states.MAVLINK_PARSE_STATE_IDLE;  
  22.             } else {  
  23.                 m = new MAVLinkPacket(c);  
  24.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_LENGTH;  
  25.             }  
  26.             break;  
  27.         case MAVLINK_PARSE_STATE_GOT_LENGTH:  
  28.             m.seq = c;  
  29.             state = MAV_states.MAVLINK_PARSE_STATE_GOT_SEQ;  
  30.             break;  
  31.         case MAVLINK_PARSE_STATE_GOT_SEQ:  
  32.             m.sysid = c;  
  33.             state = MAV_states.MAVLINK_PARSE_STATE_GOT_SYSID;  
  34.             break;  
  35.         case MAVLINK_PARSE_STATE_GOT_SYSID:  
  36.             m.compid = c;  
  37.             state = MAV_states.MAVLINK_PARSE_STATE_GOT_COMPID;  
  38.             break;  
  39.         case MAVLINK_PARSE_STATE_GOT_COMPID:  
  40.             m.msgid = c;  
  41.             if (m.len == 0) {  
  42.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;  
  43.             } else {  
  44.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_MSGID;  
  45.             }  
  46.             break;  
  47.         case MAVLINK_PARSE_STATE_GOT_MSGID:  
  48.             m.payload.add((byte) c);  
  49.             if (m.payloadIsFilled()) {  
  50.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;  
  51.             }  
  52.             break;  
  53.         case MAVLINK_PARSE_STATE_GOT_PAYLOAD:  
  54.             m.generateCRC();  
  55.             // Check first checksum byte  
  56.             if (c != m.crc.getLSB()) {  
  57.                 msg_received = false;  
  58.                 state = MAV_states.MAVLINK_PARSE_STATE_IDLE;  
  59.                 if (c == MAVLinkPacket.MAVLINK_STX) {  
  60.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_STX;  
  61.                     m.crc.start_checksum();  
  62.                 }  
  63.                 stats.crcError();  
  64.             } else {  
  65.                 state = MAV_states.MAVLINK_PARSE_STATE_GOT_CRC1;  
  66.             }  
  67.             break;  
  68.         case MAVLINK_PARSE_STATE_GOT_CRC1:  
  69.             // Check second checksum byte  
  70.             if (c != m.crc.getMSB()) {  
  71.                 msg_received = false;  
  72.                 state = MAV_states.MAVLINK_PARSE_STATE_IDLE;  
  73.                 if (c == MAVLinkPacket.MAVLINK_STX) {  
  74.                     state = MAV_states.MAVLINK_PARSE_STATE_GOT_STX;  
  75.                     m.crc.start_checksum();  
  76.                 }  
  77.                 stats.crcError();  
  78.             } else { // Successfully received the message  
  79.                 stats.newPacket(m);  
  80.                 msg_received = true;  
  81.                 state = MAV_states.MAVLINK_PARSE_STATE_IDLE;  
  82.             }  
  83.             break;  
  84.         }  
  85.         if (msg_received) {  
  86.             return m;  
  87.         } else {  
  88.             return null;  
  89.         }  
  90.     }  
  91. }  

我們發現,Parser類必須要線性的解析封包,也就是說,在同一個周期内,隻能有一條消息在Parser類中處理。并且Parser類的方法結構本質上是一個狀态機。外部代碼需要周遊傳入byte中的資料用于生成封包:

[java]  view plain  copy

  1. private void handleData(Parser parser, int bufferSize, byte[] buffer) {  
  2.             if (bufferSize < 1) {  
  3.                 return;  
  4.             }  
  5.             for (int i = 0; i < bufferSize; i++) {  
  6.                 int code = buffer[i] & 0x00ff;  
  7.                 MAVLinkPacket receivedPacket = parser.mavlink_parse_char(code);  
  8.                 if (receivedPacket != null) {  
  9.                     //test(receivedPacket);  
  10.                 }  
  11.             }  
  12.         }  

Parser類所具有的狀态清單:

[java]  view plain  copy

  1. enum MAV_states {  
  2.        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  
  3.    }  

前面說了Parser類本質上是一個狀态機,初始的狀态是:MAV_PARSE_STATE_UNINIT。一旦解析成功或者失敗,狀态将進入到IDLE。

無人機開發-Android地面站MavLink解析部分源碼

Parser類的狀态機基本可以使用上面的圖檔表示,基本上沒有什麼複雜的内容,主要的在與剛開始的資料長度的記錄。如果你的資料長度大于零的話,解析器會将你的資料緩存在一個叫做payload的資料結構中。

[java]  view plain  copy

  1. case MAVLINK_PARSE_STATE_GOT_MSGID:  
  2.            m.payload.add((byte) c);  
  3.            if (m.payloadIsFilled()) {  
  4.                state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;  
  5.            }  
  6.            break;  

PayLoad對應的類是MAVLinkPayLoad類,他是資料的緩存器和轉換器,就是将無意義的byte數組,組織成為有意義的平台資料類型。

[java]  view plain  copy

  1. public class MAVLinkPayload {  
  2.     private static final byte UNSIGNED_BYTE_MIN_VALUE = 0;  
  3.     private static final short UNSIGNED_BYTE_MAX_VALUE = Byte.MAX_VALUE - Byte.MIN_VALUE;  
  4.     private static final short UNSIGNED_SHORT_MIN_VALUE = 0;  
  5.     private static final int UNSIGNED_SHORT_MAX_VALUE = Short.MAX_VALUE - Short.MIN_VALUE;  
  6.     private static final int UNSIGNED_INT_MIN_VALUE = 0;  
  7.     private static final long UNSIGNED_INT_MAX_VALUE = (long) Integer.MAX_VALUE - Integer.MIN_VALUE;  
  8.     private static final long UNSIGNED_LONG_MIN_VALUE = 0;  
  9.     public static final int MAX_PAYLOAD_SIZE = 255;  
  10.     public final ByteBuffer payload;  
  11.     public int index;  
  12.     public MAVLinkPayload(int payloadSize) {  
  13.        if(payloadSize > MAX_PAYLOAD_SIZE) {  
  14.             payload = ByteBuffer.allocate(MAX_PAYLOAD_SIZE);  
  15.         } else {  
  16.             payload = ByteBuffer.allocate(payloadSize);  
  17.         }  
  18.     }  
  19.     public ByteBuffer getData() {  
  20.         return payload;  
  21.     }  
  22.     public int size() {  
  23.         return payload.position();  
  24.     }  
  25.     public void add(byte c) {  
  26.         payload.put(c);  
  27.     }  
  28.     public void resetIndex() {  
  29.         index = 0;  
  30.     }  
  31.     public byte getByte() {  
  32.         byte result = 0;  
  33.         result |= (payload.get(index + 0) & 0xFF);  
  34.         index += 1;  
  35.         return result;  
  36.     }  
  37.     public short getUnsignedByte(){  
  38.         short result = 0;  
  39.         result |= payload.get(index + 0) & 0xFF;  
  40.         index+= 1;  
  41.         return result;   
  42.     }  
  43.     public short getShort() {  
  44.         short result = 0;  
  45.         result |= (payload.get(index + 1) & 0xFF) << 8;  
  46.         result |= (payload.get(index + 0) & 0xFF);  
  47.         index += 2;  
  48.         return result;  
  49.     }  
  50.     public int getUnsignedShort(){  
  51.         int result = 0;  
  52.         result |= (payload.get(index + 1) & 0xFF) << 8;  
  53.         result |= (payload.get(index + 0) & 0xFF);  
  54.         index += 2;  
  55.         return result;  
  56.     }  
  57.     public int getInt() {  
  58.         int result = 0;  
  59.         result |= (payload.get(index + 3) & 0xFF) << 24;  
  60.         result |= (payload.get(index + 2) & 0xFF) << 16;  
  61.         result |= (payload.get(index + 1) & 0xFF) << 8;  
  62.         result |= (payload.get(index + 0) & 0xFF);  
  63.         index += 4;  
  64.         return result;  
  65.     }  
  66.     public long getUnsignedInt(){  
  67.         long result = 0;  
  68.         result |= (payload.get(index + 3) & 0xFFFFL) << 24;  
  69.         result |= (payload.get(index + 2) & 0xFFFFL) << 16;  
  70.         result |= (payload.get(index + 1) & 0xFFFFL) << 8;  
  71.         result |= (payload.get(index + 0) & 0xFFFFL);  
  72.         index += 4;  
  73.         return result;  
  74.     }  
  75.     public long getLong() {  
  76.         long result = 0;  
  77.         result |= (payload.get(index + 7) & 0xFFFFL) << 56;  
  78.         result |= (payload.get(index + 6) & 0xFFFFL) << 48;  
  79.         result |= (payload.get(index + 5) & 0xFFFFL) << 40;  
  80.         result |= (payload.get(index + 4) & 0xFFFFL) << 32;  
  81.         result |= (payload.get(index + 3) & 0xFFFFL) << 24;  
  82.         result |= (payload.get(index + 2) & 0xFFFFL) << 16;  
  83.         result |= (payload.get(index + 1) & 0xFFFFL) << 8;  
  84.         result |= (payload.get(index + 0) & 0xFFFFL);  
  85.         index += 8;  
  86.         return result;  
  87.     }  
  88.     public long getUnsignedLong(){  
  89.         return getLong();  
  90.     }  
  91.     public long getLongReverse() {  
  92.         long result = 0;  
  93.         result |= (payload.get(index + 0) & 0xFFFFL) << 56;  
  94.         result |= (payload.get(index + 1) & 0xFFFFL) << 48;  
  95.         result |= (payload.get(index + 2) & 0xFFFFL) << 40;  
  96.         result |= (payload.get(index + 3) & 0xFFFFL) << 32;  
  97.         result |= (payload.get(index + 4) & 0xFFFFL) << 24;  
  98.         result |= (payload.get(index + 5) & 0xFFFFL) << 16;  
  99.         result |= (payload.get(index + 6) & 0xFFFFL) << 8;  
  100.         result |= (payload.get(index + 7) & 0xFFFFL);  
  101.         index += 8;  
  102.         return result;  
  103.     }  
  104.     public float getFloat() {  
  105.         return Float.intBitsToFloat(getInt());  
  106.     }  
  107.     public void putByte(byte data) {  
  108.         add(data);  
  109.     }  
  110.     public void putUnsignedByte(short data){  
  111.         if(data < UNSIGNED_BYTE_MIN_VALUE || data > UNSIGNED_BYTE_MAX_VALUE){  
  112.             throw new IllegalArgumentException("Value is outside of the range of an unsigned byte: " + data);  
  113.         }  
  114.         putByte((byte) data);  
  115.     }  
  116.     public void putShort(short data) {  
  117.         add((byte) (data >> 0));  
  118.         add((byte) (data >> 8));  
  119.     }  
  120.     public void putUnsignedShort(int data){  
  121.         if(data < UNSIGNED_SHORT_MIN_VALUE || data > UNSIGNED_SHORT_MAX_VALUE){  
  122.             throw new IllegalArgumentException("Value is outside of the range of an unsigned short: " + data);  
  123.         }  
  124.         putShort((short) data);  
  125.     }  
  126.     public void putInt(int data) {  
  127.         add((byte) (data >> 0));  
  128.         add((byte) (data >> 8));  
  129.         add((byte) (data >> 16));  
  130.         add((byte) (data >> 24));  
  131.     }  
  132.     public void putUnsignedInt(long data){  
  133.         if(data < UNSIGNED_INT_MIN_VALUE || data > UNSIGNED_INT_MAX_VALUE){  
  134.             throw new IllegalArgumentException("Value is outside of the range of an unsigned int: " + data);  
  135.         }  
  136.         putInt((int) data);  
  137.     }  
  138.     public void putLong(long data) {  
  139.         add((byte) (data >> 0));  
  140.         add((byte) (data >> 8));  
  141.         add((byte) (data >> 16));  
  142.         add((byte) (data >> 24));  
  143.         add((byte) (data >> 32));  
  144.         add((byte) (data >> 40));  
  145.         add((byte) (data >> 48));  
  146.         add((byte) (data >> 56));  
  147.     }  
  148.     public void putUnsignedLong(long data){  
  149.         if(data < UNSIGNED_LONG_MIN_VALUE){  
  150.             throw new IllegalArgumentException("Value is outside of the range of an unsigned long: " + data);  
  151.         }  
  152.         putLong(data);  
  153.     }  
  154.     public void putFloat(float data) {  
  155.         putInt(Float.floatToIntBits(data));  
  156.     }  
  157. }  

這個類的複用性很高,我們在很多解析器裡面都可以用到它,希望大家以後如果寫自己的解析器的話可以想到它。好的,我們現在有了資料Payload我們怎麼解析出消息呢？

我們回到我們的Packet類,Packet用了一個很典型的命名unpack來用來解包:

[java]  view plain  copy

  1.  public MAVLinkMessage unpack() {  
  2.         switch (msgid) {  
  3.             case msg_sensor_offsets.MAVLINK_MSG_ID_SENSOR_OFFSETS:  
  4.                 return  new msg_sensor_offsets(this);  
  5.             case msg_set_mag_offsets.MAVLINK_MSG_ID_SET_MAG_OFFSETS:  
  6.                 return  new msg_set_mag_offsets(this);  
  7.             case msg_meminfo.MAVLINK_MSG_ID_MEMINFO:  
  8.                 return  new msg_meminfo(this);  
  9. ......  
  10. }  

如果你自定義了一種MavLink協定類型的話,代碼生成器會自動幫你生成一個case和一個消息類,而在這裡,我們找到我們所需要的心跳類case:

[java]  view plain  copy

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

在心跳消息的構造器裡,具體消息類型會對封包的具體内容做真正的解包:

[java]  view plain  copy

  1. public void unpack(MAVLinkPayload payload) {  
  2.         payload.resetIndex();  
  3.         this.custom_mode = payload.getUnsignedInt();  
  4.         this.type = payload.getUnsignedByte();  
  5.         this.autopilot = payload.getUnsignedByte();  
  6.         this.base_mode = payload.getUnsignedByte();  
  7.         this.system_status = payload.getUnsignedByte();  
  8.         this.mavlink_version = payload.getUnsignedByte();  
  9.     }  

這樣,記錄在payload中雜亂的資料就被記錄在msg_heartbeat類的變量中啦~

大家如果感興趣,就自己去生成和閱讀它的代碼,代碼量很少很好讀懂,且通用性很好很好調試。

可參考：http://blog.csdn.net/hello__zero/article/details/51258087

無人機 解讀原理 android 無人機開發-Android地面站MavLink解析部分源碼 無人機開發 Android地面站 Android地面站MavLink
上一篇: 應用mavlink通信協定的四軸offboard地面站（上位機）
下一篇: 用JAVA實作Mavlink協定，完成IOT物聯網中與硬體互動

繼續閱讀