AP_Frsky_Telem: rework SPort-to-MAVlite conversion

Breaks the single object into three distinct parts objects, and hidesas
much information from the other parts as possible.

libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.cpp

@@ -1,323 +0,0 @@
-#include <AP_HAL/AP_HAL.h>
-#include <AP_Math/AP_Math.h>
-
-#include "AP_Frsky_MAVlite.h"
-
-extern const AP_HAL::HAL& hal;
-
-void AP_Frsky_MAVlite::update_checksum(mavlite_message_t &msg, const uint8_t c)
-{
-    msg.checksum += c; //0-1FF
-    msg.checksum += msg.checksum >> 8;
-    msg.checksum &= 0xFF;
-}
-
-void AP_Frsky_MAVlite::parser_reset(void)
-{
-    _rxmsg.checksum = 0;
-    _rxmsg.len = 0;
-    _rxmsg.msgid = 0;
-
-    _rxstatus.current_rx_seq = 0;
-    _rxstatus.payload_next_byte = 0;
-    _rxstatus.parse_state = ParseState::IDLE;
-}
-
-void AP_Frsky_MAVlite::encoder_reset(mavlite_message_t &txmsg)
-{
-    txmsg.checksum = 0;
-    
-    _txstatus.current_rx_seq = 0;
-    _txstatus.payload_next_byte = 0;
-    _txstatus.parse_state = ParseState::IDLE;
-}
-
-/*
- Parses sport packets and if successfull fills the rxmsg mavlite struct
- */
-bool AP_Frsky_MAVlite::parse(mavlite_message_t &rxmsg, const AP_Frsky_SPort::sport_packet_t packet)
-{
-    for (uint8_t i=0; i<6; i++) {
-        parse(packet.raw[i+2], i);
-    }
-    if (_rxstatus.parse_state == ParseState::MESSAGE_RECEIVED) {
-        rxmsg = _rxmsg;
-        return true;
-    }
-    return false;
-}
-
-/*
- Warning:
- make sure that all packets pushed by this method are sequential and not interleaved by packets inserted by another thread!
- */
-bool AP_Frsky_MAVlite::encode(ObjectBuffer_TS<AP_Frsky_SPort::sport_packet_t> &queue, mavlite_message_t &msg)
-{
-    // let's check if there's enough room to send it
-    if (queue.space() < MAVLITE_MSG_SPORT_PACKETS_COUNT(msg.len)) {
-        return false;
-    }
-    encoder_reset(msg);
-    // prevent looping forever
-    uint8_t packet_count = 0;
-    while (_txstatus.parse_state != ParseState::MESSAGE_RECEIVED && packet_count++ < MAVLITE_MSG_SPORT_PACKETS_COUNT(MAVLITE_MAX_PAYLOAD_LEN)) {
-
-        AP_Frsky_SPort::sport_packet_t packet {0};
-
-        for (uint8_t i=0; i<6; i++) {
-            // read msg and fill packet one byte at the time, ignore sensor and frame byte
-            encode(packet.raw[i+2], i, msg);
-        }
-
-        if (_txstatus.parse_state == ParseState::ERROR) {
-            break;
-        }
-
-        queue.push(packet);
-    }
-    _txstatus.parse_state = ParseState::IDLE;
-    return true;
-}
-
-bool AP_Frsky_MAVlite::parse(uint8_t byte, uint8_t offset)
-{
-    switch (_rxstatus.parse_state) {
-    case ParseState::IDLE:
-    case ParseState::ERROR:
-        if ( offset == 0 && byte == 0x00 ) {
-            parser_reset();
-            _rxstatus.parse_state = ParseState::GOT_START;
-        } else {
-            _rxstatus.parse_state = ParseState::ERROR;
-        }
-        break;
-    case ParseState::GOT_START:
-        if ( offset == 0 && byte == 0x00 ) {
-            parser_reset();
-            _rxstatus.parse_state = ParseState::GOT_START;
-        } else {
-            _rxmsg.len = byte;
-            _rxstatus.parse_state = ParseState::GOT_LEN;
-            update_checksum(_rxmsg, byte);
-        }
-        break;
-    case ParseState::GOT_LEN:
-        if ( offset == 0 && byte == 0x00 ) {
-            parser_reset();
-            _rxstatus.parse_state = ParseState::GOT_START;
-        } else {
-            _rxmsg.msgid = byte;
-            _rxstatus.parse_state = ParseState::GOT_MSGID;
-            update_checksum(_rxmsg, byte);
-        }
-        break;
-    case ParseState::GOT_MSGID:
-        if ( offset == 0 && byte == 0x00 ) {
-            parser_reset();
-            _rxstatus.parse_state = ParseState::GOT_START;
-        } else {
-            _rxmsg.payload[_rxstatus.payload_next_byte++] = byte;
-            _rxstatus.parse_state = ParseState::GOT_PAYLOAD;
-            update_checksum(_rxmsg, byte);
-        }
-        break;
-    case ParseState::GOT_SEQ:
-        if ( offset == 0 && byte == 0x00 ) {
-            parser_reset();
-            _rxstatus.parse_state = ParseState::GOT_START;
-        } else {
-            if ( _rxstatus.payload_next_byte < _rxmsg.len ) {
-                _rxmsg.payload[_rxstatus.payload_next_byte++] = byte;
-                _rxstatus.parse_state = ParseState::GOT_PAYLOAD;
-                update_checksum(_rxmsg, byte);
-            } else {
-                if ( _rxmsg.checksum == byte ) {
-                    _rxstatus.parse_state = ParseState::MESSAGE_RECEIVED;
-                    return true;
-                } else {
-                    _rxstatus.parse_state = ParseState::ERROR;
-                }
-            }
-        }
-        break;
-    case ParseState::GOT_PAYLOAD:
-        if ( offset == 0) {
-            if ( byte == 0x00 ) {
-                parser_reset();
-                _rxstatus.parse_state = ParseState::GOT_START;
-            } else {
-                if ((byte & 0x3F) != _rxstatus.current_rx_seq + 1) {
-                    _rxstatus.parse_state = ParseState::ERROR;
-                } else {
-                    _rxstatus.current_rx_seq = (byte & 0x3F);
-                    _rxstatus.parse_state = ParseState::GOT_SEQ;
-                }
-                update_checksum(_rxmsg, byte);
-            }
-        } else {
-            if ( _rxstatus.payload_next_byte < _rxmsg.len ) {
-                _rxmsg.payload[_rxstatus.payload_next_byte++] = byte;
-                update_checksum(_rxmsg, byte);
-            } else {
-                if ( _rxmsg.checksum == byte ) {
-                    _rxstatus.parse_state = ParseState::MESSAGE_RECEIVED;
-                    return true;
-                } else {
-                    _rxstatus.parse_state = ParseState::ERROR;
-                }
-            }
-        }
-        break;
-    case ParseState::MESSAGE_RECEIVED:
-        if ( offset == 0 && byte == 0x00 ) {
-            parser_reset();
-            _rxstatus.parse_state = ParseState::GOT_START;
-        }
-        break;
-    }
-    return false;
-}
-
-bool AP_Frsky_MAVlite::encode(uint8_t &byte, const uint8_t offset, mavlite_message_t &txmsg)
-{
-    switch (_txstatus.parse_state) {
-    case ParseState::IDLE:
-    case ParseState::ERROR:
-        if ( offset == 0 ) {
-            byte = 0x00;
-            encoder_reset(txmsg);
-            _txstatus.parse_state = ParseState::GOT_START;
-        } else {
-            _txstatus.parse_state = ParseState::ERROR;
-        }
-        break;
-    case ParseState::GOT_START:
-        byte = txmsg.len;
-        _txstatus.parse_state = ParseState::GOT_LEN;
-        update_checksum(txmsg, byte);
-        break;
-    case ParseState::GOT_LEN:
-        byte = txmsg.msgid;
-        _txstatus.parse_state = ParseState::GOT_MSGID;
-        update_checksum(txmsg, byte);
-        break;
-    case ParseState::GOT_MSGID:
-        byte = txmsg.payload[_txstatus.payload_next_byte++];
-        _txstatus.parse_state = ParseState::GOT_PAYLOAD;
-        update_checksum(txmsg, byte);
-        break;
-    case ParseState::GOT_SEQ:
-        if ( _txstatus.payload_next_byte < txmsg.len ) {
-            byte = txmsg.payload[_txstatus.payload_next_byte++];
-            _txstatus.parse_state = ParseState::GOT_PAYLOAD;
-            update_checksum(txmsg, byte);
-        } else {
-            byte = txmsg.checksum;
-            _txstatus.parse_state = ParseState::MESSAGE_RECEIVED;
-            return true;
-        }
-        break;
-    case ParseState::GOT_PAYLOAD:
-        if ( offset == 0) {
-            byte = ++_txstatus.current_rx_seq;
-            update_checksum(txmsg, byte);
-            _txstatus.parse_state = ParseState::GOT_SEQ;
-        } else {
-            if ( _txstatus.payload_next_byte < txmsg.len ) {
-                byte = txmsg.payload[_txstatus.payload_next_byte++];
-                update_checksum(txmsg, byte);
-            } else {
-                byte = (uint8_t)txmsg.checksum;
-                _txstatus.parse_state = ParseState::MESSAGE_RECEIVED;
-                return true;
-            }
-        }
-        break;
-    case ParseState::MESSAGE_RECEIVED:
-        break;
-    }
-    return false;
-}
-
-bool AP_Frsky_MAVlite::mavlite_msg_get_bytes(uint8_t *bytes, const mavlite_message_t &msg, const uint8_t offset, const uint8_t count)
-{
-    if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) {
-        return false;
-    }
-    memcpy(bytes, &msg.payload[offset], count);
-    return true;
-}
-
-bool AP_Frsky_MAVlite::mavlite_msg_set_bytes(mavlite_message_t &msg, const uint8_t *bytes, const uint8_t offset, const uint8_t count)
-{
-    if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) {
-        return false;
-    }
-    memcpy(&msg.payload[offset], bytes, count);
-    msg.len += count;
-    return true;
-}
-
-
-bool AP_Frsky_MAVlite::mavlite_msg_get_float(float &value, const mavlite_message_t &msg, const uint8_t offset)
-{
-    return mavlite_msg_get_bytes((uint8_t*)&value, msg, offset, 4);
-}
-
-bool AP_Frsky_MAVlite::mavlite_msg_get_uint16(uint16_t &value, const mavlite_message_t &msg, const uint8_t offset)
-{
-    return mavlite_msg_get_bytes((uint8_t*)&value, msg, offset, 2);
-}
-
-bool AP_Frsky_MAVlite::mavlite_msg_get_uint8(uint8_t &value, const mavlite_message_t &msg, const uint8_t offset)
-{
-    return mavlite_msg_get_bytes((uint8_t*)&value, msg, offset, 1);
-}
-
-uint8_t AP_Frsky_MAVlite::bit8_unpack(const uint8_t value, const uint8_t bit_count, const uint8_t bit_offset)
-{
-    uint8_t mask = 0;
-    for (uint8_t i=bit_offset; i<=bit_count; i++) {
-        mask |= 1 << i;
-    }
-    return (value & mask) >> bit_offset;
-}
-
-void AP_Frsky_MAVlite::bit8_pack(uint8_t &value, const uint8_t bit_value, const uint8_t bit_count, const uint8_t bit_offset)
-{
-    uint8_t mask = 0;
-    for (uint8_t i=bit_offset; i<=bit_count; i++) {
-        mask |= 1 << i;
-    }
-    value |= (bit_value<<bit_offset) & mask;
-}
-
-bool AP_Frsky_MAVlite::mavlite_msg_get_string(char* value, const mavlite_message_t &msg, const uint8_t offset)
-{
-    if (mavlite_msg_get_bytes((uint8_t*)value, msg, offset, MIN((uint8_t)16, msg.len - offset))) {
-        value[MIN((uint8_t)16, msg.len - offset)] = 0x00; // terminator
-        return true;
-    }
-    return false;
-}
-
-bool AP_Frsky_MAVlite::mavlite_msg_set_float(mavlite_message_t &msg, const float value, const uint8_t offset)
-{
-    return mavlite_msg_set_bytes(msg, (uint8_t*)&value, offset, 4);
-}
-
-bool AP_Frsky_MAVlite::mavlite_msg_set_uint16(mavlite_message_t &msg, const uint16_t value, const uint8_t offset)
-{
-    return mavlite_msg_set_bytes(msg, (uint8_t*)&value, offset, 2);
-}
-
-bool AP_Frsky_MAVlite::mavlite_msg_set_uint8(mavlite_message_t &msg, const uint8_t value, const uint8_t offset)
-{
-    return mavlite_msg_set_bytes(msg, (uint8_t*)&value, offset, 1);
-}
-
-bool AP_Frsky_MAVlite::mavlite_msg_set_string(mavlite_message_t &msg, const char* value, const uint8_t offset)
-{
-    return mavlite_msg_set_bytes(msg, (uint8_t*)value, offset, MIN((uint8_t)16, strlen(value)));
-}
-

libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.h

@@ -1,74 +1,32 @@
 #pragma once
 
-#include <AP_HAL/utility/RingBuffer.h>
+/*
 
-#include "AP_Frsky_SPort.h"
+  Wire Protocol:
+
+  Several SPort packets make up a MAVlite message.
+
+  A maximum of six relevant data bytes are present in an SPort packet.
+
+  Each SPort packet starts with a sequence number, starting with zero.
+
+  If the sequence number is zero then the parser is reset.
+
+  The first sport packet contains len at offset 1.  It is the
+  *payload* length - does not include checksum, for example.  msgid is
+  at offset 2, then payload bytes.
+
+  Subsequent SPort packets contain a sequence number at offset zero,
+  followed by more payload bytes.
+
+  When sufficient payload bytes have been received (based on "len"), a
+  single checksum byte arrives.  Sometimes this checksum byte goes
+  into an SPort packet all on its own, sharing space only with the
+  sequence number.
+
+*/
 
-#include <stdint.h>
 
 #define MAVLITE_MAX_PAYLOAD_LEN                 31 // 7 float params + cmd_id + options
 #define MAVLITE_MSG_SPORT_PACKETS_COUNT(LEN)    static_cast<uint8_t>(1 + ceilf((LEN-2)/5.0f)) // number of sport packets required to transport a message with LEN payload
 #define SPORT_PACKET_QUEUE_LENGTH               static_cast<uint8_t>(30U*MAVLITE_MSG_SPORT_PACKETS_COUNT(MAVLITE_MAX_PAYLOAD_LEN))
