Parser.java

/* 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;

import com.MAVLink.MAVLinkPacket;
import com.MAVLink.Messages.MAVLinkStats;

/**
 * MAVLink parser that parses @{link MAVLinkPacket}s from a byte stream one byte
 * at a time. 
 *
 * After creating an instance of this class, simply use the @{link #mavlink_parse_char} 
 * method to parse a byte stream.
 */
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));
    }

    /**
     * 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_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;
    }

    /**
     * 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
     * @return the complete @{link MAVLinkPacket} if successfully decoded, else null
     */
    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;
    }
}