AP_Frsky_Telem: added support for frsky sport/fport bidirectional telemetry

libraries/AP_Frsky_Telem/AP_Frsky_Backend.h

@@ -26,6 +26,10 @@ public:
     {
         return false;
     }
+    virtual bool set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data)
+    {
+        return false;
+    }
 
     virtual void queue_text_message(MAV_SEVERITY severity, const char *text) { }
 

libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.cpp

@@ -0,0 +1,323 @@
+#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

@@ -0,0 +1,74 @@
+#pragma once
+
+#include <AP_HAL/utility/RingBuffer.h>
+
+#include "AP_Frsky_SPort.h"
+
+#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_Parameters.cpp

@@ -0,0 +1,49 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+#include "AP_Frsky_Parameters.h"
+
+#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+
+const AP_Param::GroupInfo AP_Frsky_Parameters::var_info[] = {
+    // @Param: UPLINK_ID
+    // @DisplayName: Uplink sensor id
+    // @Description: Change the uplink sensor id (SPort only)
+    // @Values: -1:Disable,7:7,8:8,9:9,10:10,11:11,12:12,13:13,14:14,15:15,16:16,17:17,18:18,19:19,20:20,21:21,22:22,23:23,24:24,25:25,26:26
+    // @User: Advanced
+    AP_GROUPINFO("UPLINK_ID",  1, AP_Frsky_Parameters, _uplink_id, 13),
+
+    // @Param: DNLINK1_ID
+    // @DisplayName: First downlink sensor id
+    // @Description: Change the first extra downlink sensor id (SPort only)
+    // @Values: -1:Disable,7:7,8:8,9:9,10:10,11:11,12:12,13:13,14:14,15:15,16:16,17:17,18:18,19:19,20:20,21:21,22:22,23:23,24:24,25:25,26:26
+    // @User: Advanced
+    AP_GROUPINFO("DNLINK1_ID",  2, AP_Frsky_Parameters, _dnlink1_id, 20),
+
+    // @Param: DNLINK2_ID
+    // @DisplayName: Second downlink sensor id
+    // @Description: Change the second extra downlink sensor id (SPort only)
+    // @Values: -1:Disable,7:7,8:8,9:9,10:10,11:11,12:12,13:13,14:14,15:15,16:16,17:17,18:18,19:19,20:20,21:21,22:22,23:23,24:24,25:25,26:26
+    // @User: Advanced
+    AP_GROUPINFO("DNLINK2_ID",  3, AP_Frsky_Parameters, _dnlink2_id, 7),
+
+    AP_GROUPEND
+};
+
+AP_Frsky_Parameters::AP_Frsky_Parameters()
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL

libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h

@@ -0,0 +1,41 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+#pragma once
+
+#include "AP_Frsky_Telem.h"
+
+#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Param/AP_Param.h>
+
+class AP_Frsky_Telem;
+
+class AP_Frsky_Parameters
+{
+    friend class AP_Frsky_SPort_Passthrough;
+public:
+    AP_Frsky_Parameters();
+
+    // parameters
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+    // settable parameters
+    AP_Int8 _uplink_id;
+    AP_Int8 _dnlink1_id;
+    AP_Int8 _dnlink2_id;
+};
+
+#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL

libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp

@@ -1,9 +1,14 @@
 #include "AP_Frsky_SPort.h"
 
+#include <AP_HAL/utility/sparse-endian.h>
 #include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_GPS/AP_GPS.h>
 #include <GCS_MAVLink/GCS.h>
 