-
-class AP_Frsky_MAVlite
-{
-public:
-    typedef struct {
-        uint8_t msgid = 0;                          // ID of message in payload
-        uint8_t len = 0;                            // Length of payload
-        uint8_t payload[MAVLITE_MAX_PAYLOAD_LEN];
-        int16_t checksum = 0;                       // sent at end of packet
-    } mavlite_message_t;
-
-    // public parsing methods
-    bool parse(mavlite_message_t &rxmsg, const AP_Frsky_SPort::sport_packet_t packet) ;
-    bool encode(ObjectBuffer_TS<AP_Frsky_SPort::sport_packet_t> &queue, mavlite_message_t &msg);
-
-    // public mavlite message helpers
-    static bool mavlite_msg_get_float(float &value, const mavlite_message_t &msg, const uint8_t offset);
-    static bool mavlite_msg_set_float(mavlite_message_t &msg, const float value, const uint8_t offset);
-
-    static bool mavlite_msg_get_string(char* value, const mavlite_message_t &msg, const uint8_t offset);
-    static bool mavlite_msg_set_string(mavlite_message_t &msg, const char* value, const uint8_t offset);
-
-    static bool mavlite_msg_get_uint16(uint16_t &value, const mavlite_message_t &msg, const uint8_t offset);
-    static bool mavlite_msg_set_uint16(mavlite_message_t &msg, const uint16_t value, const uint8_t offset);
-
-    static bool mavlite_msg_get_uint8(uint8_t &value, const mavlite_message_t &msg, const uint8_t offset);
-    static bool mavlite_msg_set_uint8(mavlite_message_t &msg, const uint8_t value, const uint8_t offset);
-
-    static void bit8_pack(uint8_t &value, const uint8_t bit_value, const uint8_t bit_count, const uint8_t bit_offset);
-    static uint8_t bit8_unpack(const uint8_t value, const  uint8_t bit_count, const uint8_t bit_offset);
-
-private:
-    enum class ParseState : uint8_t {
-        IDLE=0,
-        ERROR,
-        GOT_START,
-        GOT_LEN,
-        GOT_SEQ,
-        GOT_MSGID,
-        GOT_PAYLOAD,
-        MESSAGE_RECEIVED,
-    }; // state machine for mavlite messages
-
-    typedef struct {
-        ParseState parse_state = ParseState::IDLE;
-        uint8_t current_rx_seq = 0;
-        uint8_t payload_next_byte = 0;
-    } mavlite_status_t;
-
-    mavlite_status_t _txstatus;
-    mavlite_status_t _rxstatus;
-
-    mavlite_message_t _rxmsg;
-
-    static bool mavlite_msg_get_bytes(uint8_t *bytes, const mavlite_message_t &msg, const uint8_t offset, const uint8_t count);
-    static bool mavlite_msg_set_bytes(mavlite_message_t &msg, const uint8_t *bytes,  const uint8_t offset, const uint8_t count);
-
-    void encoder_reset(mavlite_message_t &txmsg);
-    void parser_reset();
-    bool parse(const uint8_t byte, const uint8_t offset);
-    bool encode(uint8_t &byte, uint8_t offset, mavlite_message_t &txmsg);
-    void update_checksum(mavlite_message_t &msg, const uint8_t c);
-};

libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_MAVliteToSPort.cpp

@@ -0,0 +1,98 @@
+#include "AP_Frsky_MAVlite_MAVliteToSPort.h"
+
+#include "AP_Frsky_MAVlite.h"
+
+#include "AP_Frsky_SPort.h"
+
+void AP_Frsky_MAVlite_MAVliteToSPort::reset()
+{
+    checksum = 0;
+
+    packet_offs = 2;  // someone else sets frame and sensorid
+    state = State::WANT_LEN;
+    next_seq = 0;
+
+    payload_count = 0;
+}
+
+void AP_Frsky_MAVlite_MAVliteToSPort::update_checksum(const uint8_t c)
+{
+    checksum += c; //0-1FF
+    checksum += checksum >> 8;
+    checksum &= 0xFF;
+}
+
+/*
+ Warning:
+ make sure that all packets pushed by this method are sequential and not interleaved by packets inserted by another thread!
+ */
+bool AP_Frsky_MAVlite_MAVliteToSPort::process(ObjectBuffer_TS<AP_Frsky_SPort::sport_packet_t> &queue, const AP_Frsky_MAVlite_Message &msg)
+{
+    // let's check if there's enough room to send it
+    if (queue.space() < MAVLITE_MSG_SPORT_PACKETS_COUNT(msg.len)) {
+        return false;
+    }
+    reset();
+
+    process_byte(msg.len, queue);
+    process_byte(msg.msgid, queue);
+
+    for (uint8_t i=0; i<msg.len; i++) {
+        process_byte(msg.payload[i], queue);
+    }
+
+    // byte isn't important in this call; checksum is used in
+    // process_byte in case we need to start a new packet just to hold
+    // it.
+    process_byte(0, queue);
+
+    return true;
+}
+
+void AP_Frsky_MAVlite_MAVliteToSPort::process_byte(const uint8_t b, ObjectBuffer_TS<AP_Frsky_SPort::sport_packet_t> &queue)
+{
+    if (packet_offs == 2) {
+        // start of a packet (since we skip setting sensorid and
+        // frame).  Emit a sequence number.
+        packet.raw[packet_offs++] = next_seq;
+        update_checksum(next_seq);
+        next_seq += 1;
+    }
+    switch (state) {
+    case State::WANT_LEN:
+        packet.raw[packet_offs++] = b;
+        update_checksum(b);
+        payload_len = b;
+        state = State::WANT_MSGID;
+        break;
+    case State::WANT_MSGID:
+        packet.raw[packet_offs++] = b;
+        update_checksum(b);
+        if (b == 0) {
+            state = State::WANT_CHECKSUM;
+        } else {
+            state = State::WANT_PAYLOAD;
+        }
+        break;
+    case State::WANT_PAYLOAD:
+        packet.raw[packet_offs++] = b;
+        update_checksum(b);
+        payload_count++;
+        if (payload_count >= payload_len) {
+            state = State::WANT_CHECKSUM;
+        }
+        break;
+    case State::WANT_CHECKSUM:
+        packet.raw[packet_offs++] = checksum;
+        queue.push(packet);
+        state = State::DONE;
+        break;
+    case State::DONE:
+        return;
+    }
+    if (packet_offs >= ARRAY_SIZE(packet.raw)) {
+        // it's cooked
+        queue.push(packet);
+        packet_offs = 2;  // skip setting sensorid and frame
+    }
+}

libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_MAVliteToSPort.h

