MAVLink是一种非常轻量级的消息传输协议, 用于地面控制终端（地面站）与无人机之间 (以及机载无人机组件之间) 进行通信。
一、自动生成mavlink协议代码：
1、安装MavLink环境；
2、生成 MAVLink 库文件；
3、装好环境后，利用mavgenerate把以下的demo.xml生成对应的mavlink JAVA/C/C++代码
<?xml version="1.0"?>
<mavlink>
<version>1</version>
<enums>
<enum name="SYS_STATE">
<description>System state.</description>
<entry value="0" name="SYS_STOP">
<description>System stop state</description>
</entry>
<entry value="1" name="SYS_RUN">
<description>System run state</description>
</entry>
</enum>
</enums>
<messages>
<message id="150" name="SysInfo">
<description>System information.</description>
<field type="uint8_t" name="sysState" >System state.</field>
<field type="uint16_t" name="voltageBattery">Battery voltage</field>
</message>
</messages>
</mavlink>
生成的代码在项目MyMavlinkDemoModulede的heartbeat中。
二、MavLink2帧格式：
以上是MavLink的一帧格式，整个协议组包和解包过程都是围绕着这帧进行的
三、自动生成mavlink的Java/C/C++代码分析：
协议握手图：
我们以Java相关的代码为GCS端，以C/C++相关代码为Drone端为例。
GCS端Java代码分析：
工具类MAVLinkMessage、MAVLinkPayload、MAVLinkPacket、Paser对象协议的封装起到至关重要的作用，而msg_sysinfo是具体到每条有效的命令。
- MAVLinkMessage 类定义了 sysid、compid、msgid的帧头的属性，提供了pack()对MAVLinkPacket的组装和unpack()剥离出命令中的属性的两个方法：
/* AUTO-GENERATED FILE. DO NOT MODIFY.
*
* This class was automatically generated by the
* java mavlink generator tool. It should not be modified by hand.
*/
package com.MAVLink.Messages;
import java.io.Serializable;
import com.MAVLink.MAVLinkPacket;
/**
* Common interface for all MAVLink messages
*/
public abstract class MAVLinkMessage implements Serializable {
private static final long serialVersionUID = -7754622750478538539L;
// The MAVLink message classes have been changed to implement Serializable,
// this way is possible to pass a mavlink message through the Service-Acctivity interface
public int sysid;
public int compid;
public int msgid;
public boolean isMavlink2;
public abstract MAVLinkPacket pack();
public abstract void unpack(MAVLinkPayload payload);
public abstract String toString();
public abstract String name();
}
- msg_sysinfo类继承了MAVLinkMessage ，属性voltageBattery、sysState对象的是XML中标签<field>的两个命令值。
// MESSAGE SysInfo PACKING
package com.MAVLink.Demo_heart;
import com.MAVLink.MAVLinkPacket;
import com.MAVLink.Messages.MAVLinkMessage;
import com.MAVLink.Messages.MAVLinkPayload;
import com.MAVLink.Messages.Units;
import com.MAVLink.Messages.Description;
public class msg_sysinfo extends MAVLinkMessage {
public static final int MAVLINK_MSG_ID_SysInfo = 150;
public static final int MAVLINK_MSG_LENGTH = 3;
private static final long serialVersionUID = MAVLINK_MSG_ID_SysInfo;
@Description("Battery voltage")
@Units("")
public int voltageBattery;
@Description("System state.")
@Units("")
public short sysState;
@Override
public MAVLinkPacket pack() {
MAVLinkPacket packet = new MAVLinkPacket(MAVLINK_MSG_LENGTH,isMavlink2);
packet.sysid = sysid;
packet.compid = compid;
packet.msgid = MAVLINK_MSG_ID_SysInfo;
packet.payload.putUnsignedShort(voltageBattery);
packet.payload.putUnsignedByte(sysState);
if (isMavlink2) {
}
return packet;
}
@Override
public void unpack(MAVLinkPayload payload) {
payload.resetIndex();
this.voltageBattery = payload.getUnsignedShort();
this.sysState = payload.getUnsignedByte();
if (isMavlink2) {
}
}
public msg_sysinfo() {
this.msgid = MAVLINK_MSG_ID_SysInfo;
}
public msg_sysinfo( int voltageBattery, short sysState) {
this.msgid = MAVLINK_MSG_ID_SysInfo;
this.voltageBattery = voltageBattery;
this.sysState = sysState;
}
public msg_sysinfo( int voltageBattery, short sysState, int sysid, int compid, boolean isMavlink2) {
this.msgid = MAVLINK_MSG_ID_SysInfo;
this.sysid = sysid;
this.compid = compid;
this.isMavlink2 = isMavlink2;
this.voltageBattery = voltageBattery;
this.sysState = sysState;
}
public msg_sysinfo(MAVLinkPacket mavLinkPacket) {
this.msgid = MAVLINK_MSG_ID_SysInfo;
this.sysid = mavLinkPacket.sysid;
this.compid = mavLinkPacket.compid;
this.isMavlink2 = mavLinkPacket.isMavlink2;
unpack(mavLinkPacket.payload);
}
@Override
public String toString() {
return "MAVLINK_MSG_ID_SysInfo - sysid:"+sysid+" compid:"+compid+" voltageBattery:"+voltageBattery+" sysState:"+sysState+"";
}
@Override
public String name() {
return "MAVLINK_MSG_ID_SysInfo";
}
}
- MAVLinkPayload类对属性中的帧的有效负载payload做了数值的定义及拼装，例：在组帧pack()方法中对sysState的数据类型及消息命令顺序位置偏移。
package com.MAVLink.Messages;
import java.nio.ByteBuffer;
public class MAVLinkPayload {
private static final byte UNSIGNED_BYTE_MIN_VALUE = 0;
private static final short UNSIGNED_BYTE_MAX_VALUE = Byte.MAX_VALUE - Byte.MIN_VALUE;
private static final short UNSIGNED_SHORT_MIN_VALUE = 0;
private static final int UNSIGNED_SHORT_MAX_VALUE = Short.MAX_VALUE - Short.MIN_VALUE;
private static final int UNSIGNED_INT_MIN_VALUE = 0;
private static final long UNSIGNED_INT_MAX_VALUE = (long) Integer.MAX_VALUE - Integer.MIN_VALUE;
private static final long UNSIGNED_LONG_MIN_VALUE = 0;
public static final int MAX_PAYLOAD_SIZE = 255;
public final ByteBuffer payload;
public int index;
public MAVLinkPayload() {
// This has to be larger than the received payloadSize since MAVLINK V2 will truncate the payloads to the last non-zero value
payload = ByteBuffer.allocate(MAX_PAYLOAD_SIZE);
}
public ByteBuffer getData() {
return payload;
}
public int size() {
return payload.position();
}
public void add(byte c) {
payload.put(c);
}
public void resetIndex() {
index = 0;
}
public byte getByte() {
byte result = 0;
result |= (payload.get(index + 0) & 0xFF);
index += 1;
return result;
}
public short getUnsignedByte() {
short result = 0;
result |= payload.get(index + 0) & 0xFF;
index += 1;
return result;
}
public short getShort() {
short result = 0;
result |= (payload.get(index + 1) & 0xFF) << 8;
result |= (payload.get(index + 0) & 0xFF);
index += 2;
return result;
}
public int getUnsignedShort() {
int result = 0;
result |= (payload.get(index + 1) & 0xFF) << 8;
result |= (payload.get(index + 0) & 0xFF);
index += 2;
return result;
}
public int getInt() {
int result = 0;
result |= (payload.get(index + 3) & 0xFF) << 24;
result |= (payload.get(index + 2) & 0xFF) << 16;
result |= (payload.get(index + 1) & 0xFF) << 8;
result |= (payload.get(index + 0) & 0xFF);
index += 4;
return result;
}
public long getUnsignedInt() {
long result = 0;
result |= (payload.get(index + 3) & 0xFFL) << 24;
result |= (payload.get(index + 2) & 0xFFL) << 16;
result |= (payload.get(index + 1) & 0xFFL) << 8;
result |= (payload.get(index + 0) & 0xFFL);
index += 4;
return result;
}
public long getLong() {
long result = 0;
result |= (payload.get(index + 7) & 0xFFL) << 56;
result |= (payload.get(index + 6) & 0xFFL) << 48;
result |= (payload.get(index + 5) & 0xFFL) << 40;
result |= (payload.get(index + 4) & 0xFFL) << 32;
result |= (payload.get(index + 3) & 0xFFL) << 24;
result |= (payload.get(index + 2) & 0xFFL) << 16;
result |= (payload.get(index + 1) & 0xFFL) << 8;
result |= (payload.get(index + 0) & 0xFFL);
index += 8;
return result;
}
public long getUnsignedLong(){
return getLong();
}
public long getLongReverse() {
long result = 0;
result |= (payload.get(index + 0) & 0xFFL) << 56;
result |= (payload.get(index + 1) & 0xFFL) << 48;
result |= (payload.get(index + 2) & 0xFFL) << 40;
result |= (payload.get(index + 3) & 0xFFL) << 32;
result |= (payload.get(index + 4) & 0xFFL) << 24;
result |= (payload.get(index + 5) & 0xFFL) << 16;
result |= (payload.get(index + 6) & 0xFFL) << 8;
result |= (payload.get(index + 7) & 0xFFL);
index += 8;
return result;
}
public float getFloat() {
return Float.intBitsToFloat(getInt());
}
public double getDouble() {
return Double.longBitsToDouble(getLong());
}
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));
}
public void putDouble(double data) {
putLong(Double.doubleToLongBits(data));
}
}
- Parser类对来自Drone端的字节数据流解析成一个完整的帧，mavlink_parse_char(int c)方法把Drone端组装成mavlink帧即MAVLinkPacket：
package com.MAVLink;
import com.MAVLink.MAVLinkPacket;
import com.MAVLink.Messages.MAVLinkStats;
public class Parser {
static final String TAG = Parser.class.getSimpleName();
static final boolean V = false;
static void logv(String tag, String msg) {
if(V) System.out.println(String.format("%s: %s", tag, msg));
}
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_INCOMPAT_FLAGS, // MAVLink 2
MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS, // MAVLink 2
MAVLINK_PARSE_STATE_GOT_SEQ,
MAVLINK_PARSE_STATE_GOT_SYSID,
MAVLINK_PARSE_STATE_GOT_COMPID,
MAVLINK_PARSE_STATE_GOT_MSGID1,
MAVLINK_PARSE_STATE_GOT_MSGID2, // MAVLink 2
MAVLINK_PARSE_STATE_GOT_MSGID3, // MAVLink 2
MAVLINK_PARSE_STATE_GOT_CRC1,
MAVLINK_PARSE_STATE_GOT_CRC2, // MAVLink 2
MAVLINK_PARSE_STATE_GOT_PAYLOAD,
MAVLINK_PARSE_STATE_GOT_SIGNATURE, // MAVLink 2
}
private MAV_states state = MAV_states.MAVLINK_PARSE_STATE_UNINIT;
public MAVLinkStats stats;
private MAVLinkPacket m;
private boolean isMavlink2;
public Parser() {
this(false);
}
public Parser(boolean ignoreRadioPacketStats) {
stats = new MAVLinkStats(ignoreRadioPacketStats);
isMavlink2 = false;
}
public MAVLinkPacket mavlink_parse_char(int c) {
// force to 8 bits
c &= 0xFF;
switch (state) {
case MAVLINK_PARSE_STATE_UNINIT:
case MAVLINK_PARSE_STATE_IDLE:
// MAVLink 1 and 2
if (c == MAVLinkPacket.MAVLINK_STX_MAVLINK2) {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_STX;
if (!isMavlink2) {
isMavlink2 = true;
logv(TAG, "Turning mavlink2 ON");
}
} else if (c == MAVLinkPacket.MAVLINK_STX_MAVLINK1) {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_STX;
if (isMavlink2) {
isMavlink2 = false;
logv(TAG, "Turning mavlink2 OFF");
}
}
break;
case MAVLINK_PARSE_STATE_GOT_STX:
// MAVLink 1 and 2
m = new MAVLinkPacket(c, isMavlink2);
if (isMavlink2) {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_LENGTH;
} else {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
}
break;
case MAVLINK_PARSE_STATE_GOT_LENGTH:
// MAVLink 1 and 2
m.incompatFlags = c;
if (c != 0 && c != 1) {
// message includes an incompatible feature flag
state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
break;
}
state = MAV_states.MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS;
break;
case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS:
// MAVLink 2 only
m.compatFlags = c;
state = MAV_states.MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
break;
case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS:
m.seq = c;
state = MAV_states.MAVLINK_PARSE_STATE_GOT_SEQ;
break;
case MAVLINK_PARSE_STATE_GOT_SEQ:
// back to MAVLink 1 and 2
m.sysid = c;
state = MAV_states.MAVLINK_PARSE_STATE_GOT_SYSID;
break;
case MAVLINK_PARSE_STATE_GOT_SYSID:
// MAVLink 1 and 2
m.compid = c;
state = MAV_states.MAVLINK_PARSE_STATE_GOT_COMPID;
break;
case MAVLINK_PARSE_STATE_GOT_COMPID:
// MAVLink 1 and 2
m.msgid = c;
if (isMavlink2) {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_MSGID1;
} else if (m.len > 0) {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_MSGID3;
} else {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;
}
break;
case MAVLINK_PARSE_STATE_GOT_MSGID1:
// MAVLink 2 only
m.msgid |= c << 8;
state = MAV_states.MAVLINK_PARSE_STATE_GOT_MSGID2;
break;
case MAVLINK_PARSE_STATE_GOT_MSGID2:
// MAVLink 2 only
m.msgid |= c << 16;
if (m.len > 0) {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_MSGID3;
} else {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;
}
break;
case MAVLINK_PARSE_STATE_GOT_MSGID3:
// back to MAVLink 1 and 2
m.payload.add((byte) c);
if (m.payloadIsFilled()) {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;
}
break;
case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
boolean crcGen = m.generateCRC(m.payload.size());
// Check first checksum byte and verify the CRC was successfully generated (msg extra exists)
if (c != m.crc.getLSB() || !crcGen) {
state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
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()) {
state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
stats.crcError();
} else { // crc is good
stats.newPacket(m);
if (!isMavlink2 || (m.incompatFlags != 0x01)) {
// If no signature, then return the message.
state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
return m;
} else {
// TODO: MAVLink 2 - signed
state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
stats.crcError();
}
}
break;
case MAVLINK_PARSE_STATE_GOT_CRC2:
// TODO: implement signature parsing and validation
state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
stats.crcError();
break;
} // switch
return null;
}
}
- MAVLinkPacket类是一个包装类包含了消息协议的组帧和解帧，encodePacket()会把mavlink帧的STX、payloadSize、flags、sysid、compid、msgid、payload、crc都拼接起来，unpack会根据msgid分离出不同类型的消息命令如msg_sysinfo：
package com.MAVLink;
import java.io.Serializable;
import com.MAVLink.Messages.MAVLinkPayload;
import com.MAVLink.Messages.MAVLinkMessage;
import com.MAVLink.Demo_heart.CRC;
import com.MAVLink.Demo_heart.*;
public class MAVLinkPacket implements Serializable {
private static final long serialVersionUID = 2095947771227815314L;
public static final int MAVLINK_STX_MAVLINK1 = 0xFE; // 254
public static final int MAVLINK_STX_MAVLINK2 = 0xFD; // 253
public static final int MAVLINK1_HEADER_LEN = 6;
public static final int MAVLINK2_HEADER_LEN = 10;
public static final int MAVLINK1_NONPAYLOAD_LEN = MAVLINK1_HEADER_LEN + 2;
public static final int MAVLINK2_NONPAYLOAD_LEN = MAVLINK2_HEADER_LEN + 2;
static final boolean V = false;
static void logv(String str) {
if(V) System.out.println(String.format("MAVLinkPacket: %s", str));
}
public final int len;
public int seq;
public int sysid;
public int compid;
public int msgid;
public MAVLinkPayload payload;
public CRC crc;
public boolean isMavlink2;
public int incompatFlags;
public int compatFlags;
public MAVLinkPacket(int payloadLength) {
this(payloadLength, false);
}
public MAVLinkPacket(final int payloadLength, final boolean isMavlink2) {
len = payloadLength;
payload = new MAVLinkPayload();
this.isMavlink2 = isMavlink2;
}
public boolean payloadIsFilled() {
return payload.size() >= len;
}
public boolean generateCRC(final int payloadSize) {
if (crc == null) {
crc = new CRC();
} else {
crc.start_checksum();
}
if (isMavlink2) {
crc.update_checksum(payloadSize);
crc.update_checksum(incompatFlags);
crc.update_checksum(compatFlags);
crc.update_checksum(seq);
crc.update_checksum(sysid);
crc.update_checksum(compid);
crc.update_checksum(msgid);
crc.update_checksum(msgid >>> 8);
crc.update_checksum(msgid >>> 16);
} else {
crc.update_checksum(payloadSize);
crc.update_checksum(seq);
crc.update_checksum(sysid);
crc.update_checksum(compid);
crc.update_checksum(msgid);
}
payload.resetIndex();
for (int i = 0; i < payloadSize; i++) {
crc.update_checksum(payload.getByte());
}
return crc.finish_checksum(msgid);
}
private int mavTrimPayload(final byte[] payload)
{
int length = payload.length;
while (length > 1 && payload[length-1] == 0) {
length--;
}
return length;
}
public byte[] encodePacket() {
final int bufLen;
final int payloadSize;
if (isMavlink2) {
payloadSize = mavTrimPayload(payload.payload.array());
bufLen = MAVLINK2_HEADER_LEN + payloadSize + 2;
} else {
payloadSize = payload.size();
bufLen = MAVLINK1_HEADER_LEN + payloadSize + 2;
}
byte[] buffer = new byte[bufLen];
int i = 0;
if (isMavlink2) {
buffer[i++] = (byte) MAVLINK_STX_MAVLINK2;
buffer[i++] = (byte) payloadSize;
buffer[i++] = (byte) incompatFlags;
buffer[i++] = (byte) compatFlags;
buffer[i++] = (byte) seq;
buffer[i++] = (byte) sysid;
buffer[i++] = (byte) compid;
buffer[i++] = (byte) (msgid & 0XFF);
buffer[i++] = (byte) ((msgid >>> 8) & 0XFF);
buffer[i++] = (byte) ((msgid >>> 16) & 0XFF);
} else {
buffer[i++] = (byte) MAVLINK_STX_MAVLINK1;
buffer[i++] = (byte) payloadSize;
buffer[i++] = (byte) seq;
buffer[i++] = (byte) sysid;
buffer[i++] = (byte) compid;
buffer[i++] = (byte) msgid;
}
for (int j = 0; j < payloadSize; ++j) {
buffer[i++] = payload.payload.get(j);
}
generateCRC(payloadSize);
buffer[i++] = (byte) (crc.getLSB());
buffer[i++] = (byte) (crc.getMSB());
logv(String.format("encode: isMavlink2=%s msgid=%d", isMavlink2, msgid));
return buffer;
}
public MAVLinkMessage unpack() {
switch (msgid) {
case msg_sysinfo.MAVLINK_MSG_ID_SysInfo:
return new msg_sysinfo(this);
default:
return null;
}
}
}
Drone端C/C++代码分析：
mavlink_msg_sysinfo.hpp封装了有效的命令类比Java代码的msg_sysinfo类、mavlink_msg_sysinfo.hpp继承Message是相对于msg_sysinfo继承了MAVLinkMessage。工具类mavlink_helpers.h包含了组帧和解帧的具体操作及接收数据、发送数据的操作。
- mavlink_msg_sysinfo的组帧的调用：
static inline uint16_t
mavlink_msg_sysinfo_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t *msg,
const mavlink_sysinfo_t *sysinfo) {
return mavlink_msg_sysinfo_pack(system_id, component_id, msg, sysinfo->sysState,
sysinfo->voltageBattery);
}- mavlink_helpers.h以下函数对传进来的数据进行组帧：
MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
mavlink_status_t* status, uint8_t min_length, uint8_t length, uint8_t crc_extra)
{
bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0;
#ifndef MAVLINK_NO_SIGN_PACKET
bool signing = (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING);
#else
bool signing = false;
#endif
uint8_t signature_len = signing? MAVLINK_SIGNATURE_BLOCK_LEN : 0;
uint8_t header_len = MAVLINK_CORE_HEADER_LEN+1;
uint8_t buf[MAVLINK_CORE_HEADER_LEN+1];
if (mavlink1) {
msg->magic = MAVLINK_STX_MAVLINK1;
header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN+1;
} else {
msg->magic = MAVLINK_STX;
}
msg->len = mavlink1?min_length:_mav_trim_payload(_MAV_PAYLOAD(msg), length);
msg->sysid = system_id;
msg->compid = component_id;
msg->incompat_flags = 0;
if (signing) {
msg->incompat_flags |= MAVLINK_IFLAG_SIGNED;
}
msg->compat_flags = 0;
msg->seq = status->current_tx_seq;
status->current_tx_seq = status->current_tx_seq + 1;
// form the header as a byte array for the crc
buf[0] = msg->magic;
buf[1] = msg->len;
if (mavlink1) {
buf[2] = msg->seq;
buf[3] = msg->sysid;
buf[4] = msg->compid;
buf[5] = msg->msgid & 0xFF;
} else {
buf[2] = msg->incompat_flags;
buf[3] = msg->compat_flags;
buf[4] = msg->seq;
buf[5] = msg->sysid;
buf[6] = msg->compid;
buf[7] = msg->msgid & 0xFF;
buf[8] = (msg->msgid >> 8) & 0xFF;
buf[9] = (msg->msgid >> 16) & 0xFF;
}
uint16_t checksum = crc_calculate(&buf[1], header_len-1);
crc_accumulate_buffer(&checksum, _MAV_PAYLOAD(msg), msg->len);
crc_accumulate(crc_extra, &checksum);
mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF);
mavlink_ck_b(msg) = (uint8_t)(checksum >> 8);
msg->checksum = checksum;
#ifndef MAVLINK_NO_SIGN_PACKET
if (signing) {
mavlink_sign_packet(status->signing,
msg->signature,
(const uint8_t *)buf, header_len,
(const uint8_t *)_MAV_PAYLOAD(msg), msg->len,
(const uint8_t *)_MAV_PAYLOAD(msg)+(uint16_t)msg->len);
}
#endif
return msg->len + header_len + 2 + signature_len;
}- mavlink_helpers.h实现了几种不同数据流的发送函数：_mav_finalize_message_chan_sen()、_mavlink_resend_uart()、mavlink_msg_to_send_buffer()。以下以mavlink_msg_to_send_buffer()为例:
/**
* @brief Pack a message to send it over a serial byte stream
*/
MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buf, const mavlink_message_t *msg)
{
uint8_t signature_len, header_len;
uint8_t *ck;
uint8_t length = msg->len;
if (msg->magic == MAVLINK_STX_MAVLINK1) {
signature_len = 0;
header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN;
buf[0] = msg->magic;
buf[1] = length;
buf[2] = msg->seq;
buf[3] = msg->sysid;
buf[4] = msg->compid;
buf[5] = msg->msgid & 0xFF;
memcpy(&buf[6], _MAV_PAYLOAD(msg), msg->len);
ck = buf + header_len + 1 + (uint16_t)msg->len;
} else {
length = _mav_trim_payload(_MAV_PAYLOAD(msg), length);
header_len = MAVLINK_CORE_HEADER_LEN;
buf[0] = msg->magic;
buf[1] = length;
buf[2] = msg->incompat_flags;
buf[3] = msg->compat_flags;
buf[4] = msg->seq;
buf[5] = msg->sysid;
buf[6] = msg->compid;
buf[7] = msg->msgid & 0xFF;
buf[8] = (msg->msgid >> 8) & 0xFF;
buf[9] = (msg->msgid >> 16) & 0xFF;
memcpy(&buf[10], _MAV_PAYLOAD(msg), length);
ck = buf + header_len + 1 + (uint16_t)length;
signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0;
}
ck[0] = (uint8_t)(msg->checksum & 0xFF);
ck[1] = (uint8_t)(msg->checksum >> 8);
if (signature_len > 0) {
memcpy(&ck[2], msg->signature, signature_len);
}
return header_len + 1 + 2 + (uint16_t)length + (uint16_t)signature_len;
}- mavlink_helpers.h的mavlink_frame_char_buffer()从接收的数据组成帧：
/**
* This is a variant of mavlink_frame_char() but with caller supplied
* parsing buffers. It is useful when you want to create a MAVLink
* parser in a library that doesn't use any global variables
*
* @param rxmsg parsing message buffer
* @param status parsing status buffer
* @param c The char to parse
*
* @param r_message NULL if no message could be decoded, otherwise the message data
* @param r_mavlink_status if a message was decoded, this is filled with the channel's stats
* @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
*
*/
MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
mavlink_status_t* status,
uint8_t c,
mavlink_message_t* r_message,
mavlink_status_t* r_mavlink_status)
{
status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
switch (status->parse_state)
{
case MAVLINK_PARSE_STATE_UNINIT:
case MAVLINK_PARSE_STATE_IDLE:
if (c == MAVLINK_STX)
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
rxmsg->len = 0;
rxmsg->magic = c;
status->flags &= ~MAVLINK_STATUS_FLAG_IN_MAVLINK1;
mavlink_start_checksum(rxmsg);
} else if (c == MAVLINK_STX_MAVLINK1)
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
rxmsg->len = 0;
rxmsg->magic = c;
status->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1;
mavlink_start_checksum(rxmsg);
}
break;
case MAVLINK_PARSE_STATE_GOT_STX:
if (status->msg_received
/* Support shorter buffers than the
default maximum packet size */
#if (MAVLINK_MAX_PAYLOAD_LEN < 255)
|| c > MAVLINK_MAX_PAYLOAD_LEN
#endif
)
{
status->buffer_overrun++;
_mav_parse_error(status);
status->msg_received = 0;
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
}
else
{
// NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
rxmsg->len = c;
status->packet_idx = 0;
mavlink_update_checksum(rxmsg, c);
if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
rxmsg->incompat_flags = 0;
rxmsg->compat_flags = 0;
status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
} else {
status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
}
}
break;
case MAVLINK_PARSE_STATE_GOT_LENGTH:
rxmsg->incompat_flags = c;
if ((rxmsg->incompat_flags & ~MAVLINK_IFLAG_MASK) != 0) {
// message includes an incompatible feature flag
_mav_parse_error(status);
status->msg_received = 0;
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
break;
}
mavlink_update_checksum(rxmsg, c);
status->parse_state = MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS;
break;
case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS:
rxmsg->compat_flags = c;
mavlink_update_checksum(rxmsg, c);
status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
break;
case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS:
rxmsg->seq = c;
mavlink_update_checksum(rxmsg, c);
status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
break;
case MAVLINK_PARSE_STATE_GOT_SEQ:
rxmsg->sysid = c;
mavlink_update_checksum(rxmsg, c);
status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
break;
case MAVLINK_PARSE_STATE_GOT_SYSID:
rxmsg->compid = c;
mavlink_update_checksum(rxmsg, c);
status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
break;
case MAVLINK_PARSE_STATE_GOT_COMPID:
rxmsg->msgid = c;
mavlink_update_checksum(rxmsg, c);
if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
if(rxmsg->len > 0) {
status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
} else {
status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
}
#ifdef MAVLINK_CHECK_MESSAGE_LENGTH
if (rxmsg->len < mavlink_min_message_length(rxmsg) ||
rxmsg->len > mavlink_max_message_length(rxmsg)) {
_mav_parse_error(status);
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
break;
}
#endif
} else {
status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID1;
}
break;
case MAVLINK_PARSE_STATE_GOT_MSGID1:
rxmsg->msgid |= ((uint32_t)c)<<8;
mavlink_update_checksum(rxmsg, c);
status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID2;
break;
case MAVLINK_PARSE_STATE_GOT_MSGID2:
rxmsg->msgid |= ((uint32_t)c)<<16;
mavlink_update_checksum(rxmsg, c);
if(rxmsg->len > 0){
status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
} else {
status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
}
#ifdef MAVLINK_CHECK_MESSAGE_LENGTH
if (rxmsg->len < mavlink_min_message_length(rxmsg) ||
rxmsg->len > mavlink_max_message_length(rxmsg))
{
_mav_parse_error(status);
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
break;
}
#endif
break;
case MAVLINK_PARSE_STATE_GOT_MSGID3:
_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
mavlink_update_checksum(rxmsg, c);
if (status->packet_idx == rxmsg->len)
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
}
break;
case MAVLINK_PARSE_STATE_GOT_PAYLOAD: {
const mavlink_msg_entry_t *e = mavlink_get_msg_entry(rxmsg->msgid);
uint8_t crc_extra = e?e->crc_extra:0;
mavlink_update_checksum(rxmsg, crc_extra);
if (c != (rxmsg->checksum & 0xFF)) {
status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1;
} else {
status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
}
rxmsg->ck[0] = c;
// zero-fill the packet to cope with short incoming packets
if (e && status->packet_idx < e->max_msg_len) {
memset(&_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx], 0, e->max_msg_len - status->packet_idx);
}
break;
}
case MAVLINK_PARSE_STATE_GOT_CRC1:
case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:
if (status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c != (rxmsg->checksum >> 8)) {
// got a bad CRC message
status->msg_received = MAVLINK_FRAMING_BAD_CRC;
} else {
// Successfully got message
status->msg_received = MAVLINK_FRAMING_OK;
}
rxmsg->ck[1] = c;
if (rxmsg->incompat_flags & MAVLINK_IFLAG_SIGNED) {
status->parse_state = MAVLINK_PARSE_STATE_SIGNATURE_WAIT;
status->signature_wait = MAVLINK_SIGNATURE_BLOCK_LEN;
// If the CRC is already wrong, don't overwrite msg_received,
// otherwise we can end up with garbage flagged as valid.
if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) {
status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
}
} else {
if (status->signing &&
(status->signing->accept_unsigned_callback == NULL ||
!status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {
// If the CRC is already wrong, don't overwrite msg_received.
if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) {
status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;
}
}
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
if (r_message != NULL) {
memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
}
}
break;
case MAVLINK_PARSE_STATE_SIGNATURE_WAIT:
rxmsg->signature[MAVLINK_SIGNATURE_BLOCK_LEN-status->signature_wait] = c;
status->signature_wait--;
if (status->signature_wait == 0) {
// we have the whole signature, check it is OK
#ifndef MAVLINK_NO_SIGNATURE_CHECK
bool sig_ok = mavlink_signature_check(status->signing, status->signing_streams, rxmsg);
#else
bool sig_ok = true;
#endif
if (!sig_ok &&
(status->signing->accept_unsigned_callback &&
status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {
// accepted via application level override
sig_ok = true;
}
if (sig_ok) {
status->msg_received = MAVLINK_FRAMING_OK;
} else {
status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;
}
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
if (r_message !=NULL) {
memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
}
}
break;
}
// If a message has been successfully decoded, check index
if (status->msg_received == MAVLINK_FRAMING_OK)
{
//while(status->current_seq != rxmsg->seq)
//{
// status->packet_rx_drop_count++;
// status->current_seq++;
//}
status->current_rx_seq = rxmsg->seq;
// Initial condition: If no packet has been received so far, drop count is undefined
if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
// Count this packet as received
status->packet_rx_success_count++;
}
if (r_message != NULL) {
r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg
}
if (r_mavlink_status != NULL) {
r_mavlink_status->parse_state = status->parse_state;
r_mavlink_status->packet_idx = status->packet_idx;
r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
r_mavlink_status->packet_rx_drop_count = status->parse_error;
r_mavlink_status->flags = status->flags;
}
status->parse_error = 0;
if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) {
/*
the CRC came out wrong. We now need to overwrite the
msg CRC with the one on the wire so that if the
caller decides to forward the message anyway that
mavlink_msg_to_send_buffer() won't overwrite the
checksum
*/
if (r_message != NULL) {
r_message->checksum = rxmsg->ck[0] | (rxmsg->ck[1]<<8);
}
}
return status->msg_received;
}四、封装自定义的类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的属性值：
public class AckSysInfo extends BaseAckMsg {
public static final int MAVLINK_MSG_ID_SysInfo = 150;
public static final int DEST_ID_SysInfo = 3;
public static final int SRC_ID_SysInfo = 2;
public static final int MAVLINK_MSG_LENGTH = 3;
private static final long serialVersionUID = MAVLINK_MSG_ID_SysInfo;
public int voltageBattery;
public short sysState;
public AckSysInfo() {
}
public AckSysInfo(MyLinkPacket myLinkPacket) {
this.sysid = myLinkPacket.sysid;
desId = myLinkPacket.destId;
srcId = myLinkPacket.srcId;
this.compid = myLinkPacket.compid;
this.msgid = MAVLINK_MSG_ID_SysInfo;
unpack(myLinkPacket);
}
//解析出payload包含的实体数值
private void unpack(MyLinkPacket myLinkPacket) {
super.decrypt(myLinkPacket);
sysState = myLinkPacket.payload.getShort();
voltageBattery = myLinkPacket.payload.getInt();
}
@Override
public String toString() {
return "AckSysInfo{" +
"voltageBattery=" + voltageBattery +
", sysState=" + sysState +
'}';
}
}- CmdSysInfo会包playload的属性值包装成一个MyLinkPacket：
public class CmdSysInfo extends BaseCmd {
public static final int MAVLINK_MSG_ID_SysInfo = 150;
public static final int MAVLINK_MSG_LENGTH = 3;
public static final int DEST_ID_SysInfo = 3;
public static final int SRC_ID_SysInfo = 2;
private static final long serialVersionUID = MAVLINK_MSG_ID_SysInfo;
public int voltageBattery;
public short sysState;
public CmdSysInfo(int voltageBattery, short sysState) {
this.voltageBattery = voltageBattery;
this.sysState = sysState;
}
@Override
public MyLinkPacket pack() {
MyLinkPacket myLinkPacket = new MyLinkPacket(MAVLINK_MSG_LENGTH);
//设置帧头的sysid、destId、srcId、compid、msgid
myLinkPacket.sysid = 1;
myLinkPacket.destId = DEST_ID_SysInfo;
myLinkPacket.srcId = SRC_ID_SysInfo;
myLinkPacket.compid = 3;
myLinkPacket.msgid = 3;
//添加命令playload实际的值
myLinkPacket.payload.putUnsignedShort(voltageBattery);
myLinkPacket.payload.putUnsignedByte(sysState);
return null;
}
@Override
public String toString() {
return "AckSysInfo{" +
"voltageBattery=" + voltageBattery +
", sysState=" + sysState +
'}';
}
}
接收ACK类：
- ReceiveThread是一个接收处理类，如接收UDP的数据或者是AOA串口的数据这个因项目而异。作用是在于接收的数据处理成对应的ACK（如：AckSysInfo）的数据。
- MyParser如MavLink2中Parse功能一样，这里增加了对SrcId和destId的解析处理。
public class MyParser {
static final String TAG = MyParser.class.getSimpleName();
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_INCOMPAT_FLAGS, // MAVLink 2
MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS, // MAVLink 2
MAVLINK_PARSE_STATE_GOT_SEQ,
MAVLINK_PARSE_STATE_GOT_SYSID,
MAVLINK_PARSE_STATE_GOT_DESTID,
MAVLINK_PARSE_STATE_GOT_SRCID,
MAVLINK_PARSE_STATE_GOT_COMPID,
MAVLINK_PARSE_STATE_GOT_MSGID1,
MAVLINK_PARSE_STATE_GOT_MSGID2,
MAVLINK_PARSE_STATE_GOT_MSGID3,
MAVLINK_PARSE_STATE_GOT_CRC1,
MAVLINK_PARSE_STATE_GOT_CRC2,
MAVLINK_PARSE_STATE_GOT_PAYLOAD,
MAVLINK_PARSE_STATE_GOT_SIGNATURE,
}
private MAV_states state = MAV_states.MAVLINK_PARSE_STATE_UNINIT;
public MyLinkStats stats;
private MyLinkPacket m;
public MyParser() {
this(false);
}
public MyParser(boolean ignoreRadioPacketStats) {
stats = new MyLinkStats(ignoreRadioPacketStats);
}
public MyLinkPacket mavlink_parse_char(int c) {
// force to 8 bits
c &= 0xFF;
switch (state) {
case MAVLINK_PARSE_STATE_UNINIT:
case MAVLINK_PARSE_STATE_IDLE:
// MAVLink 1 and 2
if (c == MyLinkPacket.MAVLINK_STX) {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_STX;
}
break;
case MAVLINK_PARSE_STATE_GOT_STX:
// MAVLink 1 and 2
m = new MyLinkPacket(c);
state = MAV_states.MAVLINK_PARSE_STATE_GOT_LENGTH;
break;
case MAVLINK_PARSE_STATE_GOT_LENGTH:
m.incompatFlags = c;
if (c != 0 && c != 1) {
state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
break;
}
state = MAV_states.MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS;
break;
case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS:
m.compatFlags = c;
state = MAV_states.MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
break;
case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS:
m.seq = c;
state = MAV_states.MAVLINK_PARSE_STATE_GOT_SEQ;
break;
case MAVLINK_PARSE_STATE_GOT_SEQ:
// back to MAVLink 1 and 2
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_DESTID;
break;
case MAVLINK_PARSE_STATE_GOT_DESTID:
m.compid = c;
state = MAV_states.MAVLINK_PARSE_STATE_GOT_SRCID;
break;
case MAVLINK_PARSE_STATE_GOT_SRCID:
m.compid = c;
state = MAV_states.MAVLINK_PARSE_STATE_GOT_COMPID;
break;
case MAVLINK_PARSE_STATE_GOT_COMPID:
// MAVLink 1 and 2
m.msgid = c;
state = MAV_states.MAVLINK_PARSE_STATE_GOT_MSGID1;
if (m.len > 0) {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_MSGID3;
} else {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;
}
break;
case MAVLINK_PARSE_STATE_GOT_MSGID1:
m.msgid |= c << 8;
state = MAV_states.MAVLINK_PARSE_STATE_GOT_MSGID2;
break;
case MAVLINK_PARSE_STATE_GOT_MSGID2:
m.msgid |= c << 16;
if (m.len > 0) {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_MSGID3;
} else {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;
}
break;
case MAVLINK_PARSE_STATE_GOT_MSGID3:
//将payload的数据加入
m.payload.add((byte) c);
if (m.payloadIsFilled()) {
state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;
}
break;
case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
boolean crcGen = m.generateCRC(m.payload.size());
// Check first checksum byte and verify the CRC was successfully generated (msg extra exists)
if (c != m.crc.getLSB() || !crcGen) {
state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
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()) {
state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
stats.crcError();
} else { // crc is good
// stats.newPacket(m);
state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
stats.crcError();
}
break;
case MAVLINK_PARSE_STATE_GOT_CRC2:
state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
stats.crcError();
break;
} // switch
return null;
}
}发送Cmd类
- SendMav是把CmdSysInfo拼装一个MyLinkPacket装成字节流的形式；
- MyLinkPacket编码成一个类MavLink协议帧：
public class MyLinkPacket implements Serializable {
private static final long serialVersionUID = 2095947771227815314L;
public static final int MAVLINK_STX = 0xFD; // 253
public static final int HEADER_LEN = 12;
public final int len;
public int seq;
public int sysid;
public int destId;
public int srcId;
public int compid;
public int msgid;
public MyLinkPayload payload;
public CRC crc;
public int incompatFlags;
public int compatFlags;
public MyLinkPacket(int payloadLength) {
len = payloadLength;
payload = new MyLinkPayload();
}
public boolean payloadIsFilled() {
return payload.size() >= len;
}
public boolean generateCRC(final int payloadSize) {
if (crc == null) {
crc = new CRC();
} else {
crc.start_checksum();
}
crc.update_checksum(payloadSize);
crc.update_checksum(incompatFlags);
crc.update_checksum(compatFlags);
crc.update_checksum(seq);
crc.update_checksum(sysid);
crc.update_checksum(destId);
crc.update_checksum(srcId);
crc.update_checksum(compid);
crc.update_checksum(msgid);
crc.update_checksum(msgid >>> 8);
crc.update_checksum(msgid >>> 16);
payload.resetIndex();
for (int i = 0; i < payloadSize; i++) {
crc.update_checksum(payload.getByte());
}
return crc.finish_checksum(msgid);
}
private int mavTrimPayload(final byte[] payload) {
int length = payload.length;
while (length > 1 && payload[length - 1] == 0) {
length--;
}
return length;
}
public byte[] encodePacket() {
final int bufLen;
final int payloadSize;
payloadSize = mavTrimPayload(payload.payload.array());
bufLen = HEADER_LEN + payloadSize + 2;
byte[] buffer = new byte[bufLen];
int i = 0;
buffer[i++] = (byte) MAVLINK_STX;
buffer[i++] = (byte) payloadSize;
buffer[i++] = (byte) incompatFlags;
buffer[i++] = (byte) compatFlags;
buffer[i++] = (byte) seq;
buffer[i++] = (byte) sysid;
buffer[i++] = (byte) destId;
buffer[i++] = (byte) srcId;
buffer[i++] = (byte) compid;
buffer[i++] = (byte) (msgid & 0XFF);
buffer[i++] = (byte) ((msgid >>> 8) & 0XFF);
buffer[i++] = (byte) ((msgid >>> 16) & 0XFF);
for (int j = 0; j < payloadSize; ++j) {
buffer[i++] = payload.payload.get(j);
}
generateCRC(payloadSize);
buffer[i++] = (byte) (crc.getLSB());
buffer[i++] = (byte) (crc.getMSB());
return buffer;
}
}自定义类MavLink1 Drone端C/C++代码分析：
1080
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