+#include "AP_Frsky_SPortParser.h"
+
+#include <string.h>
+
 /*
  * send telemetry data
  * for FrSky SPort protocol (X-receivers)
@@ -204,3 +209,137 @@ void  AP_Frsky_SPort::send_sport_frame(uint8_t frame, uint16_t appid, uint32_t d
 #endif
     _port->write(buf2, len);
 }
+
+extern const AP_HAL::HAL& hal;
+
+bool AP_Frsky_SPortParser::should_process_packet(const uint8_t *packet, bool discard_duplicates)
+{
+    // check for duplicate packets
+    if (discard_duplicates && _parse_state.last_packet != nullptr) {
+        /*
+          Note: the polling byte packet[0] should be ignored in the comparison
+          because we might get the same packet with different polling bytes
+          We have 2 types of duplicate packets: ghost identical packets sent by the receiver
+          and user duplicate packets sent to compensate for bad link and frame loss, this
+          check should address both types.
+        */
+        if (memcmp(&packet[1], &_parse_state.last_packet[1], SPORT_PACKET_SIZE-1) == 0) {
+            return false;
+        }
+        memcpy(_parse_state.last_packet, packet, SPORT_PACKET_SIZE);
+    }
+    //check CRC
+    int16_t crc = 0;
+    for (uint8_t i=1; i<SPORT_PACKET_SIZE; ++i) {
+        crc += _parse_state.rx_buffer[i]; // 0-1FE
+        crc += crc >> 8;  // 0-1FF
+        crc &= 0x00ff;    // 0-FF
+    }
+    return (crc == 0x00ff);
+}
+
+bool AP_Frsky_SPortParser::process_byte(AP_Frsky_SPort::sport_packet_t &sport_packet, const uint8_t data)
+{
+    switch (_parse_state.state) {
+    case ParseState::START:
+        if (_parse_state.rx_buffer_count < TELEMETRY_RX_BUFFER_SIZE) {
+            _parse_state.rx_buffer[_parse_state.rx_buffer_count++] = data;
+        }
+        _parse_state.state = ParseState::IN_FRAME;
+        break;
+
+    case ParseState::IN_FRAME:
+        if (data == FRAME_DLE) {
+            _parse_state.state = ParseState::XOR; // XOR next byte
+        } else if (data == FRAME_HEAD) {
+            _parse_state.state = ParseState::IN_FRAME ;
+            _parse_state.rx_buffer_count = 0;
+            break;
+        } else if (_parse_state.rx_buffer_count < TELEMETRY_RX_BUFFER_SIZE) {
+            _parse_state.rx_buffer[_parse_state.rx_buffer_count++] = data;
+        }
+        break;
+
+    case ParseState::XOR:
+        if (_parse_state.rx_buffer_count < TELEMETRY_RX_BUFFER_SIZE) {
+            _parse_state.rx_buffer[_parse_state.rx_buffer_count++] = data ^ STUFF_MASK;
+        }
+        _parse_state.state = ParseState::IN_FRAME;
+        break;
+
+    case ParseState::IDLE:
+        if (data == FRAME_HEAD) {
+            _parse_state.rx_buffer_count = 0;
+            _parse_state.state = ParseState::START;
+        }
+        break;
+
+    } // switch
+
+    if (_parse_state.rx_buffer_count >= SPORT_PACKET_SIZE) {
+        _parse_state.rx_buffer_count = 0;
+        _parse_state.state = ParseState::IDLE;
+        // feed the packet only if it's not a duplicate
+        return get_packet(sport_packet, true);
+    }
+    return false;
+}
+
+bool AP_Frsky_SPortParser::get_packet(AP_Frsky_SPort::sport_packet_t &sport_packet, bool discard_duplicates)
+{
+    if (!should_process_packet(_parse_state.rx_buffer, discard_duplicates)) {
+        return false;
+    }
+
+    const AP_Frsky_SPort::sport_packet_t sp {
+        _parse_state.rx_buffer[0],
+        _parse_state.rx_buffer[1],
+        le16toh_ptr(&_parse_state.rx_buffer[2]),
+        le32toh_ptr(&_parse_state.rx_buffer[4])
+    };
+
+    sport_packet = sp;
+    return true;
+}
+
+/*
+ * Calculates the sensor id from the physical sensor index [0-27]
+        0x00, 	// Physical ID 0 - Vario2 (altimeter high precision)
+        0xA1, 	// Physical ID 1 - FLVSS Lipo sensor
+        0x22, 	// Physical ID 2 - FAS-40S current sensor
+        0x83, 	// Physical ID 3 - GPS / altimeter (normal precision)
+        0xE4, 	// Physical ID 4 - RPM
+        0x45, 	// Physical ID 5 - SP2UART(Host)
+        0xC6, 	// Physical ID 6 - SPUART(Remote)
+        0x67, 	// Physical ID 7 - Ardupilot/Betaflight EXTRA DOWNLINK
+        0x48, 	// Physical ID 8 -
+        0xE9, 	// Physical ID 9 -
+        0x6A, 	// Physical ID 10 -
+        0xCB, 	// Physical ID 11 -
+        0xAC, 	// Physical ID 12 -
+        0x0D, 	// Physical ID 13 - Ardupilot/Betaflight UPLINK
+        0x8E, 	// Physical ID 14 -
+        0x2F, 	// Physical ID 15 -
+        0xD0, 	// Physical ID 16 -
+        0x71, 	// Physical ID 17 -
+        0xF2, 	// Physical ID 18 -
+        0x53, 	// Physical ID 19 -
+        0x34, 	// Physical ID 20 - Ardupilot/Betaflight EXTRA DOWNLINK
+        0x95, 	// Physical ID 21 -
+        0x16, 	// Physical ID 22 - GAS Suite
+        0xB7, 	// Physical ID 23 - IMU ACC (x,y,z)
+        0x98, 	// Physical ID 24 -
+        0x39, 	// Physical ID 25 - Power Box
+        0xBA 	// Physical ID 26 - Temp
+        0x1B	// Physical ID 27 - ArduPilot/Betaflight DEFAULT DOWNLINK
+ * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
+ */
+#define BIT(x, index) (((x) >> index) & 0x01)
+uint8_t AP_Frsky_SPort::calc_sensor_id(const uint8_t physical_id)
+{
+    uint8_t result = physical_id;
+    result += (BIT(physical_id, 0) ^ BIT(physical_id, 1) ^ BIT(physical_id, 2)) << 5;
+    result += (BIT(physical_id, 2) ^ BIT(physical_id, 3) ^ BIT(physical_id, 4)) << 6;
+    result += (BIT(physical_id, 0) ^ BIT(physical_id, 2) ^ BIT(physical_id, 4)) << 7;
+    return result;
+}