@@ -0,0 +1,52 @@
+#pragma once
+
+#include "AP_Frsky_MAVlite_Message.h"
+#include "AP_Frsky_SPort.h"
+
+#include <AP_HAL/utility/RingBuffer.h>
+
+#include <stdint.h>
+
+/*
+ * An instance of this class encodes a MAVlite message into several
+ * SPort packets, and pushes them onto the supplied queue.
+ *
+ * process() will return false if there is insufficient room in the
+ * queue for all SPort packets.
+ *
+ * See AP_Frsky_MAVlite.h for a description of the encoding of a
+ * MAVlite message in SPort packets.
+ */
+
+class AP_Frsky_MAVlite_MAVliteToSPort {
+public:
+
+    // insert sport packets calculated from mavlite msg into queue
+    bool process(ObjectBuffer_TS<AP_Frsky_SPort::sport_packet_t> &queue,
+                 const AP_Frsky_MAVlite_Message &msg) WARN_IF_UNUSED;
+
+private:
+
+    enum class State : uint8_t {
+        WANT_LEN,
+        WANT_MSGID,
+        WANT_PAYLOAD,
+        WANT_CHECKSUM,
+        DONE,
+    };
+    State state = State::WANT_LEN;
+
+    void reset();
+
+    void process_byte(uint8_t byte, ObjectBuffer_TS<AP_Frsky_SPort::sport_packet_t> &queue);
+
+    AP_Frsky_SPort::sport_packet_t packet {};
+    uint8_t packet_offs = 0;
+
+    uint8_t next_seq = 0;
+    uint8_t payload_count = 0;
+    uint8_t payload_len;
+
+    int16_t checksum;                       // sent at end of packet
+    void update_checksum(const uint8_t c);
+};

libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_Message.cpp

@@ -0,0 +1,55 @@
+#include "AP_Frsky_MAVlite_Message.h"
+
+#include <AP_Math/AP_Math.h>
+
+bool AP_Frsky_MAVlite_Message::get_bytes(uint8_t *bytes, const uint8_t offset, const uint8_t count) const
+{
+    if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) {
+        return false;
+    }
+    memcpy(bytes, &payload[offset], count);
+    return true;
+}
+
+bool AP_Frsky_MAVlite_Message::set_bytes(const uint8_t *bytes, const uint8_t offset, const uint8_t count)
+{
+    if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) {
+        return false;
+    }
+    memcpy(&payload[offset], bytes, count);
+    len += count;
+    return true;
+}
+
+bool AP_Frsky_MAVlite_Message::get_string(char* value, const uint8_t offset) const
+{
+    if (get_bytes((uint8_t*)value, offset, MIN((uint8_t)16, len - offset))) {
+        value[MIN((uint8_t)16, len - offset)] = 0x00; // terminator
+        return true;
+    }
+    return false;
+}
+
+bool AP_Frsky_MAVlite_Message::set_string(const char* value, const uint8_t offset)
+{
+    return set_bytes((uint8_t*)value, offset, MIN((uint8_t)16, strlen(value)));
+}
+
+
+uint8_t AP_Frsky_MAVlite_Message::bit8_unpack(const uint8_t value, const uint8_t bit_count, const uint8_t bit_offset)
+{
+    uint8_t mask = 0;
+    for (uint8_t i=bit_offset; i<=bit_count; i++) {
+        mask |= 1 << i;
+    }
+    return (value & mask) >> bit_offset;
+}
+
+void AP_Frsky_MAVlite_Message::bit8_pack(uint8_t &value, const uint8_t bit_value, const uint8_t bit_count, const uint8_t bit_offset)
+{
+    uint8_t mask = 0;
+    for (uint8_t i=bit_offset; i<=bit_count; i++) {
+        mask |= 1 << i;
+    }
+    value |= (bit_value<<bit_offset) & mask;
+}

libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_Message.h

@@ -0,0 +1,46 @@
+#pragma once
+
+#include "AP_Frsky_MAVlite.h"
+
+#include <AP_Common/AP_Common.h>
+
+#include <stdint.h>
+
+class AP_Frsky_MAVlite_Message {
+public:
+    // helpers
+    bool get_float(float &value, const uint8_t offset) const WARN_IF_UNUSED {
+        return get_bytes((uint8_t*)&value, offset, 4);
+    }
+    bool set_float(const float value, const uint8_t offset) WARN_IF_UNUSED {
+        return set_bytes((uint8_t*)&value, offset, 4);
+    }
+
+    bool get_string(char* value, const uint8_t offset) const WARN_IF_UNUSED;
+    bool set_string(const char* value, const uint8_t offset) WARN_IF_UNUSED;
+
+    bool get_uint16(uint16_t &value, const uint8_t offset) const WARN_IF_UNUSED {
+        return get_bytes((uint8_t*)&value, offset, 2);
+    }
+    bool set_uint16(const uint16_t value, const uint8_t offset) WARN_IF_UNUSED {
+        return set_bytes((uint8_t*)&value, offset, 2);
+    }
+
+    bool get_uint8(uint8_t &value, const uint8_t offset) const WARN_IF_UNUSED {
+        return get_bytes((uint8_t*)&value, offset, 1);;
+    }
+    bool set_uint8(const uint8_t value, const uint8_t offset) WARN_IF_UNUSED {
+        return set_bytes((uint8_t*)&value, offset, 1);
+    }
+
+    uint8_t msgid = 0;                          // ID of message in payload
+    uint8_t len = 0;                            // Length of payload
+    uint8_t payload[MAVLITE_MAX_PAYLOAD_LEN];
+
+    static void bit8_pack(uint8_t &value, const uint8_t bit_value, const uint8_t bit_count, const uint8_t bit_offset);
+    static uint8_t bit8_unpack(const uint8_t value, const  uint8_t bit_count, const uint8_t bit_offset);
+
+private:
+    bool get_bytes(uint8_t *bytes, const uint8_t offset, const uint8_t count) const WARN_IF_UNUSED;
+    bool set_bytes(const uint8_t *bytes,  const uint8_t offset, const uint8_t count) WARN_IF_UNUSED;
+};

libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_SPortToMAVlite.cpp

@@ -0,0 +1,104 @@
+#include "AP_Frsky_MAVlite_SPortToMAVlite.h"
+
+#include "AP_Frsky_MAVlite.h"
+
+void AP_Frsky_MAVlite_SPortToMAVlite::reset(void)
+{
+    checksum = 0;
+
+    expected_seq = 0;
+    payload_next_byte = 0;
+    parse_state = State::WANT_LEN;
+}
+
+void AP_Frsky_MAVlite_SPortToMAVlite::update_checksum(const uint8_t c)
+{
+    checksum += c; //0-1FF
+    checksum += checksum >> 8;
+    checksum &= 0xFF;
+}
+
+/*
+ Parses sport packets and if successfull fills the rxmsg mavlite struct
+ */
+bool AP_Frsky_MAVlite_SPortToMAVlite::process(AP_Frsky_MAVlite_Message &rxmsg, const AP_Frsky_SPort::sport_packet_t &packet)
+{
+    // the two skipped bytes in packet.raw here are sensor and frame.
+    // appid and data are used to transport the mavlite message.
+
+    // deal with packet sequence number:
+    const uint8_t received_seq = (packet.raw[2] & 0x3F);
+    // if the first byte of any sport passthrough packet is zero then we reset:
+    if (received_seq == 0) {
+        reset();
+    }
+    if (received_seq != expected_seq) {
+        parse_state = State::ERROR;
+        return false;
+    }
+    update_checksum(received_seq);
+    expected_seq = received_seq + 1;
+
+    // deal with the remainder (post-sequence) of the packet:
+    for (uint8_t i=3; i<ARRAY_SIZE(packet.raw); i++) {
+        parse(packet.raw[i]);
+    }
+    if (parse_state == State::MESSAGE_RECEIVED) {
+        rxmsg = _rxmsg;
+        return true;
+    }
+    return false;
+}
+
+void AP_Frsky_MAVlite_SPortToMAVlite::parse(uint8_t byte)
+{
+    switch (parse_state) {
+
+    case State::IDLE:
+        // it is an error to receive anything but offset==0 byte=0xx0
+        // in this state
+        parse_state = State::ERROR;
+        return;
+
+    case State::ERROR:
+        // waiting for offset==0 && byte==0x00 to bump us into WANT_LEN
+        return;
+
+    case State::WANT_LEN:
+        _rxmsg.len = byte;
+        update_checksum(byte);
+        parse_state = State::WANT_MSGID;
+        return;
+
+    case State::WANT_MSGID:
+        _rxmsg.msgid = byte;
+        update_checksum(byte);
+        if (_rxmsg.len == 0) {
+            parse_state = State::WANT_CHECKSUM;
+        } else {
+            parse_state = State::WANT_PAYLOAD;
+        }
+        return;
+
+    case State::WANT_PAYLOAD:
+        // add byte to payload
+        _rxmsg.payload[payload_next_byte++] = byte;
+        update_checksum(byte);
+
+        if (payload_next_byte >= _rxmsg.len) {
+            parse_state = State::WANT_CHECKSUM;
+        }
+        return;
+
+    case State::WANT_CHECKSUM:
+        if (checksum != byte) {
+            parse_state = State::ERROR;
+            return;
+        }
+        parse_state = State::MESSAGE_RECEIVED;
+        return;
+
+    case State::MESSAGE_RECEIVED:
+        return;
+    }
+}

libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_SPortToMAVlite.h

@@ -0,0 +1,48 @@
+#pragma once
+
+#include "AP_Frsky_MAVlite_Message.h"
+#include "AP_Frsky_SPort.h"
+
+#include <stdint.h>
+
+/*
+ * An instance of this class decodes a stream of SPort packets into a
+ * MAVlite message (see AP_Frsky_MAVlite_Message.h).  It is expected
+ * that the same rxmsg is passed into process() multiple times, each
+ * time with a new sport packet.  If a packet is successfully decodes
+ * then process() will return true and rxmsg can be used as a MAVlite
+ * message.
+ *
+ * See AP_Frsky_MAVlite.h for a description of the encoding of a
+ * MAVlite message in SPort packets.
+ */
+class AP_Frsky_MAVlite_SPortToMAVlite {
+public:
+
+    bool process(AP_Frsky_MAVlite_Message &rxmsg,
+                 const AP_Frsky_SPort::sport_packet_t &packet) WARN_IF_UNUSED;
+
+private:
+
+    void reset();
+
+    uint8_t expected_seq;
+    uint8_t payload_next_byte;
+
+    enum class State : uint8_t {
+        IDLE=0,
+        ERROR,
+        WANT_LEN,
+        WANT_MSGID,
+        WANT_PAYLOAD,
+        WANT_CHECKSUM,
+        MESSAGE_RECEIVED,
+    };
+    State parse_state = State::IDLE;
+
+    AP_Frsky_MAVlite_Message _rxmsg;
+    void parse(const uint8_t byte);
+
+    int16_t checksum;                       // sent at end of packet
+    void update_checksum(const uint8_t c);
+};

