Android Ground station-mavlink parsing part of the source code

Mavlink is a lightweight communication protocol that is mainly used for communication between terminals and small unmanned vehicles. Because of its versatility, mavlink can be translated into a variety of languages for the code to be used in a variety of different environments. For more details on how to generate the corresponding Mavlink code from the tool, please visit:

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

The message defined by the Mavlink protocol is broadly divided into two categories, one generic message and the other a custom message. The data structure of a generic message and a custom message is the same, and the difference is only reflected in it. I take the most commonly used heartbeat message in Mavlink as an example:

<message id= "0" name= "HEARTBEAT" > <description>the HEARTBEAT message shows that a system is present and RESPO Nding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system Appropria Te (e.g laying out the user interface based on the autopilot) .</description> <field type= "uint8_t" Name= "Typ E ">type of the MAV (Quadrotor, helicopter, etc, up-to-types, defined in Mav_type ENUM) </field> <field Typ E= "uint8_t" name= "autopilot" >autopilot type/class. Defined in Mav_class enum</field> <field type= "uint8_t" name= "Base_mode" >system mode bitfield, see MAV_MODE_ FLAGS ENUM in mavlink/include/mavlink_types.h</field> <field type= "uint32_t" name= "Custom_mode" > Navigation mode bitfield, see Mav_autopilot_custom_mode ENUM for some examples. This field is autopilot-specific.</field> <field type= "uint8_t" name= "system_status" >system status Flag, See Mav_status ENUM&Lt;/field> <field type= "uint8_t_mavlink_version" name= "mavlink_version" >mavlink version</field></ Message>

The Heartbeat and the heartbeat in the push are a meaning. Due to the uncertainty of the network environment, the heartbeat is added to ensure the reliability of long connection. The message definition for Mavlink is based on the common XML format, and the root node is the <message> node. The message ID is from 0~255, in order to differentiate the custom message. The Name property defines the names of this message, and the description node records the purpose of the message. The field node is used to record the fields in the message. The number in the uint8_t represents how many bits the domain occupies. That is, uint8_t occupies 8 bits of one byte, while uint32_t occupies 32 bit,4 bytes. The meanings each domain represents are:

1. Type : representative of the types of small unmanned vehicles, possibly helicopters, automobiles, multi-rotor, etc.

2.Autopilot: represents this operating system platform, the type of platform by the Mav_type class:

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: A basic model for recording small vehicles

4.Custom_mode: recording the characteristic mode of the small-sized intersection tool

5.mavlink_version:version number of the Mavlink protocol

One might wonder why there is a basic model and a feature pattern, because Mavlink is a protocol that takes into account many types of small vehicles, so that all the basic patterns are not guaranteed to cover all traffic.

Next, we use the Mavlink-generator on the website to generate a set of Java code that is used in our Android program.

The generated code is very portable and we can copy it directly to our Android project. Let's look at the subcontracting of the generated code:

Common package: put some common Mavlink message and CRC Check tool

Ardupilotmega Package: store messages specific to the mega board

Messages Package: provides the message base class and some cache processing classes

enums Pack: Store Some constants

Mavlinkpacket class: used to record raw messages

Parser class: used to parse the data passed in the channel, generate the Mavlinkpacket format of the message.

Since the topic of this article is Mavlink messages in the Android ground station parsing, so we do not excessively focus on the channel and the business itself. We look at the above sub-package we will find that, in fact, for the analysis, the most important thing is the parser class. Before we start parsing, we'll recall the data structure of the heartbeat message through a graph, because we'll take it as an example:

The heartbeat complete message we received is a byte array, so we need to parse it, parse out our own object model, and call the Mavlink_parse_char (int c) method. Here's the problem, we're reading a byte array, but we're going to pass an int in the method. For this reason we might as well take a look at the parser class:

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 was a convenience function which handles the complete MAVLink * parsing. The function would parse one byte at a time and return the "complete packet once it could is successfully decoded.     Checksum and other * failures'll 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 = = MAVLINK PACKET.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; }    }}

We find that the parser class must have a linear parsing message, that is, only one message can be processed in the parser class within the same period. And the method structure of the parser class is essentially a state machine. The external code needs to traverse the data passed in to byte to generate the message:

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);}}        }

The list of states that the parser class has:

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    }

The parser class is essentially a state machine, the initial state is: Mav_parse_state_uninit. Once the parse succeeds or fails, the status goes to idle.

The state machine of the parser class can basically use the above image to indicate that there is basically no complicated content, mainly in the record with the first data length. If your data is longer than 0, the parser caches your data in a data structure called payload.

Case Mavlink_parse_state_got_msgid:            m.payload.add ((byte) c);            if (m.payloadisfilled ()) {state                = mav_states. mavlink_parse_state_got_payload;            }            Break

The corresponding class of payload is the Mavlinkpayload class, which is the buffer and converter of the data, that is, the meaningless byte array, organized into a meaningful platform data type.

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.all        Ocate (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;        Getunsignedbyte () {short result = 0;        Result |= payload.get (index + 0) & 0xFF;        index+= 1;     return result;        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_V        Alue) {throw new IllegalArgumentException ("Value is outside of the range of a 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 a 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 a Unsi        gned 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 Illegalarg        Umentexception ("Value is outside of the range of a unsigned long:" + data);    } putlong (data);    } public void Putfloat (float data) {Putint (float.floattointbits (data)); }}

The reusability of this class is very high, we can use it in many parsers, I hope you can think of it later if you write your own parser. Okay, now that we have the data payload, how do we parse the message?

We went back to our packet class, and packet used a very typical naming unpack to unpack the package:

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), ...}

If you customize a type of Mavlink protocol, the code generator will automatically help you generate a case and a message class, and here we find the heartbeat class we need:

Case Msg_heartbeat. Mavlink_msg_id_heartbeat:                return  new Msg_heartbeat (this);

In the heart of the heartbeat message constructor, the specific message type will do a real unpacking of the specific contents of the packet:

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 ();            }

In this way, the data recorded in the payload is recorded in the variables of the Msg_heartbeat class ~

If you are interested, you can generate and read its code, the code is very few very good read, and the versatility is very good debugging.

Android Ground station-mavlink parsing part of the source code