libraries/AP_Frsky_Telem/AP_Frsky_SPort.h

@@ -11,6 +11,16 @@ public:
 
     void send() override;
 
+    typedef union {
+        struct PACKED {
+            uint8_t sensor;
+            uint8_t frame;
+            uint16_t appid;
+            uint32_t data;
+        };
+        uint8_t raw[8];
+    } sport_packet_t;
+
 protected:
 
     void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data);

libraries/AP_Frsky_Telem/AP_Frsky_SPortParser.h

@@ -0,0 +1,49 @@
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+
+#include "AP_Frsky_SPort.h"
+
+#include <stdint.h>
+
+// for SPort X protocol
+#define FRAME_HEAD                  0x7E
+#define FRAME_DLE                   0x7D
+#define FRAME_XOR                   0x20
+// for SPort D protocol
+#define START_STOP_D                0x5E
+#define BYTESTUFF_D                 0x5D
+// for SPort packet parser
+#define TELEMETRY_RX_BUFFER_SIZE    19U  // 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
+#define SPORT_PACKET_SIZE           9U
+#define STUFF_MASK                  0x20
+#define SPORT_DATA_FRAME            0x10
+#define SPORT_UPLINK_FRAME          0x30
+#define SPORT_UPLINK_FRAME_RW       0x31
+#define SPORT_DOWNLINK_FRAME        0x32
+
+class AP_Frsky_SPortParser
+{
+public:
+
+    // packet parser helpers
+    bool process_byte(AP_Frsky_SPort::sport_packet_t &sport_packet, const uint8_t data);
+
+private:
+    enum class ParseState : uint8_t {
+        IDLE,
+        START,
+        IN_FRAME,
+        XOR,
+    };
+
+    struct {
+        uint8_t rx_buffer_count;
+        uint8_t rx_buffer[TELEMETRY_RX_BUFFER_SIZE];
+        uint8_t last_packet[SPORT_PACKET_SIZE];
+        ParseState state;
+    } _parse_state;
+
+    bool should_process_packet(const uint8_t *packet, bool discard_duplicates);
+    bool get_packet(AP_Frsky_SPort::sport_packet_t &sport_packet, bool discard_duplicates);
+};

libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp

@@ -9,6 +9,16 @@
 #include <AP_RangeFinder/AP_RangeFinder.h>
 #include <GCS_MAVLink/GCS.h>
 
+#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+#include <AC_Fence/AC_Fence.h>
+#include <AP_Logger/AP_Logger.h>
+#include <AP_Param/AP_Param.h>
+#include <AP_Vehicle/AP_Vehicle.h>
+#include <SRV_Channel/SRV_Channel.h>
+#include <stdio.h>
+#include "AP_Frsky_MAVlite.h"
+#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+
 /*
 for FrSky SPort Passthrough
 */
@@ -99,12 +109,50 @@ void AP_Frsky_SPort_Passthrough::setup_wfq_scheduler(void)
     set_scheduler_entry(GPS_STATUS, 700, 500);  // 0x5002 GPS status
     set_scheduler_entry(HOME, 400, 500);        // 0x5004 Home
     set_scheduler_entry(BATT_2, 1300, 500);     // 0x5008 Battery 2 status
-    set_scheduler_entry(BATT_1, 1300, 500);     // 0x5008 Battery 1 status
+    set_scheduler_entry(BATT_1, 1300, 500);     // 0x5003 Battery 1 status
     set_scheduler_entry(PARAM, 1700, 1000);     // 0x5007 parameters
+#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+    set_scheduler_entry(MAV, 35, 25);           // mavlite
+    // initialize sport sensor IDs
+    set_sensor_id(_frsky_parameters->_uplink_id, _SPort_bidir.uplink_sensor_id);
+    set_sensor_id(_frsky_parameters->_dnlink1_id, _SPort_bidir.downlink1_sensor_id);
+    set_sensor_id(_frsky_parameters->_dnlink2_id, _SPort_bidir.downlink2_sensor_id);
+    // initialize sport
+    hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_Frsky_SPort_Passthrough::process_rx_queue, void));
+#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
 }
 
+/*
+  dynamically change scheduler priorities based on queue sizes
+*/
 void AP_Frsky_SPort_Passthrough::adjust_packet_weight(bool queue_empty)
 {
+    /*
+        When queues are empty set a low priority (high weight), when queues
+        are not empty set a higher priority (low weight) based on the following
+        relative priority order: mavlite > status text > attitude.
+     */
+#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+    if (!_SPort_bidir.tx_packet_queue.is_empty()) {
+        _scheduler.packet_weight[MAV] = 30;             // mavlite
+        if (!queue_empty) {
+            _scheduler.packet_weight[TEXT] = 45;        // messages
+            _scheduler.packet_weight[ATTITUDE] = 80;    // attitude
+        } else {
+            _scheduler.packet_weight[TEXT] = 5000;      // messages
+            _scheduler.packet_weight[ATTITUDE] = 80;    // attitude
+        }
+    } else {
+        _scheduler.packet_weight[MAV] = 5000;           // mavlite
+        if (!queue_empty) {
+            _scheduler.packet_weight[TEXT] = 45;        // messages
+            _scheduler.packet_weight[ATTITUDE] = 80;    // attitude
+        } else {
+            _scheduler.packet_weight[TEXT] = 5000;      // messages
+            _scheduler.packet_weight[ATTITUDE] = 45;    // attitude
+        }
+    }
+#else   //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
     if (!queue_empty) {
         _scheduler.packet_weight[TEXT] = 45;     // messages
         _scheduler.packet_weight[ATTITUDE] = 80;     // attitude
@@ -112,6 +160,7 @@ void AP_Frsky_SPort_Passthrough::adjust_packet_weight(bool queue_empty)
         _scheduler.packet_weight[TEXT] = 5000;   // messages
         _scheduler.packet_weight[ATTITUDE] = 45;     // attitude
     }