libraries/AP_Frsky_Telem/AP_Frsky_SPort.h

@@ -33,6 +33,8 @@ protected:
 
     uint32_t calc_gps_latlng(bool &send_latitude);
 
+    static uint8_t calc_sensor_id(const uint8_t physical_id);
+
 private:
 
     struct {

libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp

@@ -674,9 +674,9 @@ void AP_Frsky_SPort_Passthrough::process_rx_queue()
     AP_Frsky_SPort::sport_packet_t packet;
     uint8_t loop_count = 0; // prevent looping forever
     while (_SPort_bidir.rx_packet_queue.pop(packet) && loop_count++ < MAVLITE_MSG_SPORT_PACKETS_COUNT(MAVLITE_MAX_PAYLOAD_LEN)) {
-        AP_Frsky_MAVlite::mavlite_message_t rxmsg;
+        AP_Frsky_MAVlite_Message rxmsg;
 
-        if (_mavlite_handler.parse(rxmsg, packet)) {
+        if (sport_to_mavlite.process(rxmsg, packet)) {
             mavlite_process_message(rxmsg);
             break; // process only 1 mavlite message each call
         }
@@ -711,19 +711,26 @@ void AP_Frsky_SPort_Passthrough::process_tx_queue()
  * Handle the COMMAND_LONG mavlite message
  * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
  */
-void AP_Frsky_SPort_Passthrough::mavlite_handle_command_long(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg)
+void AP_Frsky_SPort_Passthrough::mavlite_handle_command_long(const AP_Frsky_MAVlite_Message &rxmsg)
 {
     mavlink_command_long_t mav_command_long {};
 
     uint8_t cmd_options;
     float params[7] {};
 
-    AP_Frsky_MAVlite::mavlite_msg_get_uint16(mav_command_long.command, rxmsg, 0);
+    if (!rxmsg.get_uint16(mav_command_long.command, 0)) {
-    AP_Frsky_MAVlite::mavlite_msg_get_uint8(cmd_options, rxmsg, 2);
+        return;
-    uint8_t param_count = AP_Frsky_MAVlite::bit8_unpack(cmd_options, 3, 0);               // first 3 bits
+    }
+    if (!rxmsg.get_uint8(cmd_options, 2)) {
+        return;
+    }
+    uint8_t param_count = AP_Frsky_MAVlite_Message::bit8_unpack(cmd_options, 3, 0);               // first 3 bits
 
     for (uint8_t cmd_idx=0; cmd_idx<param_count; cmd_idx++) {
-        AP_Frsky_MAVlite::mavlite_msg_get_float(params[cmd_idx], rxmsg, 3+(4*cmd_idx));   // base offset is 3, relative offset is 4*cmd_idx
+        // base offset is 3, relative offset is 4*cmd_idx
+        if (!rxmsg.get_float(params[cmd_idx], 3+(4*cmd_idx))) {
+            return;
+        }
     }
 
     mav_command_long.param1 = params[0];
@@ -796,10 +803,14 @@ void AP_Frsky_SPort_Passthrough::mavlite_handle_command_long(const AP_Frsky_MAVl
 
 void AP_Frsky_SPort_Passthrough::mavlite_send_command_ack(const MAV_RESULT mav_result, const uint16_t cmdid)
 {
-    AP_Frsky_MAVlite::mavlite_message_t txmsg;
+    AP_Frsky_MAVlite_Message txmsg;
     txmsg.msgid = MAVLINK_MSG_ID_COMMAND_ACK;
-    AP_Frsky_MAVlite::mavlite_msg_set_uint16(txmsg, cmdid, 0);
+    if (!txmsg.set_uint16(cmdid, 0)) {
-    AP_Frsky_MAVlite::mavlite_msg_set_uint8(txmsg, (uint8_t)mav_result, 2);
+        return;
+    }
+    if (!txmsg.set_uint8((uint8_t)mav_result, 2)) {
+        return;
+    }
     mavlite_send_message(txmsg);
 }
 
@@ -844,20 +855,26 @@ MAV_RESULT AP_Frsky_SPort_Passthrough::mavlite_handle_command_do_fence_enable(ui
  * Handle the PARAM_REQUEST_READ mavlite message
  * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
  */
-void AP_Frsky_SPort_Passthrough::mavlite_handle_param_request_read(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg)
+void AP_Frsky_SPort_Passthrough::mavlite_handle_param_request_read(const AP_Frsky_MAVlite_Message &rxmsg)
 {
     float param_value;
     char param_name[AP_MAX_NAME_SIZE+1];
-    AP_Frsky_MAVlite::mavlite_msg_get_string(param_name, rxmsg, 0);
+    if (!rxmsg.get_string(param_name, 0)) {
+        return;
+    }
     // find existing param
     if (!AP_Param::get(param_name,param_value)) {
         gcs().send_text(MAV_SEVERITY_WARNING, "Param read failed (%s)", param_name);
         return;
     }
-    AP_Frsky_MAVlite::mavlite_message_t txmsg;
+    AP_Frsky_MAVlite_Message txmsg;
     txmsg.msgid = MAVLINK_MSG_ID_PARAM_VALUE;
-    AP_Frsky_MAVlite::mavlite_msg_set_float(txmsg, param_value, 0);
+    if (!txmsg.set_float(param_value, 0)) {
-    AP_Frsky_MAVlite::mavlite_msg_set_string(txmsg, param_name, 4);
+        return;
+    }
+    if (!txmsg.set_string(param_name, 4)) {
+        return;
+    }
     mavlite_send_message(txmsg);
 }
 
@@ -865,13 +882,17 @@ void AP_Frsky_SPort_Passthrough::mavlite_handle_param_request_read(const AP_Frsk
  * Handle the PARAM_SET mavlite message
  * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
  */
-void AP_Frsky_SPort_Passthrough::mavlite_handle_param_set(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg)
+void AP_Frsky_SPort_Passthrough::mavlite_handle_param_set(const AP_Frsky_MAVlite_Message &rxmsg)
 {
     float param_value;
     char param_name[AP_MAX_NAME_SIZE+1];
     // populate packet with mavlite payload
-    AP_Frsky_MAVlite::mavlite_msg_get_float(param_value, rxmsg, 0);
+    if (!rxmsg.get_float(param_value, 0)) {
-    AP_Frsky_MAVlite::mavlite_msg_get_string(param_name, rxmsg, 4);
+        return;
+    }
+    if (!rxmsg.get_string(param_name, 4)) {
+        return;
+    }
     // find existing param so we can get the old value
     enum ap_var_type var_type;
     // set parameter
@@ -891,10 +912,14 @@ void AP_Frsky_SPort_Passthrough::mavlite_handle_param_set(const AP_Frsky_MAVlite
         gcs().send_text(MAV_SEVERITY_WARNING, "Param read failed (%s)", param_name);
         return;
     }
-    AP_Frsky_MAVlite::mavlite_message_t txmsg;
+    AP_Frsky_MAVlite_Message txmsg;
     txmsg.msgid = MAVLINK_MSG_ID_PARAM_VALUE;
-    AP_Frsky_MAVlite::mavlite_msg_set_float(txmsg, param_value, 0);
+    if (!txmsg.set_float(param_value, 0)) {
-    AP_Frsky_MAVlite::mavlite_msg_set_string(txmsg, param_name, 4);
+        return;
+    }
+    if (!txmsg.set_string(param_name, 4)) {
+        return;
+    }
     mavlite_send_message(txmsg);
 }
 
@@ -945,7 +970,7 @@ MAV_RESULT AP_Frsky_SPort_Passthrough::mavlite_handle_command_preflight_reboot(v
  * Process an incoming mavlite message
  * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
  */
-void AP_Frsky_SPort_Passthrough::mavlite_process_message(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg)
+void AP_Frsky_SPort_Passthrough::mavlite_process_message(const AP_Frsky_MAVlite_Message &rxmsg)
 {
     switch (rxmsg.msgid) {
         case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
@@ -965,9 +990,9 @@ void AP_Frsky_SPort_Passthrough::mavlite_process_message(const AP_Frsky_MAVlite:
  * Message is chunked in sport packets pushed in the tx queue
  * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
  */
-bool AP_Frsky_SPort_Passthrough::mavlite_send_message(AP_Frsky_MAVlite::mavlite_message_t &txmsg)
+bool AP_Frsky_SPort_Passthrough::mavlite_send_message(AP_Frsky_MAVlite_Message &txmsg)
 {
-    return _mavlite_handler.encode(_SPort_bidir.tx_packet_queue, txmsg);
+    return mavlite_to_sport.process(_SPort_bidir.tx_packet_queue, txmsg);
 }
 
 

libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h

@@ -9,6 +9,8 @@
 #include "AP_Frsky_Telem.h"
 
 #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+#include "AP_Frsky_MAVlite_SPortToMAVlite.h"
+#include "AP_Frsky_MAVlite_MAVliteToSPort.h"
 #define FRSKY_WFQ_TIME_SLOT_MAX     12U
 #define SPORT_TX_PACKET_DUPLICATES          1   // number of duplicates packets we send (fport only)
 #else
@@ -116,7 +118,8 @@ private:
     } _SPort_bidir;
 
     AP_Frsky_SPortParser _sport_handler;
-    AP_Frsky_MAVlite _mavlite_handler;
+    AP_Frsky_MAVlite_SPortToMAVlite sport_to_mavlite;
+    AP_Frsky_MAVlite_MAVliteToSPort mavlite_to_sport;
 
     void set_sensor_id(AP_Int8 idx, uint8_t &sensor);
     // tx/rx sport packet processing
@@ -125,13 +128,13 @@ private:
     void process_tx_queue(void);
 
     // mavlite messages tx/rx methods
-    bool mavlite_send_message(AP_Frsky_MAVlite::mavlite_message_t &txmsg);
+    bool mavlite_send_message(AP_Frsky_MAVlite_Message &txmsg);
-    void mavlite_process_message(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg);
+    void mavlite_process_message(const AP_Frsky_MAVlite_Message &rxmsg);
 
     // gcs mavlite methods
-    void mavlite_handle_param_request_read(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg);
+    void mavlite_handle_param_request_read(const AP_Frsky_MAVlite_Message &rxmsg);
-    void mavlite_handle_param_set(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg);
+    void mavlite_handle_param_set(const AP_Frsky_MAVlite_Message &rxmsg);
-    void mavlite_handle_command_long(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg);
+    void mavlite_handle_command_long(const AP_Frsky_MAVlite_Message &rxmsg);
     void mavlite_send_command_ack(const MAV_RESULT mav_result, const uint16_t cmdid);
     MAV_RESULT mavlite_handle_command_preflight_calibration_baro();
     MAV_RESULT mavlite_handle_command_do_fence_enable(uint16_t param1);