Tower实战2:：MavlinkMessage消息解读

最新推荐文章于 2025-06-24 11:14:17 发布

weixin_30244681

最新推荐文章于 2025-06-24 11:14:17 发布

阅读量293

点赞数

CC 4.0 BY-SA版权

原文链接：http://www.cnblogs.com/seanzhoutao/p/6101557.html

本文详细介绍了MAVLink消息的格式及其字段含义，并通过代码示例展示了如何将字节数组转换为MAVLink消息对象的过程。此外，还介绍了MAVLinkMessage类的基本结构和常用方法。

1如图所示，这个一个MavlinkMessage的具体消息格式，至于每个字段什么意思可以查阅其官方文档，或者相关质量

红色的是起始标志位（stx），在v1.0版本中以“FE”作为起始标志。这个标志位在mavlink消息帧接收端进行消息解码时有用处。
第二个格子代表的是灰色部分（payload，称作有效载荷，要用的数据在有效载荷里面）的字节长度（len），范围从0到255之间。在mavlink消息帧接收端可以用它和实际收到的有效载荷的长度比较，以验证有效载荷的长度是否正确。
第三个格子代表的是本次消息帧的序号（seq），每次发完一个消息，这个字节的内容会加1，加到255后会从0重新开始。这个序号用于mavlink消息帧接收端计算消息丢失比例用的，相当于是信号强度。
第四个格子代表了发送本条消息帧的设备的系统编号（sys），使用PIXHAWK刷PX4固件时默认的系统编号为1，用于mavlink消息帧接收端识别是哪个设备发来的消息。
第五个格子代表了发送本条消息帧的设备的单元编号（comp），使用PIXHAWK刷PX4固件时默认的单元编号为50，用于mavlink消息帧接收端识别是设备的哪个单元发来的消息（暂时没什么用）。
第六个格子代表了有效载荷中消息包的编号（msg），注意它和序号是不同的，这个字节很重要，mavlink消息帧接收端要根据这个编号来确定有效载荷里到底放了什么消息包并根据编号选择对应的方式来处理有效载荷里的信息包。
最后两个字节是16位校验位。

2 以上部分文档是参照别人的文档来的，也是简要说明每个字段的意思，是不是有点tcp-ip的影子；废话不多说.在Tower中定义了一个所有了的基类 MAVLinkMessage

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 trought the Service-Acctivity interface
    
    /**
     *  Simply a common interface for all MAVLink Messages
     */
    
    public  int sysid;
    public int compid;
    public int msgid;
    public abstract MAVLinkPacket pack();
    public abstract void unpack(MAVLinkPayload payload);
}

然后让每一个消息类去继承这个类，比如我们使用 msg_heartbeat ，这个一个维护心跳的message

public class msg_heartbeat extends MAVLinkMessage {
    public static final int MAVLINK_MSG_ID_HEARTBEAT = 0;
    public static final int MAVLINK_MSG_LENGTH = 9;
    private static final long serialVersionUID = MAVLINK_MSG_ID_HEARTBEAT;
}

4 如果你忽略底层传输的流，你可以把通信理解为如下图

就是你给飞控发了一个消息，飞控收到以后在处理，在给你发一个消息

5 在MavlinkMSG里面有一个抽象方放 public abstract MAVLinkPacket pack();

 public MAVLinkPacket pack() {
        MAVLinkPacket packet = new MAVLinkPacket();
        packet.len = MAVLINK_MSG_LENGTH;
        packet.sysid = 255;
        packet.compid = 190;
        packet.msgid = MAVLINK_MSG_ID_HEARTBEAT;
        packet.payload.putInt(custom_mode);
        packet.payload.putByte(type);
        packet.payload.putByte(autopilot);
        packet.payload.putByte(base_mode);
        packet.payload.putByte(system_status);
        packet.payload.putByte(mavlink_version);

        return packet;
    }

 public void unpack(MAVLinkPayload payload) {
        payload.resetIndex();
        this.custom_mode = payload.getInt();
        this.type = payload.getByte();
        this.autopilot = payload.getByte();
        this.base_mode = payload.getByte();
        this.system_status = payload.getByte();
        this.mavlink_version = payload.getByte();

    }

这2个方法是解包和封包的方法，至于如何转换成bytestream，你可以不必去关注，剩下的就是你拿到这个message之后的事情，下一篇估计会继续写如果通过message来实现飞机的起飞。和一下具体常用的msg的具体定义，和收到msg之后是如果回调的

6 那接下来我来看怎么把一个byte[] 转换成 message的

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;

    static 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;
                m = new MAVLinkPacket();
            }
            break;

        case MAVLINK_PARSE_STATE_GOT_STX:
            if (msg_received) {
                msg_received = false;
                state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
            } else {
                m.len = 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;
        }
    }

}

当收到一个254的时候，把他作为开始，然后一位一位的接受，到len== pload.size() 的时候结束，最后在crc 校验，如果不波被改变了，这个会有一个溢出报错。解决办法私信我；

在完成的时候会得到一个msg；

  for (int i = 0; i < bufferSize; i++) {
                MAVLinkPacket receivedPacket = parser.mavlink_parse_char(buffer[i] & 0x00ff);
                if (receivedPacket != null) {
                    MAVLinkMessage msg = receivedPacket.unpack();
                    reportReceivedMessage(msg);
                    queueToLog(receivedPacket);
                }
            }

这样又到了MavlinkMessageHandle里面了

转载于:https://www.cnblogs.com/seanzhoutao/p/6101557.html