+#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
 }
 
 // WFQ scheduler
@@ -122,12 +171,22 @@ bool AP_Frsky_SPort_Passthrough::is_packet_ready(uint8_t idx, bool queue_empty)
     case TEXT:
         packet_ready = !queue_empty;
         break;
+    case GPS_LAT:
+    case GPS_LON:
+        // force gps coords to use sensor 0x1B, always send when used with external data
+        packet_ready = _use_external_data || (_passthrough.new_byte == SENSOR_ID_27);
+        break;
     case AP_STATUS:
         packet_ready = gcs().vehicle_initialised();
         break;
     case BATT_2:
         packet_ready = AP::battery().num_instances() > 1;
         break;
+#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+        case MAV:
+            packet_ready = !_SPort_bidir.tx_packet_queue.is_empty();
+            break;
+#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
     default:
         packet_ready = true;
         break;
@@ -157,8 +216,7 @@ void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx)
         send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(_passthrough.send_latitude)); // gps latitude or longitude
         _passthrough.gps_lng_sample = calc_gps_latlng(_passthrough.send_latitude);
         // force the scheduler to select GPS lon as packet that's been waiting the most
-        // this guarantees that gps coords are sent at max
-        // _scheduler.avg_polling_period*number_of_downlink_sensors time separation
+            // this guarantees that lat and lon are sent as consecutive packets
         _scheduler.packet_timer[GPS_LON] = _scheduler.packet_timer[GPS_LAT] - 10000;
         break;
     case GPS_LON: // 0x800 GPS lon
@@ -185,6 +243,11 @@ void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx)
     case PARAM: // 0x5007 parameters
         send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param());
         break;
+#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+    case MAV: // mavlite
+        process_tx_queue();
+        break;
+#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
     }
 }
 
@@ -205,11 +268,17 @@ void AP_Frsky_SPort_Passthrough::send(void)
     for (uint16_t i = 0; i < numc; i++) {
         prev_byte = _passthrough.new_byte;
         _passthrough.new_byte = _port->read();
-    }
-    if (prev_byte == FRAME_HEAD) {
-        if (_passthrough.new_byte == SENSOR_ID_27) { // byte 0x7E is the header of each poll request
-            run_wfq_scheduler();
+#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+        AP_Frsky_SPort::sport_packet_t sp;
+
+        if (_sport_handler.process_byte(sp, _passthrough.new_byte)) {
+            queue_rx_packet(sp);
         }
+#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+    }
+    // check if we should respond to this polling byte
+    if (prev_byte == FRAME_HEAD && is_passthrough_byte(_passthrough.new_byte)) {
+        run_wfq_scheduler();
     }
 }
 
@@ -358,6 +427,20 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_batt(uint8_t instance)
     return batt;
 }
 
+/*
+ * true if we need to respond to the last polling byte
+ * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
+ */
+bool AP_Frsky_SPort_Passthrough::is_passthrough_byte(const uint8_t byte)
+{
+#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+    if( byte == _SPort_bidir.downlink1_sensor_id || byte == _SPort_bidir.downlink2_sensor_id ) {
+        return true;
+    }
+#endif
+    return byte == SENSOR_ID_27;
+}
+
 /*
  * prepare various autopilot status data
  * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
@@ -465,6 +548,27 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_attiandrng(void)
     return attiandrng;
 }
 
+#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+/*
+  allow external transports (e.g. FPort), to supply telemetry data
+ */
+bool AP_Frsky_SPort_Passthrough::set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data)
+{
+    // queue only Uplink packets
+    if (frame == SPORT_UPLINK_FRAME || frame == SPORT_UPLINK_FRAME_RW) {
+        const AP_Frsky_SPort::sport_packet_t sp {
+            0x00,   // this is ignored by process_sport_rx_queue() so no need for a real sensor ID
+            frame,
+            appid,
+            data
+        };
+
+        _SPort_bidir.rx_packet_queue.push_force(sp);
+        return true;
+    }
+    return false;
+}
+
 /*
   fetch Sport data for an external transport, such as FPort
  */
@@ -545,3 +649,341 @@ uint16_t AP_Frsky_SPort_Passthrough::prep_number(int32_t number, uint8_t digits,
     }
     return res;
 }
