Android教程網 >> Android技術 >> 關於Android編程 >> Android地面站-MavLink解析部分源碼

Android地面站-MavLink解析部分源碼

編輯：關於Android編程

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

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

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

  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).
  Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
  Autopilot type / class. defined in MAV_CLASS ENUM
  System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
  Navigation mode bitfield, see MAV_AUTOPILOT_CUSTOM_MODE ENUM for some examples. This field is autopilot-specific.
  System status flag, see MAV_STATUS ENUM
  MAVLink version

這裡的心跳和push中的心跳是一個意思。由於網絡環境的不確定性,加入心跳來保證長連接的可靠性。MavLink的消息定義以通用的XML格式為基准,並且根節點是節點。消息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類的變量中啦~

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

上一頁:Android網絡編程（總結）
下一頁:Android開發本地及網絡Mp3音樂播放器(九)音樂收藏與列表切換

關於Android編程

Android編程入門 Android技術基礎 Android資訊關於Android編程 Android開發 Android開發環境 Android開發實例

相關文章