+
+
+
+
+/*
+ * Queue uplink packets in the sport rx queue
+ * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
+ */
+void AP_Frsky_SPort_Passthrough::queue_rx_packet(const AP_Frsky_SPort::sport_packet_t packet)
+{
+    // queue only Uplink packets
+    if (packet.sensor == _SPort_bidir.uplink_sensor_id && packet.frame == SPORT_UPLINK_FRAME) {
+        _SPort_bidir.rx_packet_queue.push_force(packet);
+    }
+}
+
+/*
+ * Extract up to 1 mavlite message from the sport rx packet queue
+ * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
+ */
+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;
+
+        if (_mavlite_handler.parse(rxmsg, packet)) {
+            mavlite_process_message(rxmsg);
+            break; // process only 1 mavlite message each call
+        }
+    }
+}
+
+/*
+ * Process the sport tx queue
+ * pop and send 1 sport packet
+ * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
+ */
+void AP_Frsky_SPort_Passthrough::process_tx_queue()
+{
+    AP_Frsky_SPort::sport_packet_t packet;
+
+    if (!_SPort_bidir.tx_packet_queue.peek(packet)) {
+        return;
+    }
+
+    // when using fport repeat each packet to account for
+    // fport packet loss (around 15%)
+    if (!_use_external_data || _SPort_bidir.tx_packet_duplicates++ == SPORT_TX_PACKET_DUPLICATES) {
+        _SPort_bidir.tx_packet_queue.pop();
+        _SPort_bidir.tx_packet_duplicates = 0;
+    }
+
+    send_sport_frame(SPORT_DOWNLINK_FRAME, packet.appid, packet.data);
+}
+
+
+/*
+ * 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)
+{
+    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);
+    AP_Frsky_MAVlite::mavlite_msg_get_uint8(cmd_options, rxmsg, 2);
+    uint8_t param_count = AP_Frsky_MAVlite::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
+    }
+
+    mav_command_long.param1 = params[0];
+    mav_command_long.param2 = params[1];
+    mav_command_long.param3 = params[2];
+    mav_command_long.param4 = params[3];
+    mav_command_long.param5 = params[4];
+    mav_command_long.param6 = params[5];
+    mav_command_long.param7 = params[6];
+
+    MAV_RESULT mav_result = MAV_RESULT_FAILED;
+    // filter allowed commands
+    switch (mav_command_long.command) {
+        //case MAV_CMD_ACCELCAL_VEHICLE_POS:
+        case MAV_CMD_DO_SET_MODE:
+            if (AP::vehicle()->set_mode(mav_command_long.param1, ModeReason::FRSKY_COMMAND)) {
+                mav_result = MAV_RESULT_ACCEPTED;
+            }
+            break;
+        //case MAV_CMD_DO_SET_HOME:
+        case MAV_CMD_DO_FENCE_ENABLE:
+            mav_result = mavlite_handle_command_do_fence_enable((uint16_t)mav_command_long.param1);
+            break;
+        case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+            if (is_equal(mav_command_long.param1,1.0f) && is_zero(mav_command_long.param2)) {
+                mav_result = mavlite_handle_command_preflight_reboot();
+            }
+            break;
+        //case MAV_CMD_DO_START_MAG_CAL:
+        //case MAV_CMD_DO_ACCEPT_MAG_CAL:
+        //case MAV_CMD_DO_CANCEL_MAG_CAL:
+        //case MAV_CMD_START_RX_PAIR:
+        //case MAV_CMD_DO_DIGICAM_CONFIGURE:
+        //case MAV_CMD_DO_DIGICAM_CONTROL:
+        //case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
+        //case MAV_CMD_DO_GRIPPER:
+        //case MAV_CMD_DO_MOUNT_CONFIGURE:
+        //case MAV_CMD_DO_MOUNT_CONTROL:
+        //case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
+        //case MAV_CMD_DO_SET_ROI_SYSID:
+        //case MAV_CMD_DO_SET_ROI_LOCATION:
+        //case MAV_CMD_DO_SET_ROI:
+        case MAV_CMD_PREFLIGHT_CALIBRATION:
+            if (is_equal(mav_command_long.param1,0.0f) && is_equal(mav_command_long.param2,0.0f) && is_equal(mav_command_long.param3,1.0f)) {
+                mav_result = mavlite_handle_command_preflight_calibration_baro();
+            }
+            break;
+        //case MAV_CMD_BATTERY_RESET:
+        //case MAV_CMD_PREFLIGHT_UAVCAN:
+        //case MAV_CMD_FLASH_BOOTLOADER:
+        //case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
+        //case MAV_CMD_GET_HOME_POSITION:
+        //case MAV_CMD_PREFLIGHT_STORAGE:
+        //case MAV_CMD_SET_MESSAGE_INTERVAL:
+        //case MAV_CMD_GET_MESSAGE_INTERVAL:
+        //case MAV_CMD_REQUEST_MESSAGE:
+        //case MAV_CMD_DO_SET_SERVO:
+        //case MAV_CMD_DO_REPEAT_SERVO:
+        //case MAV_CMD_DO_SET_RELAY:
+        //case MAV_CMD_DO_REPEAT_RELAY:
+        //case MAV_CMD_DO_FLIGHTTERMINATION:
+        //case MAV_CMD_COMPONENT_ARM_DISARM:
+        //case MAV_CMD_FIXED_MAG_CAL_YAW:
+        default:
+            mav_result = MAV_RESULT_UNSUPPORTED;
+            break;
+    }
+    mavlite_send_command_ack(mav_result, mav_command_long.command);
+}
+
+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;
+    txmsg.msgid = MAVLINK_MSG_ID_COMMAND_ACK;
+    AP_Frsky_MAVlite::mavlite_msg_set_uint16(txmsg, cmdid, 0);
+    AP_Frsky_MAVlite::mavlite_msg_set_uint8(txmsg, (uint8_t)mav_result, 2);
+    mavlite_send_message(txmsg);
+}
+
+MAV_RESULT AP_Frsky_SPort_Passthrough::mavlite_handle_command_preflight_calibration_baro()
+{
+    if (hal.util->get_soft_armed()) {
+        return MAV_RESULT_DENIED;
+    }
+    // fast barometer calibration
+    gcs().send_text(MAV_SEVERITY_INFO, "Updating barometer calibration");
+    AP::baro().update_calibration();
+    gcs().send_text(MAV_SEVERITY_INFO, "Barometer calibration complete");
+
+    AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
+    if (airspeed != nullptr) {
+        airspeed->calibrate(false);
+    }
+
+    return MAV_RESULT_ACCEPTED;
+}
+
+MAV_RESULT AP_Frsky_SPort_Passthrough::mavlite_handle_command_do_fence_enable(uint16_t param1)
+{
+    AC_Fence *fence = AP::fence();
+    if (fence == nullptr) {
+        return MAV_RESULT_UNSUPPORTED;
+    }
+
+    switch (param1) {
+        case 0:
+            fence->enable(false);
+            return MAV_RESULT_ACCEPTED;
+        case 1:
+            fence->enable(true);
+            return MAV_RESULT_ACCEPTED;
+        default:
+            return MAV_RESULT_FAILED;
+    }
+}
+
+/*
+ * 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)
+{
+    float param_value;
+    char param_name[AP_MAX_NAME_SIZE+1];
+    AP_Frsky_MAVlite::mavlite_msg_get_string(param_name, rxmsg, 0);
+    // 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;
+    txmsg.msgid = MAVLINK_MSG_ID_PARAM_VALUE;
+    AP_Frsky_MAVlite::mavlite_msg_set_float(txmsg, param_value, 0);
+    AP_Frsky_MAVlite::mavlite_msg_set_string(txmsg, param_name, 4);
+    mavlite_send_message(txmsg);
+}
+
+/*
+ * 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)
+{
+    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);
+    AP_Frsky_MAVlite::mavlite_msg_get_string(param_name, rxmsg, 4);
+    // find existing param so we can get the old value
+    enum ap_var_type var_type;
+    // set parameter
+    AP_Param *vp;
+    uint16_t parameter_flags = 0;
+    vp = AP_Param::find(param_name, &var_type, &parameter_flags);
+    if (vp == nullptr || isnan(param_value) || isinf(param_value)) {
+        return;
+    }
+    if ((parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) || vp->is_read_only()) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Param write denied (%s)", param_name);
+    } else if (!AP_Param::set_and_save(param_name, param_value)) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Param write failed (%s)", param_name);
+    }
+    // let's read back the last value, either the readonly one or the updated one
+    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;
+    txmsg.msgid = MAVLINK_MSG_ID_PARAM_VALUE;
+    AP_Frsky_MAVlite::mavlite_msg_set_float(txmsg, param_value, 0);
+    AP_Frsky_MAVlite::mavlite_msg_set_string(txmsg, param_name, 4);
+    mavlite_send_message(txmsg);
+}
+
+/*
+  Handle a MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN command 
+  for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
+  Optionally disable PX4IO overrides. This is done for quadplanes to
+  prevent the mixer running while rebooting which can start the VTOL
+  motors. That can be dangerous when a preflight reboot is done with
+  the pilot close to the aircraft and can also damage the aircraft
+ */
+MAV_RESULT AP_Frsky_SPort_Passthrough::mavlite_handle_command_preflight_reboot(void)
+{
+    if (hal.util->get_soft_armed()) {
+        // refuse reboot when armed
+        return MAV_RESULT_FAILED;
+    }
+#if APM_BUILD_TYPE(APM_BUILD_ArduSub)
+    SRV_Channels::zero_rc_outputs();
+#endif
+    // send ack before we reboot
+    mavlite_send_command_ack(MAV_RESULT_ACCEPTED, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN);
+    // Notify might want to blink some LEDs:
+    AP_Notify *notify = AP_Notify::get_singleton();
+    if (notify) {
+        AP_Notify::flags.firmware_update = 1;
+        notify->update();
+    }
+    // force safety on
+    hal.rcout->force_safety_on();
+
+    // flush pending parameter writes
+    AP_Param::flush();
+
+    // do not process incoming mavlink messages while we delay:
+    hal.scheduler->register_delay_callback(nullptr, 5);
+
+    // delay to give the ACK a chance to get out, the LEDs to flash,
+    // the IO board safety to be forced on, the parameters to flush, ...
+    hal.scheduler->delay(1000);
+
+    hal.scheduler->reboot(false);
+
+    return MAV_RESULT_FAILED;
+}
+
+/*
+ * 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)
+{
+    switch (rxmsg.msgid) {
+        case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
+            mavlite_handle_param_request_read(rxmsg);
+            break;
+        case MAVLINK_MSG_ID_PARAM_SET:
+            mavlite_handle_param_set(rxmsg);
+            break;
+        case MAVLINK_MSG_ID_COMMAND_LONG:
+            mavlite_handle_command_long(rxmsg);
+            break;
+    }
+}
+
+/*
+ * Send a mavlite message
+ * 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)
+{
+    return _mavlite_handler.encode(_SPort_bidir.tx_packet_queue, txmsg);
+}
+
+
+/*
+ * Utility method to apply constraints in changing sensor id values
+ * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
+ */
+void AP_Frsky_SPort_Passthrough::set_sensor_id(AP_Int8 param_idx, uint8_t &sensor)
+{
+    int8_t idx = param_idx.get();
+
+    if (idx == -1) {
+        // disable this sensor
+        sensor = 0xFF;
+        return;
+    }
+    sensor = calc_sensor_id(idx);
+}
+#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL

libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h

@@ -3,17 +3,27 @@
 #include "AP_Frsky_SPort.h"
 #include <AP_RCTelemetry/AP_RCTelemetry.h>
 
-// for fair scheduler
-#define TIME_SLOT_MAX               11
+#include "AP_Frsky_SPortParser.h"
+#include "AP_Frsky_MAVlite.h"
+
+#include "AP_Frsky_Telem.h"
+
+#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+#define FRSKY_WFQ_TIME_SLOT_MAX     12U
+#define SPORT_TX_PACKET_DUPLICATES          1   // number of duplicates packets we send (fport only)
+#else
+#define FRSKY_WFQ_TIME_SLOT_MAX     11U
+#endif
 
 class AP_Frsky_SPort_Passthrough : public AP_Frsky_SPort, public AP_RCTelemetry
 {
 public:
 
-    AP_Frsky_SPort_Passthrough(AP_HAL::UARTDriver *port, bool use_external_data) :
+    AP_Frsky_SPort_Passthrough(AP_HAL::UARTDriver *port, bool use_external_data, AP_Frsky_Parameters *&frsky_parameters) :
         AP_Frsky_SPort(port),
-        AP_RCTelemetry(TIME_SLOT_MAX),
-        _use_external_data(use_external_data)
+        AP_RCTelemetry(FRSKY_WFQ_TIME_SLOT_MAX),
+        _use_external_data(use_external_data),
+        _frsky_parameters(frsky_parameters)
     { }
 
     bool init() override;
@@ -29,19 +39,21 @@ public:
     bool get_next_msg_chunk(void) override;
 
     bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data) override;
+    bool set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data) override;
 
     void queue_text_message(MAV_SEVERITY severity, const char *text) override
     {
         AP_RCTelemetry::queue_message(severity, text);
     }
 
-
 protected:
 
     void send() override;
 
 private:
 
+    AP_Frsky_Parameters *&_frsky_parameters;
+
     enum PassthroughParam : uint8_t {
         FRAME_TYPE =          1,
         BATT_FS_VOLTAGE =     2,
@@ -61,7 +73,10 @@ private:
         HOME =          7,  // 0x5004 Home
         BATT_2 =        8,  // 0x5008 Battery 2 status
         BATT_1 =        9,  // 0x5008 Battery 1 status
-        PARAM =         10  // 0x5007 parameters
+        PARAM =         10, // 0x5007 parameters
+#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+        MAV =           11,  // mavlite
+#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
     };
 
     // methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format
@@ -88,8 +103,46 @@ private:
         uint8_t char_index; // index of which character to get in the message
     } _msg_chunk;
 
+
+#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+    // bidirectional sport telemetry
+    struct {
+        uint8_t uplink_sensor_id = 0x0D;
+        uint8_t downlink1_sensor_id = 0x34;
+        uint8_t downlink2_sensor_id = 0x67;
+        uint8_t tx_packet_duplicates;
+        ObjectBuffer_TS<AP_Frsky_SPort::sport_packet_t> rx_packet_queue{SPORT_PACKET_QUEUE_LENGTH};
+        ObjectBuffer_TS<AP_Frsky_SPort::sport_packet_t> tx_packet_queue{SPORT_PACKET_QUEUE_LENGTH};
+    } _SPort_bidir;
+
+    AP_Frsky_SPortParser _sport_handler;
+    AP_Frsky_MAVlite _mavlite_handler;
+
+    void set_sensor_id(AP_Int8 idx, uint8_t &sensor);
+    // tx/rx sport packet processing
+    void queue_rx_packet(const AP_Frsky_SPort::sport_packet_t sp);
+    void process_rx_queue(void);
+    void process_tx_queue(void);
+
+    // mavlite messages tx/rx methods
+    bool mavlite_send_message(AP_Frsky_MAVlite::mavlite_message_t &txmsg);
+    void mavlite_process_message(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg);
+
+    // gcs mavlite methods
+    void mavlite_handle_param_request_read(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg);
+    void mavlite_handle_param_set(const AP_Frsky_MAVlite::mavlite_message_t &rxmsg);
+    void mavlite_handle_command_long(const AP_Frsky_MAVlite::mavlite_message_t &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);
+    MAV_RESULT mavlite_handle_command_preflight_reboot(void);
+#endif
+
     void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data);
 
+    // true if we need to respond to the last polling byte
+    bool is_passthrough_byte(const uint8_t byte);
+
     uint8_t _paramID;
 
     uint32_t calc_gps_status(void);

libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp

@@ -23,9 +23,12 @@
 */
 
 #include "AP_Frsky_Telem.h"
+#include "AP_Frsky_Parameters.h"
 
 #include <AP_SerialManager/AP_SerialManager.h>
 
+#include <AP_Vehicle/AP_Vehicle.h>
+
 #include "AP_Frsky_D.h"
 #include "AP_Frsky_SPort.h"
 #include "AP_Frsky_SPort_Passthrough.h"
@@ -37,8 +40,12 @@ AP_Frsky_Telem *AP_Frsky_Telem::singleton;
 AP_Frsky_Telem::AP_Frsky_Telem()
 {
     singleton = this;
+#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+    _frsky_parameters = &AP::vehicle()->frsky_parameters;
+#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
 }
 
+
 AP_Frsky_Telem::~AP_Frsky_Telem(void)
 {
     singleton = nullptr;
@@ -58,7 +65,7 @@ bool AP_Frsky_Telem::init(bool use_external_data)
     } else if ((port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort, 0))) {
         _backend = new AP_Frsky_SPort(port);
     } else if (use_external_data || (port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough, 0))) {
-        _backend = new AP_Frsky_SPort_Passthrough(port, use_external_data);
+        _backend = new AP_Frsky_SPort_Passthrough(port, use_external_data, _frsky_parameters);
     }
 
     if (_backend == nullptr) {
@@ -82,6 +89,14 @@ bool AP_Frsky_Telem::_get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &
     return _backend->get_telem_data(frame, appid, data);
 }
 
+bool AP_Frsky_Telem::_set_telem_data(uint8_t frame, uint16_t appid, uint32_t data)
+{
+    if (_backend == nullptr) {
+        return false;
+    }
+    return _backend->set_telem_data(frame, appid, data);
+}
+
 /*
   fetch Sport data for an external transport, such as FPort
  */
@@ -96,14 +111,35 @@ bool AP_Frsky_Telem::get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &d
             singleton->init(true);
         }
     }
-    if (!singleton) {
+    if (singleton == nullptr) {
         return false;
     }
     return singleton->_get_telem_data(frame, appid, data);
 }
 
-namespace AP {
-    AP_Frsky_Telem *frsky_telem() {
-        return AP_Frsky_Telem::get_singleton();
+/*
+  allow external transports (e.g. FPort), to supply telemetry data
+ */
+bool AP_Frsky_Telem::set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data)
+{
+    if (!singleton && !hal.util->get_soft_armed()) {
+        // if telem data is requested when we are disarmed and don't
+        // yet have a AP_Frsky_Telem object then try to allocate one
+        new AP_Frsky_Telem();
+        if (singleton) {
+            singleton->init(true);
+        }
+    }
+    if (singleton == nullptr) {
+        return false;
     }
+    return singleton->_set_telem_data(frame, appid, data);
+}
+
+namespace AP
+{
+AP_Frsky_Telem *frsky_telem()
+{
+    return AP_Frsky_Telem::get_singleton();
+}
 };

libraries/AP_Frsky_Telem/AP_Frsky_Telem.h

@@ -14,9 +14,16 @@
 */
 #pragma once
 
+#ifndef HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+#define HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL 1
+#endif
+
 #include "AP_Frsky_Backend.h"
 
+class AP_Frsky_Parameters;
+
 class AP_Frsky_Telem {
+
 public:
     AP_Frsky_Telem();
 
@@ -35,6 +42,10 @@ public:
 
     // get next telemetry data for external consumers of SPort data
     static bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data);
+#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+    // set telemetry data from external producer of SPort data
+    static bool set_telem_data(const uint8_t frame,const uint16_t appid, const uint32_t data);
+#endif
 
     void queue_message(MAV_SEVERITY severity, const char *text) {
         if (_backend == nullptr) {
@@ -47,8 +58,14 @@ private:
 
     AP_Frsky_Backend *_backend;
 
+#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+    AP_Frsky_Parameters* _frsky_parameters;
+
     // get next telemetry data for external consumers of SPort data (internal function)
     bool _get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data);
+    // set telemetry data from external producer of SPort data (internal function)
+    bool _set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data);
+#endif
 
     static AP_Frsky_Telem *singleton;