Add FPort support

FPort code is mostly imported from Betaflight. To make future RX
updates easier, a few more changes have been done to make the
RX/Telemetry code in BF and INAV a bit closer.

- Imported RSSI handling code from BF with some changes
- frsky_vfas_cell_voltage setting renamed to report_cell_voltage
- Added telemetry_halfduplex setting
- sbus_inversion renamed to serialrx_inverted, should apply to all
protocols eventually
- New MSP cmds MSP_TX_INFO/MSP_SET_TX_INFO now allow setting the RSSI
via MSP, thus RX_MSP doesn't need to use an extra channel anymore.

Makefile

@@ -631,6 +631,7 @@ COMMON_SRC = \
             io/rcdevice.c \
             io/rcdevice_cam.c \
             msp/msp_serial.c \
+            rx/fport.c \
             rx/ibus.c \
             rx/jetiexbus.c \
             rx/msp.c \
@@ -645,6 +646,7 @@ COMMON_SRC = \
             rx/rx_spi.c \
             rx/crsf.c \
             rx/sbus.c \
+            rx/sbus_channels.c \
             rx/spektrum.c \
             rx/sumd.c \
             rx/sumh.c \
@@ -717,6 +719,7 @@ HIGHEND_SRC = \
             telemetry/ibus.c \
             telemetry/ltm.c \
             telemetry/mavlink.c \
+            telemetry/msp_shared.c \
             telemetry/smartport.c \
             telemetry/telemetry.c \
             io/vtx_string.c \

src/main/blackbox/blackbox.c

@@ -1174,7 +1174,7 @@ static void loadMainState(timeUs_t currentTimeUs)
     blackboxCurrent->surfaceRaw = rangefinderGetLatestRawAltitude();
 #endif
 
-    blackboxCurrent->rssi = rssi;
+    blackboxCurrent->rssi = getRSSI();
 
 #ifdef USE_SERVOS
     //Tail servo for tricopters

src/main/build/debug.h

@@ -57,6 +57,8 @@ typedef enum {
     DEBUG_PITOT,
     DEBUG_AGL,
     DEBUG_FLOW_RAW,
+    DEBUG_SBUS,
+    DEBUG_FPORT,
     DEBUG_ALWAYS,
     DEBUG_COUNT
 } debugType_e;

src/main/common/streambuf.c

@@ -20,6 +20,13 @@
 
 #include "streambuf.h"
 
+sbuf_t *sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end)
+{
+    sbuf->ptr = ptr;
+    sbuf->end = end;
+    return sbuf;
+}
+
 void sbufWriteU8(sbuf_t *dst, uint8_t val)
 {
     *dst->ptr++ = val;

src/main/common/streambuf.h

@@ -28,6 +28,8 @@ typedef struct sbuf_s {
     uint8_t *end;
 } sbuf_t;
 
+sbuf_t *sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end);
+
 void sbufWriteU8(sbuf_t *dst, uint8_t val);
 void sbufWriteU16(sbuf_t *dst, uint16_t val);
 void sbufWriteU32(sbuf_t *dst, uint32_t val);

src/main/fc/fc_msp.c

@@ -105,7 +105,6 @@
 #endif
 
 extern timeDelta_t cycleTime; // FIXME dependency on mw.c
-extern uint16_t rssi; // FIXME dependency on mw.c
 
 static const char * const flightControllerIdentifier = INAV_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
 static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
@@ -538,7 +537,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
     case MSP_ANALOG:
         sbufWriteU8(dst, (uint8_t)constrain(getBatteryVoltage() / 10, 0, 255));
         sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
-        sbufWriteU16(dst, rssi);
+        sbufWriteU16(dst, getRSSI());
         sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
         break;
 
@@ -549,7 +548,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU16(dst, constrain(getPower(), 0, 0x7FFFFFFF));           // power draw
         sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
         sbufWriteU16(dst, (uint16_t)constrain(getMWhDrawn(), 0, 0xFFFF)); // milliWatt hours drawn from battery
-        sbufWriteU16(dst, rssi);
+        sbufWriteU16(dst, getRSSI());
         sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
         sbufWriteU8(dst, batteryWasFullWhenPluggedIn() | (batteryUsesCapacityThresholds() << 1) | (getBatteryState() << 2));
         sbufWriteU32(dst, getBatteryRemainingCapacity());
@@ -1229,6 +1228,16 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 #endif
         break;
 
+    case MSP_TX_INFO:
+        sbufWriteU8(dst, rssiSource);
+        uint8_t rtcDateTimeIsSet = 0;
+        dateTime_t dt;
+        if (rtcGetDateTime(&dt)) {
+            rtcDateTimeIsSet = 1;
+        }
+        sbufWriteU8(dst, rtcDateTimeIsSet);
+        break;
+
     case MSP_RTC:
         {
             int32_t seconds = 0;
@@ -2244,6 +2253,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         }
         break;
 
+    case MSP_SET_TX_INFO:
+        {
+            setRssiMsp(sbufReadU8(src));
+        }
+        break;
+
     case MSP_SET_NAME:
         {
             char *name = systemConfigMutable()->name;

src/main/fc/settings.yaml

@@ -25,7 +25,7 @@ tables:
     values: ["NONE", "PWM", "PPM", "SERIAL", "MSP", "SPI", "UIB"]
     enum: rxReceiverType_e
   - name: serial_rx
-    values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF"]
+    values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT"]
   - name: rx_spi_protocol
     values: ["V202_250K", "V202_1M", "SYMA_X", "SYMA_X5C", "CX10", "CX10A", "H8_3D", "INAV", "ELERES"]
     enum: rx_spi_protocol_e
@@ -69,7 +69,7 @@ tables:
   - name: i2c_speed
     values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
   - name: debug_modes
-    values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "RFIND", "RFIND_Q", "PITOT", "AGL", "FLOW_RAW","ALWAYS"]
+    values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "RFIND", "RFIND_Q", "PITOT", "AGL", "FLOW_RAW", "SBUS", "FPORT", "ALWAYS"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -327,7 +327,7 @@ groups:
       - name: serialrx_provider
         condition: USE_SERIAL_RX
         table: serial_rx
-      - name: sbus_inversion
+      - name: serialrx_inverted
         condition: USE_SERIAL_RX
         type: bool
       - name: rx_spi_protocol
@@ -1318,7 +1318,7 @@ groups:
       - name: frsky_vfas_precision
         min: FRSKY_VFAS_PRECISION_LOW
         max: FRSKY_VFAS_PRECISION_HIGH
-      - name: frsky_vfas_cell_voltage
+      - name: report_cell_voltage
         type: bool
       - name: hott_alarm_sound_interval
         field: hottAlarmSoundInterval
@@ -1341,6 +1341,9 @@ groups:
         field: ltmUpdateRate
         condition: USE_TELEMETRY_LTM
         table: ltm_rates
+      - name: telemetry_halfduplex
+        field: halfDuplex
+        type: bool
 
   - name: PG_ELERES_CONFIG
     type: eleresConfig_t

src/main/io/ledstrip.c

@@ -75,8 +75,6 @@
 #include "telemetry/telemetry.h"
 
 
-extern uint16_t rssi; // FIXME dependency on mw.c
-
 PG_REGISTER_WITH_RESET_FN(ledStripConfig_t, ledStripConfig, PG_LED_STRIP_CONFIG, 0);
 
 static bool ledStripInitialised = false;
@@ -488,7 +486,7 @@ static void applyLedFixedLayers(void)
 
             case LED_FUNCTION_RSSI:
                 color = HSV(RED);
-                hOffset += scaleRange(rssi * 100, 0, 1023, -30, 120);
+                hOffset += scaleRange(getRSSI() * 100, 0, 1023, -30, 120);
                 break;
 
             default:
@@ -615,7 +613,7 @@ static void applyLedRssiLayer(bool updateNow, timeUs_t *timer)
     int timeOffset = 1;
 
     if (updateNow) {
-       state = (rssi * 100) / 1023;
+       state = (getRSSI() * 100) / 1023;
 
        if (state > 50) {
            flash = false;

src/main/io/osd.c

@@ -436,7 +436,7 @@ static inline void osdFormatFlyTime(char *buff, textAttributes_t *attr)
  */
 static uint16_t osdConvertRSSI(void)
 {
-    uint16_t osdRssi = rssi * 100 / 1024; // change range
+    uint16_t osdRssi = getRSSI() * 100 / 1024; // change range
     if (osdRssi >= 100) {
         osdRssi = 99;
     }

src/main/msp/msp_protocol.h

@@ -242,6 +242,9 @@
 
 #define MSP_DISPLAYPORT                 182
 
+#define MSP_SET_TX_INFO                 186 // in message           Used to send runtime information from TX lua scripts to the firmware
+#define MSP_TX_INFO                     187 // out message          Used by TX lua scripts to read information from the firmware
+
 //
 // Multwii original MSP commands
 //

src/main/rx/crsf.c

@@ -162,8 +162,10 @@ STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void)
     return crc;
 }
 
-STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void)
+STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
+    UNUSED(rxRuntimeConfig);
+
     if (crsfFrameDone) {
         crsfFrameDone = false;
         if (crsfFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {

src/main/rx/fport.c

@@ -0,0 +1,398 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "platform.h"
+
+#if defined(USE_SERIAL_RX)
+
+#include "build/debug.h"
+
+#include "common/maths.h"
+#include "common/utils.h"
+
+#include "drivers/time.h"
+
+#include "io/serial.h"
+
+#ifdef USE_TELEMETRY
+#include "telemetry/telemetry.h"
+#include "telemetry/smartport.h"
+#endif
+
+#include "rx/rx.h"
+#include "rx/sbus_channels.h"
+#include "rx/fport.h"
+
+
+#define FPORT_TIME_NEEDED_PER_FRAME_US 3000
+#define FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US 2000
+#define FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500
+#define FPORT_MAX_TELEMETRY_AGE_MS 500
+
+
+#define FPORT_FRAME_MARKER 0x7E
+
+#define FPORT_ESCAPE_CHAR 0x7D
+#define FPORT_ESCAPE_MASK 0x20
+
+#define FPORT_CRC_VALUE 0xFF
+
+#define FPORT_BAUDRATE 115200
+
+#define FPORT_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO)
+
+enum {
+    DEBUG_FPORT_FRAME_INTERVAL = 0,
+    DEBUG_FPORT_FRAME_ERRORS,
+    DEBUG_FPORT_FRAME_LAST_ERROR,
+    DEBUG_FPORT_TELEMETRY_DELAY,
+};
+
+enum {
+    DEBUG_FPORT_NO_ERROR = 0,
+    DEBUG_FPORT_ERROR_TIMEOUT,
+    DEBUG_FPORT_ERROR_OVERSIZE,
+    DEBUG_FPORT_ERROR_SIZE,
+    DEBUG_FPORT_ERROR_CHECKSUM,
+    DEBUG_FPORT_ERROR_TYPE,
+    DEBUG_FPORT_ERROR_TYPE_SIZE,
+};
+
+enum {
+    FPORT_FRAME_TYPE_CONTROL = 0x00,
+    FPORT_FRAME_TYPE_TELEMETRY_REQUEST = 0x01,
+    FPORT_FRAME_TYPE_TELEMETRY_RESPONSE = 0x81,
+
+};
+
+enum {
+    FPORT_FRAME_ID_NULL = 0x00,     // (master/slave)
+    FPORT_FRAME_ID_DATA = 0x10,     // (master/slave)
+    FPORT_FRAME_ID_READ = 0x30,     // (master)
+    FPORT_FRAME_ID_WRITE = 0x31,    // (master)
+    FPORT_FRAME_ID_RESPONSE = 0x32, // (slave)
+};
+
+typedef struct fportControlData_s {
+    sbusChannels_t channels;
+    uint8_t rssi;
+} fportControlData_t;
+
+typedef union fportData_s {
+    fportControlData_t controlData;
+    smartPortPayload_t telemetryData;
+} fportData_t;
+
+typedef struct fportFrame_s {
+    uint8_t type;
+    fportData_t data;
+} fportFrame_t;
+
+#if defined(USE_SERIALRX_FPORT)
+
+static const smartPortPayload_t emptySmartPortFrame = { .frameId = 0, .valueId = 0, .data = 0 };
+
+#define FPORT_REQUEST_FRAME_LENGTH sizeof(fportFrame_t)
+#define FPORT_RESPONSE_FRAME_LENGTH (sizeof(uint8_t) + sizeof(smartPortPayload_t))
+
+#define FPORT_FRAME_PAYLOAD_LENGTH_CONTROL (sizeof(uint8_t) + sizeof(fportControlData_t))
+#define FPORT_FRAME_PAYLOAD_LENGTH_TELEMETRY_REQUEST (sizeof(uint8_t) + sizeof(smartPortPayload_t))
+
+#define NUM_RX_BUFFERS 3
+#define BUFFER_SIZE (FPORT_REQUEST_FRAME_LENGTH + 2 * sizeof(uint8_t))
+
+typedef struct fportBuffer_s {
+    uint8_t data[BUFFER_SIZE];
+    uint8_t length;
+} fportBuffer_t;
+
+static fportBuffer_t rxBuffer[NUM_RX_BUFFERS];
+
+static volatile uint8_t rxBufferWriteIndex = 0;
+static volatile uint8_t rxBufferReadIndex = 0;
+
+static volatile timeUs_t lastTelemetryFrameReceivedUs;
+static volatile bool clearToSend = false;
+
+static volatile uint8_t framePosition = 0;
+
+static smartPortPayload_t *mspPayload = NULL;
+static timeUs_t lastRcFrameReceivedMs = 0;
+
+static serialPort_t *fportPort;
+static bool telemetryEnabled = false;
+
+static void reportFrameError(uint8_t errorReason) {
+    static volatile uint16_t frameErrors = 0;
+
+    DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_ERRORS, ++frameErrors);
+    DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_LAST_ERROR, errorReason);
+}
+
+// Receive ISR callback
+static void fportDataReceive(uint16_t c, void *data)
+{
+    UNUSED(data);
+
+    static timeUs_t frameStartAt = 0;
+    static bool escapedCharacter = false;
+    static timeUs_t lastFrameReceivedUs = 0;
+    static bool telemetryFrame = false;
+
+    const timeUs_t currentTimeUs = micros();
+
+    clearToSend = false;
+
+    if (framePosition > 1 && cmpTimeUs(currentTimeUs, frameStartAt) > FPORT_TIME_NEEDED_PER_FRAME_US + 500) {
+        reportFrameError(DEBUG_FPORT_ERROR_TIMEOUT);
+
+        framePosition = 0;
+     }
+
+    uint8_t val = (uint8_t)c;
+
+    if (val == FPORT_FRAME_MARKER) {
+        if (framePosition > 1) {
+            const uint8_t nextWriteIndex = (rxBufferWriteIndex + 1) % NUM_RX_BUFFERS;
+            if (nextWriteIndex != rxBufferReadIndex) {
+                rxBuffer[rxBufferWriteIndex].length = framePosition - 1;
+                rxBufferWriteIndex = nextWriteIndex;
+            }
+
+            if (telemetryFrame) {
+                clearToSend = true;
+                lastTelemetryFrameReceivedUs = currentTimeUs;
+                telemetryFrame = false;
+            }
+
+            DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_INTERVAL, currentTimeUs - lastFrameReceivedUs);
+            lastFrameReceivedUs = currentTimeUs;
+
+            escapedCharacter = false;
+        }
+
+        frameStartAt = currentTimeUs;
+        framePosition = 1;
+    } else if (framePosition > 0) {
+        if (framePosition >= BUFFER_SIZE + 1) {
+                framePosition = 0;
+
+                reportFrameError(DEBUG_FPORT_ERROR_OVERSIZE);
+        } else {
+            if (escapedCharacter) {
+                val = val ^ FPORT_ESCAPE_MASK;
+                escapedCharacter = false;
+            } else if (val == FPORT_ESCAPE_CHAR) {
+                escapedCharacter = true;
+
+                return;
+            }
+
+            if (framePosition == 2 && val == FPORT_FRAME_TYPE_TELEMETRY_REQUEST) {
+                telemetryFrame = true;
+            }
+
+            rxBuffer[rxBufferWriteIndex].data[framePosition - 1] = val;
+            framePosition = framePosition + 1;
+        }
+    }
+}
+
+#if defined(USE_TELEMETRY_SMARTPORT)
+static void smartPortWriteFrameFport(const smartPortPayload_t *payload)
+{
+    framePosition = 0;
+
+    uint16_t checksum = 0;
+    smartPortSendByte(FPORT_RESPONSE_FRAME_LENGTH, &checksum, fportPort);
+    smartPortSendByte(FPORT_FRAME_TYPE_TELEMETRY_RESPONSE, &checksum, fportPort);
+    smartPortWriteFrameSerial(payload, fportPort, checksum);
+}
+#endif
+
+static bool checkChecksum(uint8_t *data, uint8_t length)
+{
+    uint16_t checksum = 0;
+    for (unsigned i = 0; i < length; i++) {
+        checksum = checksum + *(uint8_t *)(data + i);
+    }
+
+    checksum = (checksum & 0xff) + (checksum >> 8);
+
+    return checksum == FPORT_CRC_VALUE;
+}
+
+static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    static smartPortPayload_t payloadBuffer;
+    static bool hasTelemetryRequest = false;
+
+    uint8_t result = RX_FRAME_PENDING;
+
+    if (rxBufferReadIndex != rxBufferWriteIndex) {
+        uint8_t bufferLength = rxBuffer[rxBufferReadIndex].length;
+        uint8_t frameLength = rxBuffer[rxBufferReadIndex].data[0];
+        if (frameLength != bufferLength - 2) {
+            reportFrameError(DEBUG_FPORT_ERROR_SIZE);
+        } else {
+            if (!checkChecksum(&rxBuffer[rxBufferReadIndex].data[0], bufferLength)) {
+                reportFrameError(DEBUG_FPORT_ERROR_CHECKSUM);
+            } else {
+                fportFrame_t *frame = (fportFrame_t *)&rxBuffer[rxBufferReadIndex].data[1];
+
+                switch (frame->type) {
+                case FPORT_FRAME_TYPE_CONTROL:
+                    if (frameLength != FPORT_FRAME_PAYLOAD_LENGTH_CONTROL) {
+                        reportFrameError(DEBUG_FPORT_ERROR_TYPE_SIZE);
+                    } else {
+                        result |= sbusChannelsDecode(rxRuntimeConfig, &frame->data.controlData.channels);
+
+                        setRssiUnfiltered(scaleRange(constrain(frame->data.controlData.rssi, 0, 100), 0, 100, 0, 1024), RSSI_SOURCE_RX_PROTOCOL);
+
+                        lastRcFrameReceivedMs = millis();
+                    }
+
+                    break;
+                case FPORT_FRAME_TYPE_TELEMETRY_REQUEST:
+                    if (frameLength != FPORT_FRAME_PAYLOAD_LENGTH_TELEMETRY_REQUEST) {
+                        reportFrameError(DEBUG_FPORT_ERROR_TYPE_SIZE);
+                    } else {
+#if defined(USE_TELEMETRY_SMARTPORT)
+                        if (!telemetryEnabled) {
+                            break;
+                        }
+
+                        switch(frame->data.telemetryData.frameId) {
+                        case FPORT_FRAME_ID_NULL:
+                        case FPORT_FRAME_ID_DATA: // never used
+                            hasTelemetryRequest = true;
+
+                            break;
+                        case FPORT_FRAME_ID_READ:
+                        case FPORT_FRAME_ID_WRITE: // never used
+                            memcpy(&payloadBuffer, &frame->data.telemetryData, sizeof(smartPortPayload_t));
+                            mspPayload = &payloadBuffer;
+
+                            break;
+                        default:
+
+                           break;
+                        }
+#endif
+                    }
+
+                    break;
+                default:
+                    reportFrameError(DEBUG_FPORT_ERROR_TYPE);
+
+                    break;
+                }
+            }
+
+        }
+
+        rxBufferReadIndex = (rxBufferReadIndex + 1) % NUM_RX_BUFFERS;
+    }
+
+    if ((mspPayload || hasTelemetryRequest) && cmpTimeUs(micros(), lastTelemetryFrameReceivedUs) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) {
+        hasTelemetryRequest = false;
+
+        result = result | RX_FRAME_PROCESSING_REQUIRED;
+    }
+
+    if (lastRcFrameReceivedMs && ((millis() - lastRcFrameReceivedMs) > FPORT_MAX_TELEMETRY_AGE_MS)) {
+        setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL);
+        lastRcFrameReceivedMs = 0;
+    }
+
+    return result;
+}
+
+static bool fportProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    UNUSED(rxRuntimeConfig);
+
+#if defined(USE_TELEMETRY_SMARTPORT)
+    timeUs_t currentTimeUs = micros();
+    if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US) {
+       clearToSend = false;
+    }
+
+    if (clearToSend) {
+        DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_DELAY, currentTimeUs - lastTelemetryFrameReceivedUs);
+
+        processSmartPortTelemetry(mspPayload, &clearToSend, NULL);
+
+        if (clearToSend) {
+            smartPortWriteFrameFport(&emptySmartPortFrame);
+
+            clearToSend = false;
+        }
+    }
+
+    mspPayload = NULL;
+#endif
+
+    return true;
+}
+
+bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    static uint16_t sbusChannelData[SBUS_MAX_CHANNEL];
+    rxRuntimeConfig->channelData = sbusChannelData;
+    sbusChannelsInit(rxConfig, rxRuntimeConfig);
+
+    rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
+    rxRuntimeConfig->rxRefreshRate = 11000;
+
+    rxRuntimeConfig->rcFrameStatusFn = fportFrameStatus;
+    rxRuntimeConfig->rcProcessFrameFn = fportProcessFrame;
+
+    const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
+    if (!portConfig) {
+        return false;
+    }
+
+    fportPort = openSerialPort(portConfig->identifier,
+        FUNCTION_RX_SERIAL,
+        fportDataReceive,
+        NULL,
+        FPORT_BAUDRATE,
+        MODE_RXTX,
+        FPORT_PORT_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
+    );
+
+    if (fportPort) {
+#if defined(USE_TELEMETRY_SMARTPORT)
+        telemetryEnabled = initSmartPortTelemetryExternal(smartPortWriteFrameFport);
+#endif
+
+        if (rssiSource == RSSI_SOURCE_NONE) {
+            rssiSource = RSSI_SOURCE_RX_PROTOCOL;
+        }
+    }
+
+    return fportPort != NULL;
+}
+#endif
+
+#endif

src/main/rx/fport.h

@@ -0,0 +1,20 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+bool fportRxInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig);

src/main/rx/ibus.c

@@ -171,8 +171,10 @@ static void updateChannelData(void) {
     }
 }
 
-static uint8_t ibusFrameStatus(void)
+static uint8_t ibusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
+    UNUSED(rxRuntimeConfig);
+
     uint8_t frameStatus = RX_FRAME_PENDING;
 
     if (!ibusFrameDone) {

src/main/rx/jetiexbus.c

@@ -372,8 +372,10 @@ static void jetiExBusDataReceive(uint16_t c, void *rxCallbackData)
 
 
 // Check if it is time to read a frame from the data...
-uint8_t jetiExBusFrameStatus(void)
+static uint8_t jetiExBusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
+    UNUSED(rxRuntimeConfig);
+
     if (jetiExBusFrameState != EXBUS_STATE_RECEIVED)
         return RX_FRAME_PENDING;
 

src/main/rx/jetiexbus.h

@@ -18,5 +18,3 @@
 #pragma once
 
 bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
-uint8_t jetiExBusFrameStatus(void);
-

src/main/rx/msp.c

@@ -52,8 +52,10 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount)
     rxMspFrameDone = true;
 }
 
-uint8_t rxMspFrameStatus(void)
+static uint8_t rxMspFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
+    UNUSED(rxRuntimeConfig);
+
     if (!rxMspFrameDone) {
         return RX_FRAME_PENDING;
     }

src/main/rx/msp.h

@@ -19,6 +19,5 @@
 
 #include "rx/rx.h"
 
-uint8_t rxMspFrameStatus(void);
 void rxMspFrameReceive(uint16_t *frame, int channelCount);
 void rxMspInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);

src/main/rx/pwm.c

@@ -54,8 +54,10 @@ static uint16_t readRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan
     return channelData[channel];
 }
 
-static uint8_t pwmFrameStatus(void)
+static uint8_t pwmFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
+    UNUSED(rxRuntimeConfig);
+
     timeUs_t currentTimeUs = micros();
 
     // PWM doesn't indicate individual updates, if low level code indicates that we have valid signal
@@ -76,8 +78,10 @@ static uint8_t pwmFrameStatus(void)
     return RX_FRAME_PENDING;
 }
 
-static uint8_t ppmFrameStatus(void)
+static uint8_t ppmFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
+    UNUSED(rxRuntimeConfig);
+
     // PPM receiver counts received frames so we actually know if new data is available
     if (isPPMDataBeingReceived()) {
         for (int channel = 0; channel < MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT; channel++) {

src/main/rx/rx.c

@@ -49,6 +49,7 @@
 #include "io/serial.h"
 
 #include "rx/rx.h"
+#include "rx/fport.h"
 #include "rx/pwm.h"
 #include "rx/sbus.h"
 #include "rx/spektrum.h"
@@ -68,13 +69,25 @@
 
 const char rcChannelLetters[] = "AERT5678";
 
-uint16_t rssi = 0;                  // range: [0;1023]
+static uint16_t rssi = 0;                  // range: [0;1023]
+static timeUs_t lastMspRssiUpdateUs = 0;
+
+#define MSP_RSSI_TIMEOUT_US 1500000   // 1.5 sec
+
+rssiSource_e rssiSource;
+
+static bool rxDataProcessingRequired = false;
+static bool auxiliaryProcessingRequired = false;
 
 static bool rxSignalReceived = false;
 static bool rxFlightChannelsValid = false;
+static bool rxIsInFailsafeMode = true;
 
-static timeUs_t rxLastUpdateTimeUs = 0;
+static timeUs_t rxNextUpdateAtUs = 0;
-static timeUs_t rxLastValidFrameTimeUs = 0;
+static uint32_t needRxSignalBefore = 0;
+static uint32_t needRxSignalMaxDelayUs;
+static uint32_t suspendRxSignalUntil = 0;
+static uint8_t skipRxSamples = 0;
 
 int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
 int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
@@ -110,7 +123,7 @@ PG_RESET_TEMPLATE(rxConfig_t, rxConfig,
     .serialrx_provider = SERIALRX_PROVIDER,
     .rx_spi_protocol = RX_SPI_DEFAULT_PROTOCOL,
     .spektrum_sat_bind = 0,
-    .sbus_inversion = 1,
+    .serialrx_inverted = 0,
     .midrc = RX_MIDRC,
     .mincheck = 1100,
     .maxcheck = 1900,
@@ -150,8 +163,10 @@ static uint16_t nullReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t
     return PPM_RCVR_TIMEOUT;
 }
 
-static uint8_t nullFrameStatus(void)
+static uint8_t nullFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
+    UNUSED(rxRuntimeConfig);
+
     return RX_FRAME_PENDING;
 }
 
@@ -207,6 +222,11 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
     case SERIALRX_CRSF:
         enabled = crsfRxInit(rxConfig, rxRuntimeConfig);
         break;
+#endif
+#ifdef USE_SERIALRX_FPORT
+    case SERIALRX_FPORT:
+        enabled = fportRxInit(rxConfig, rxRuntimeConfig);
+        break;
 #endif
     default:
         enabled = false;
@@ -291,6 +311,10 @@ void rxInit(void)
             // Already configured for NONE
             break;
     }
+
+    if (rxConfig()->rssi_channel > 0) {
+        rssiSource = RSSI_SOURCE_RX_CHANNEL;
+    }
 }
 
 static uint8_t calculateChannelRemapping(const uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap)
@@ -314,10 +338,14 @@ bool rxAreFlightChannelsValid(void)
 void suspendRxSignal(void)
 {
     failsafeOnRxSuspend();
+    suspendRxSignalUntil = micros() + SKIP_RC_ON_SUSPEND_PERIOD;
+    skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
 }
 
 void resumeRxSignal(void)
 {
+    suspendRxSignalUntil = micros();
+    skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
     failsafeOnRxResume();
 }
 
@@ -325,21 +353,49 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
 {
     UNUSED(currentDeltaTime);
 
-    bool rxDataReceived = false;
+    if (rxSignalReceived) {
-    const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn();
+        if (currentTimeUs >= needRxSignalBefore) {
-    if (frameStatus & RX_FRAME_COMPLETE) {
-        rxDataReceived = true;
-        rxSignalReceived = ((frameStatus & RX_FRAME_FAILSAFE) == 0);
-        rxLastValidFrameTimeUs = currentTimeUs;
-    }
-    else {  // RX_FRAME_PENDING
-        // Check for valid signal timeout - if we are RX_FRAME_PENDING for too long assume signall loss
-        if ((currentTimeUs - rxLastValidFrameTimeUs) >= rxRuntimeConfig.rxSignalTimeout) {
             rxSignalReceived = false;
         }
     }
 
-    return rxDataReceived || ((int32_t)(currentTimeUs - rxLastUpdateTimeUs) >= 0); // data driven or 50Hz
+#if defined(USE_PWM) || defined(USE_PPM)
+    if (feature(FEATURE_RX_PPM)) {
+        if (isPPMDataBeingReceived()) {
+            rxDataProcessingRequired = true;
+            rxSignalReceived = true;
+            rxIsInFailsafeMode = false;
+            needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
+            resetPPMDataReceivedState();
+        }
+    } else if (feature(FEATURE_RX_PARALLEL_PWM)) {
+        if (isPWMDataBeingReceived()) {
+            rxDataProcessingRequired = true;
+            rxSignalReceived = true;
+            rxIsInFailsafeMode = false;
+            needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
+        }
+    } else
+#endif
+    {
+        const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig);
+        if (frameStatus & RX_FRAME_COMPLETE) {
+            rxDataProcessingRequired = true;
+            rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0;
+            rxSignalReceived = !rxIsInFailsafeMode;
+            needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
+        }
+
+        if (frameStatus & RX_FRAME_PROCESSING_REQUIRED) {
+            auxiliaryProcessingRequired = true;
+        }
+    }
+
+    if (cmpTimeUs(currentTimeUs, rxNextUpdateAtUs) > 0) {
+        rxDataProcessingRequired = true;
+    }
+
+    return rxDataProcessingRequired || auxiliaryProcessingRequired; // data driven or 50Hz
 }
 
 #define FILTERING_SAMPLE_COUNT  5
@@ -377,8 +433,29 @@ static uint16_t applyChannelFiltering(uint8_t chan, uint16_t sample)
 
 void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
 {
+    UNUSED(currentTimeUs);
+
     const timeMs_t currentTimeMs = millis();
-    rxLastUpdateTimeUs = currentTimeUs + DELAY_50_HZ;
+
+    if (auxiliaryProcessingRequired) {
+        auxiliaryProcessingRequired = !rxRuntimeConfig.rcProcessFrameFn(&rxRuntimeConfig);
+    }
+
+    if (!rxDataProcessingRequired) {
+        return false;
+    }
+
+    rxDataProcessingRequired = false;
+    rxNextUpdateAtUs = currentTimeUs + DELAY_50_HZ;
+
+    // only proceed when no more samples to skip and suspend period is over
+    if (skipRxSamples) {
+        if (currentTimeUs > suspendRxSignalUntil) {
+            skipRxSamples--;
+        }
+
+        return true;
+    }
 
     rxFlightChannelsValid = true;
     
@@ -435,6 +512,50 @@ void parseRcChannels(const char *input)
     }
 }
 
+void setRssiFiltered(uint16_t newRssi, rssiSource_e source)
+{
+    if (source != rssiSource) {
+        return;
+    }
+
+    rssi = newRssi;
+}
+
+#define RSSI_SAMPLE_COUNT 16
+#define RSSI_MAX_VALUE 1023
+
+void setRssiUnfiltered(uint16_t rssiValue, rssiSource_e source)
+{
+    if (source != rssiSource) {
+        return;
+    }
+
+    static uint16_t rssiSamples[RSSI_SAMPLE_COUNT];
+    static uint8_t rssiSampleIndex = 0;
+    static unsigned sum = 0;
+
+    sum = sum + rssiValue;
+    sum = sum - rssiSamples[rssiSampleIndex];
+    rssiSamples[rssiSampleIndex] = rssiValue;
+    rssiSampleIndex = (rssiSampleIndex + 1) % RSSI_SAMPLE_COUNT;
+
+    int16_t rssiMean = sum / RSSI_SAMPLE_COUNT;
+
+    rssi = rssiMean;
+}
+
+void setRssiMsp(uint8_t newMspRssi)
+{
+    if (rssiSource == RSSI_SOURCE_NONE) {
+        rssiSource = RSSI_SOURCE_MSP;
+    }
+
+    if (rssiSource == RSSI_SOURCE_MSP) {
+        rssi = ((uint16_t)newMspRssi) << 2;
+        lastMspRssiUpdateUs = micros();
+    }
+}
+
 #ifdef USE_RX_ELERES
 static bool updateRSSIeleres(uint32_t currentTime)
 {
@@ -491,14 +612,25 @@ void updateRSSI(timeUs_t currentTimeUs)
     bool rssiUpdated = false;
 
     // Read RSSI
-    if (rxConfig()->rssi_channel > 0) {
+    switch (rssiSource) {
-        rssiUpdated = updateRSSIPWM();
+    case RSSI_SOURCE_RX_CHANNEL:
-    } else if (feature(FEATURE_RSSI_ADC)) {
+        updateRSSIPWM();
-        rssiUpdated = updateRSSIADC(currentTimeUs);
+        break;
+    case RSSI_SOURCE_ADC:
+        updateRSSIADC(currentTimeUs);
+        break;
+    case RSSI_SOURCE_MSP:
+        if (cmpTimeUs(micros(), lastMspRssiUpdateUs) > MSP_RSSI_TIMEOUT_US) {
+            rssi = 0;
+        }
+        break;
+    default:
 #ifdef USE_RX_ELERES
-    } else if (rxConfig()->receiverType == RX_TYPE_SPI && rxConfig()->rx_spi_protocol == RFM22_ELERES) {
+        if (rxConfig()->receiverType == RX_TYPE_SPI && rxConfig()->rx_spi_protocol == RFM22_ELERES) {
-        rssiUpdated = updateRSSIeleres(currentTimeUs);
+            rssiUpdated = updateRSSIeleres(currentTimeUs);
+        }
 #endif
+        break;
     }
 
     if (rssiUpdated) {
@@ -508,13 +640,15 @@ void updateRSSI(timeUs_t currentTimeUs)
         }
 
         // Apply scaling
-        debug[0] = rssi;
-        debug[1] = rxConfig()->rssi_scale;
         rssi = constrain((uint32_t)rssi * rxConfig()->rssi_scale / 100, 0, 1023);
-        debug[2] = rssi;
     }
 }
 
+uint16_t getRSSI(void)
+{
+    return rssi;
+}
+
 uint16_t rxGetRefreshRate(void)
 {
     return rxRuntimeConfig.rxRefreshRate;

src/main/rx/rx.h

@@ -51,7 +51,8 @@
 typedef enum {
     RX_FRAME_PENDING = 0,               // No new data available from receiver
     RX_FRAME_COMPLETE = (1 << 0),       // There is new data available
-    RX_FRAME_FAILSAFE = (1 << 1)        // Receiver detected loss of RC link. Only valid when RX_FRAME_COMPLETE is set as well
+    RX_FRAME_FAILSAFE = (1 << 1),       // Receiver detected loss of RC link. Only valid when RX_FRAME_COMPLETE is set as well
+    RX_FRAME_PROCESSING_REQUIRED = (1 << 2),
 } rxFrameState_e;
 
 typedef enum {
@@ -74,7 +75,8 @@ typedef enum {
     SERIALRX_XBUS_MODE_B_RJ01 = 6,
     SERIALRX_IBUS = 7,
     SERIALRX_JETIEXBUS = 8,
-    SERIALRX_CRSF = 9
+    SERIALRX_CRSF = 9,
+    SERIALRX_FPORT = 10,
 } rxSerialReceiverType_e;
 
 #define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT          16
@@ -110,7 +112,7 @@ typedef struct rxConfig_s {
     uint8_t receiverType;                   // RC receiver type (rxReceiverType_e enum)
     uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS];  // mapping of radio channels to internal RPYTA+ order
     uint8_t serialrx_provider;              // Type of UART-based receiver (rxSerialReceiverType_e enum). Only used if receiverType is RX_TYPE_SERIAL
-    uint8_t sbus_inversion;                 // default sbus (Futaba, FrSKY) is inverted. Support for uninverted OpenLRS (and modified FrSKY) receivers.
+    uint8_t serialrx_inverted;              // Flip the default inversion of the protocol - e.g. sbus (Futaba, FrSKY) is inverted if this is false, uninverted if it's true. Support for uninverted OpenLRS (and modified FrSKY) receivers.
     uint8_t halfDuplex;                     // allow rx to operate in half duplex mode on F4, ignored for F1 and F3.
     uint8_t rx_spi_protocol;                // type of SPI receiver protocol (rx_spi_protocol_e enum). Only used if receiverType is RX_TYPE_SPI
     uint32_t rx_spi_id;
@@ -132,9 +134,10 @@ PG_DECLARE(rxConfig_t, rxConfig);
 
 #define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0]))
 
-struct rxRuntimeConfig_s;
+typedef struct rxRuntimeConfig_s rxRuntimeConfig_t;
-typedef uint16_t (*rcReadRawDataFnPtr)(const struct rxRuntimeConfig_s *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
+typedef uint16_t (*rcReadRawDataFnPtr)(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
-typedef uint8_t (*rcFrameStatusFnPtr)(void);
+typedef uint8_t (*rcFrameStatusFnPtr)(rxRuntimeConfig_t *rxRuntimeConfig);
+typedef bool (*rcProcessFrameFnPtr)(const rxRuntimeConfig_t *rxRuntimeConfig);
 
 typedef struct rxRuntimeConfig_s {
     uint8_t channelCount;                  // number of rc channels as reported by current input driver
@@ -143,10 +146,22 @@ typedef struct rxRuntimeConfig_s {
     bool requireFiltering;
     rcReadRawDataFnPtr rcReadRawFn;
     rcFrameStatusFnPtr rcFrameStatusFn;
+    rcProcessFrameFnPtr rcProcessFrameFn;
+    uint16_t *channelData;
+    void *frameData;
 } rxRuntimeConfig_t;
 
+typedef enum {
+    RSSI_SOURCE_NONE = 0,
+    RSSI_SOURCE_ADC,
+    RSSI_SOURCE_RX_CHANNEL,
+    RSSI_SOURCE_RX_PROTOCOL,
+    RSSI_SOURCE_MSP,
+} rssiSource_e;
+
+extern rssiSource_e rssiSource;
+
 extern rxRuntimeConfig_t rxRuntimeConfig; //!!TODO remove this extern, only needed once for channelCount
-extern uint16_t rssi;
 
 void rxInit(void);
 bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime);
@@ -156,7 +171,12 @@ void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs);
 
 void parseRcChannels(const char *input);
 
+void setRssiFiltered(uint16_t newRssi, rssiSource_e source);
+void setRssiUnfiltered(uint16_t rssiValue, rssiSource_e source);
+void setRssiMsp(uint8_t newMspRssi);
 void updateRSSI(timeUs_t currentTimeUs);
+uint16_t getRSSI(void);
+
 void resetAllRxChannelRangeConfigurations(void);
 
 void suspendRxSignal(void);

src/main/rx/sbus.c

@@ -21,19 +21,23 @@
 
 #include "platform.h"
 
-#ifdef USE_SERIALRX_SBUS
+#ifdef USE_SERIAL_RX
+
+#include "build/debug.h"
 
 #include "common/utils.h"
 
-#include "drivers/serial.h"
 #include "drivers/time.h"
 
 #include "io/serial.h"
 
+#ifdef USE_TELEMETRY
+#include "telemetry/telemetry.h"
+#endif
+
 #include "rx/rx.h"
 #include "rx/sbus.h"
-
+#include "rx/sbus_channels.h"
-#include "telemetry/telemetry.h"
 
 /*
  * Observations
@@ -49,58 +53,32 @@
 
 #define SBUS_TIME_NEEDED_PER_FRAME 3000
 
-#ifndef CJMCU
-//#define DEBUG_SBUS_PACKETS
-#endif
-
-#ifdef DEBUG_SBUS_PACKETS
-static uint16_t sbusStateFlags = 0;
-
 #define SBUS_STATE_FAILSAFE (1 << 0)
 #define SBUS_STATE_SIGNALLOSS (1 << 1)
 
-#endif
+#define SBUS_FRAME_SIZE (SBUS_CHANNEL_DATA_LENGTH + 2)
-
-#define SBUS_MAX_CHANNEL 18
-#define SBUS_FRAME_SIZE 25
 
 #define SBUS_FRAME_BEGIN_BYTE 0x0F
 
 #define SBUS_BAUDRATE 100000
+
+#if !defined(SBUS_PORT_OPTIONS)
 #define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN)
+#endif
 
 #define SBUS_DIGITAL_CHANNEL_MIN 173
 #define SBUS_DIGITAL_CHANNEL_MAX 1812
 
-static bool sbusFrameDone = false;
+enum {
+    DEBUG_SBUS_FRAME_FLAGS = 0,
+    DEBUG_SBUS_STATE_FLAGS,
+    DEBUG_SBUS_FRAME_TIME,
+};
 
-static uint32_t sbusChannelData[SBUS_MAX_CHANNEL];
-
-#define SBUS_FLAG_CHANNEL_17        (1 << 0)
-#define SBUS_FLAG_CHANNEL_18        (1 << 1)
-#define SBUS_FLAG_SIGNAL_LOSS       (1 << 2)
-#define SBUS_FLAG_FAILSAFE_ACTIVE   (1 << 3)
 
 struct sbusFrame_s {
     uint8_t syncByte;
-    // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
+    sbusChannels_t channels;
-    unsigned int chan0 : 11;
-    unsigned int chan1 : 11;
-    unsigned int chan2 : 11;
-    unsigned int chan3 : 11;
-    unsigned int chan4 : 11;
-    unsigned int chan5 : 11;
-    unsigned int chan6 : 11;
-    unsigned int chan7 : 11;
-    unsigned int chan8 : 11;
-    unsigned int chan9 : 11;
-    unsigned int chan10 : 11;
-    unsigned int chan11 : 11;
-    unsigned int chan12 : 11;
-    unsigned int chan13 : 11;
-    unsigned int chan14 : 11;
-    unsigned int chan15 : 11;
-    uint8_t flags;
     /**
      * The endByte is 0x00 on FrSky and some futaba RX's, on Some SBUS2 RX's the value indicates the telemetry byte that is sent after every 4th sbus frame.
      *
@@ -111,130 +89,87 @@ struct sbusFrame_s {
     uint8_t endByte;
 } __attribute__ ((__packed__));
 
-typedef union {
+typedef union sbusFrame_u {
     uint8_t bytes[SBUS_FRAME_SIZE];
     struct sbusFrame_s frame;
 } sbusFrame_t;
 
-static sbusFrame_t sbusFrame;
+typedef struct sbusFrameData_s {
+    sbusFrame_t frame;
+    uint32_t startAtUs;
+    uint16_t stateFlags;
+    uint8_t position;
+    bool done;
+} sbusFrameData_t;
+
 
 // Receive ISR callback
-static void sbusDataReceive(uint16_t c, void *rxCallbackData)
+static void sbusDataReceive(uint16_t c, void *data)
 {
-    UNUSED(rxCallbackData);
+    sbusFrameData_t *sbusFrameData = data;
 
-    static uint8_t sbusFramePosition = 0;
+    const uint32_t nowUs = micros();
-    static uint32_t sbusFrameStartAt = 0;
-    timeUs_t now = micros();
 
-    timeDelta_t sbusFrameTime = cmpTimeUs(now, sbusFrameStartAt);
+    const int32_t sbusFrameTime = nowUs - sbusFrameData->startAtUs;
 
     if (sbusFrameTime > (long)(SBUS_TIME_NEEDED_PER_FRAME + 500)) {
-        sbusFramePosition = 0;
+        sbusFrameData->position = 0;
     }
 
-    if (sbusFramePosition == 0) {
+    if (sbusFrameData->position == 0) {
         if (c != SBUS_FRAME_BEGIN_BYTE) {
             return;
         }
-        sbusFrameStartAt = now;
+        sbusFrameData->startAtUs = nowUs;
     }
 
-    if (sbusFramePosition < SBUS_FRAME_SIZE) {
+    if (sbusFrameData->position < SBUS_FRAME_SIZE) {
-        sbusFrame.bytes[sbusFramePosition++] = (uint8_t)c;
+        sbusFrameData->frame.bytes[sbusFrameData->position++] = (uint8_t)c;
-        if (sbusFramePosition == SBUS_FRAME_SIZE) {
+        if (sbusFrameData->position < SBUS_FRAME_SIZE) {
-            // endByte currently ignored
+            sbusFrameData->done = false;
-            sbusFrameDone = true;
-#ifdef DEBUG_SBUS_PACKETS
-            debug[2] = sbusFrameTime;
-#endif
         } else {
-            sbusFrameDone = false;
+            sbusFrameData->done = true;
+            DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_TIME, sbusFrameTime);
         }
     }
 }
 
-uint8_t sbusFrameStatus(void)
+static uint8_t sbusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
-    if (!sbusFrameDone) {
+    sbusFrameData_t *sbusFrameData = rxRuntimeConfig->frameData;
+    if (!sbusFrameData->done) {
         return RX_FRAME_PENDING;
     }
-    sbusFrameDone = false;
+    sbusFrameData->done = false;
 
-#ifdef DEBUG_SBUS_PACKETS
+    DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_FLAGS, sbusFrameData->frame.frame.channels.flags);
-    sbusStateFlags = 0;
-    debug[1] = sbusFrame.frame.flags;
-#endif
 
-    sbusChannelData[0] = sbusFrame.frame.chan0;
+    if (sbusFrameData->frame.frame.channels.flags & SBUS_FLAG_SIGNAL_LOSS) {
-    sbusChannelData[1] = sbusFrame.frame.chan1;
+        sbusFrameData->stateFlags |= SBUS_STATE_SIGNALLOSS;
-    sbusChannelData[2] = sbusFrame.frame.chan2;
+        DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusFrameData->stateFlags);
-    sbusChannelData[3] = sbusFrame.frame.chan3;
+    }
-    sbusChannelData[4] = sbusFrame.frame.chan4;
+    if (sbusFrameData->frame.frame.channels.flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
-    sbusChannelData[5] = sbusFrame.frame.chan5;
+        sbusFrameData->stateFlags |= SBUS_STATE_FAILSAFE;
-    sbusChannelData[6] = sbusFrame.frame.chan6;
+        DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusFrameData->stateFlags);
-    sbusChannelData[7] = sbusFrame.frame.chan7;
-    sbusChannelData[8] = sbusFrame.frame.chan8;
-    sbusChannelData[9] = sbusFrame.frame.chan9;
-    sbusChannelData[10] = sbusFrame.frame.chan10;
-    sbusChannelData[11] = sbusFrame.frame.chan11;
-    sbusChannelData[12] = sbusFrame.frame.chan12;
-    sbusChannelData[13] = sbusFrame.frame.chan13;
-    sbusChannelData[14] = sbusFrame.frame.chan14;
-    sbusChannelData[15] = sbusFrame.frame.chan15;
-
-    if (sbusFrame.frame.flags & SBUS_FLAG_CHANNEL_17) {
-        sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MAX;
-    } else {
-        sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MIN;
     }
 
-    if (sbusFrame.frame.flags & SBUS_FLAG_CHANNEL_18) {
+    DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusFrameData->stateFlags);
-        sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MAX;
-    } else {
-        sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MIN;
-    }
 
-    if (sbusFrame.frame.flags & SBUS_FLAG_SIGNAL_LOSS) {
+    return sbusChannelsDecode(rxRuntimeConfig, &sbusFrameData->frame.frame.channels);
-#ifdef DEBUG_SBUS_PACKETS
-        sbusStateFlags |= SBUS_STATE_SIGNALLOSS;
-        debug[0] = sbusStateFlags;
-#endif
-    }
-    if (sbusFrame.frame.flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
-        // internal failsafe enabled and rx failsafe flag set
-#ifdef DEBUG_SBUS_PACKETS
-        sbusStateFlags |= SBUS_STATE_FAILSAFE;
-        debug[0] = sbusStateFlags;
-#endif
-        // RX *should* still be sending valid channel data, so use it.
-        return RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE;
-    }
-
-#ifdef DEBUG_SBUS_PACKETS
-    debug[0] = sbusStateFlags;
-#endif
-    return RX_FRAME_COMPLETE;
-}
-
-static uint16_t sbusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
-{
-    UNUSED(rxRuntimeConfig);
-    // Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R
-    // http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D
-    return (0.625f * sbusChannelData[chan]) + 880;
 }
 
 bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 {
-    for (int b = 0; b < SBUS_MAX_CHANNEL; b++) {
+    static uint16_t sbusChannelData[SBUS_MAX_CHANNEL];
-        sbusChannelData[b] = (16 * rxConfig->midrc) / 10 - 1408;
+    static sbusFrameData_t sbusFrameData;
-    }
+
+    rxRuntimeConfig->channelData = sbusChannelData;
+    rxRuntimeConfig->frameData = &sbusFrameData;
+    sbusChannelsInit(rxConfig, rxRuntimeConfig);
 
     rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
     rxRuntimeConfig->rxRefreshRate = 11000;
 
-    rxRuntimeConfig->rcReadRawFn = sbusReadRawRC;
     rxRuntimeConfig->rcFrameStatusFn = sbusFrameStatus;
 
     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
@@ -251,10 +186,10 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
     serialPort_t *sBusPort = openSerialPort(portConfig->identifier,
         FUNCTION_RX_SERIAL,
         sbusDataReceive,
-        NULL,
+        &sbusFrameData,
         SBUS_BAUDRATE,
         portShared ? MODE_RXTX : MODE_RX,
-        SBUS_PORT_OPTIONS | (rxConfig->sbus_inversion ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
+        SBUS_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0 : SERIAL_INVERTED) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
         );
 
 #ifdef USE_TELEMETRY
@@ -265,4 +200,4 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 
     return sBusPort != NULL;
 }
-#endif // USE_SERIALRX_SBUS
+#endif

src/main/rx/sbus.h

@@ -17,5 +17,4 @@
 
 #pragma once
 
-uint8_t sbusFrameStatus(void);
 bool sbusInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig);

src/main/rx/sbus_channels.c

@@ -0,0 +1,96 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include "platform.h"
+
+#ifdef USE_SERIAL_RX
+
+#include "common/utils.h"
+
+#include "rx/rx.h"
+#include "rx/sbus_channels.h"
+
+#define DEBUG_SBUS_FRAME_INTERVAL 3
+
+#define SBUS_FLAG_CHANNEL_17        (1 << 0)
+#define SBUS_FLAG_CHANNEL_18        (1 << 1)
+
+#define SBUS_DIGITAL_CHANNEL_MIN 173
+#define SBUS_DIGITAL_CHANNEL_MAX 1812
+
+uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels)
+{
+    uint16_t *sbusChannelData = rxRuntimeConfig->channelData;
+    sbusChannelData[0] = channels->chan0;
+    sbusChannelData[1] = channels->chan1;
+    sbusChannelData[2] = channels->chan2;
+    sbusChannelData[3] = channels->chan3;
+    sbusChannelData[4] = channels->chan4;
+    sbusChannelData[5] = channels->chan5;
+    sbusChannelData[6] = channels->chan6;
+    sbusChannelData[7] = channels->chan7;
+    sbusChannelData[8] = channels->chan8;
+    sbusChannelData[9] = channels->chan9;
+    sbusChannelData[10] = channels->chan10;
+    sbusChannelData[11] = channels->chan11;
+    sbusChannelData[12] = channels->chan12;
+    sbusChannelData[13] = channels->chan13;
+    sbusChannelData[14] = channels->chan14;
+    sbusChannelData[15] = channels->chan15;
+
+    if (channels->flags & SBUS_FLAG_CHANNEL_17) {
+        sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MAX;
+    } else {
+        sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MIN;
+    }
+
+    if (channels->flags & SBUS_FLAG_CHANNEL_18) {
+        sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MAX;
+    } else {
+        sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MIN;
+    }
+
+    if (channels->flags & SBUS_FLAG_SIGNAL_LOSS) {
+    }
+    if (channels->flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
+        // internal failsafe enabled and rx failsafe flag set
+        // RX *should* still be sending valid channel data, so use it.
+        return RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE;
+    }
+
+    return RX_FRAME_COMPLETE;
+}
+
+static uint16_t sbusChannelsReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
+{
+    // Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R
+    // http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D
+    return (5 * rxRuntimeConfig->channelData[chan] / 8) + 880;
+}
+
+void sbusChannelsInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    rxRuntimeConfig->rcReadRawFn = sbusChannelsReadRawRC;
+    for (int b = 0; b < SBUS_MAX_CHANNEL; b++) {
+        rxRuntimeConfig->channelData[b] = (16 * rxConfig->midrc) / 10 - 1408;
+    }
+}
+#endif

src/main/rx/sbus_channels.h

@@ -0,0 +1,52 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#define SBUS_MAX_CHANNEL 18
+
+#define SBUS_FLAG_SIGNAL_LOSS       (1 << 2)
+#define SBUS_FLAG_FAILSAFE_ACTIVE   (1 << 3)
+
+typedef struct sbusChannels_s {
+    // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
+    unsigned int chan0 : 11;
+    unsigned int chan1 : 11;
+    unsigned int chan2 : 11;
+    unsigned int chan3 : 11;
+    unsigned int chan4 : 11;
+    unsigned int chan5 : 11;
+    unsigned int chan6 : 11;
+    unsigned int chan7 : 11;
+    unsigned int chan8 : 11;
+    unsigned int chan9 : 11;
+    unsigned int chan10 : 11;
+    unsigned int chan11 : 11;
+    unsigned int chan12 : 11;
+    unsigned int chan13 : 11;
+    unsigned int chan14 : 11;
+    unsigned int chan15 : 11;
+    uint8_t flags;
+} __attribute__((__packed__)) sbusChannels_t;
+
+#define SBUS_CHANNEL_DATA_LENGTH sizeof(sbusChannels_t)
+
+uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels);
+
+void sbusChannelsInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);

src/main/rx/spektrum.c

@@ -112,8 +112,10 @@ static void spektrumDataReceive(uint16_t c, void *rxCallbackData)
 
 static uint32_t spekChannelData[SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT];
 
-uint8_t spektrumFrameStatus(void)
+static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
+    UNUSED(rxRuntimeConfig);
+
     if (!rcFrameComplete) {
         return RX_FRAME_PENDING;
     }

src/main/rx/spektrum.h

@@ -20,6 +20,5 @@
 #define SPEKTRUM_SAT_BIND_DISABLED 0
 #define SPEKTRUM_SAT_BIND_MAX 10
 
-uint8_t spektrumFrameStatus(void);
 void spektrumBind(rxConfig_t *rxConfig);
 bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);

src/main/rx/sumd.c

@@ -114,8 +114,10 @@ static void sumdDataReceive(uint16_t c, void *rxCallbackData)
 #define SUMD_FRAME_STATE_OK 0x01
 #define SUMD_FRAME_STATE_FAILSAFE 0x81
 
-uint8_t sumdFrameStatus(void)
+static uint8_t sumdFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
+    UNUSED(rxRuntimeConfig);
+
     uint8_t channelIndex;
 
     uint8_t frameStatus = RX_FRAME_PENDING;

src/main/rx/sumd.h

@@ -17,5 +17,4 @@
 
 #pragma once
 
-uint8_t sumdFrameStatus(void);
 bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);

src/main/rx/sumh.c

@@ -82,8 +82,10 @@ static void sumhDataReceive(uint16_t c, void *rxCallbackData)
     }
 }
 
-uint8_t sumhFrameStatus(void)
+uint8_t sumhFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
+    UNUSED(rxRuntimeConfig);
+
     uint8_t channelIndex;
 
     if (!sumhFrameDone) {

src/main/rx/sumh.h

@@ -17,5 +17,4 @@
 
 #pragma once
 
-uint8_t sumhFrameStatus(void);
 bool sumhInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);

src/main/rx/uib_rx.c

@@ -81,8 +81,10 @@ static uint16_t rxUIBReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint
     }
 }
 
-static uint8_t rxUIBFrameStatus(void)
+static uint8_t rxUIBFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
+    UNUSED(rxRuntimeConfig);
+
     if (!uavInterconnectBusIsInitialized()) {
         return RX_FRAME_FAILSAFE;
     }

src/main/rx/xbus.c

@@ -255,8 +255,10 @@ static void xBusDataReceive(uint16_t c, void *rxCallbackData)
 }
 
 // Indicate time to read a frame from the data...
-uint8_t xBusFrameStatus(void)
+static uint8_t xBusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
+    UNUSED(rxRuntimeConfig);
+
     if (!xBusFrameReceived) {
         return RX_FRAME_PENDING;
     }

src/main/rx/xbus.h

@@ -17,5 +17,4 @@
 
 #pragma once
 
-bool xBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
+bool xBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
-uint8_t xBusFrameStatus(void);

src/main/sensors/battery.c

@@ -234,6 +234,11 @@ bool batteryUsesCapacityThresholds(void)
     return batteryUseCapacityThresholds;
 }
 
+bool isBatteryVoltageConfigured(void)
+{
+    return feature(FEATURE_VBAT);
+}
+
 uint16_t getBatteryVoltage(void)
 {
     return vbat;
@@ -267,6 +272,11 @@ uint32_t getBatteryRemainingCapacity(void)
     return batteryRemainingCapacity;
 }
 
+bool isAmperageConfigured(void)
+{
+    return batteryConfig()->current.type != CURRENT_SENSOR_NONE;
+}
+
 int32_t getAmperage(void)
 {
     return amperage;

src/main/sensors/battery.h

@@ -81,6 +81,7 @@ bool batteryUsesCapacityThresholds(void);
 void batteryUpdate(uint32_t vbatTimeDelta);
 void batteryInit(void);
 
+bool isBatteryVoltageConfigured(void);
 uint16_t getBatteryVoltage(void);
 uint16_t getBatteryVoltageLatestADC(void);
 uint16_t getBatteryWarningVoltage(void);
@@ -88,6 +89,7 @@ uint8_t getBatteryCellCount(void);
 uint16_t getBatteryAverageCellVoltage(void);
 uint32_t getBatteryRemainingCapacity(void);
 
+bool isAmperageConfigured(void);
 int32_t getAmperage(void);
 int32_t getAmperageLatestADC(void);
 int32_t getPower(void);

src/main/target/ALIENFLIGHTF4/config.c

@@ -70,7 +70,6 @@ void targetConfiguration(void)
         rxConfigMutable()->spektrum_sat_bind_autoreset = 1;
     } else {
         rxConfigMutable()->serialrx_provider = SERIALRX_SBUS;
-        rxConfigMutable()->sbus_inversion = 0;
         serialConfigMutable()->portConfigs[3].functionMask = FUNCTION_TELEMETRY_FRSKY;
         telemetryConfigMutable()->telemetry_inversion = 0;
         featureConfigMutable()->enabledFeatures |= (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY);

src/main/target/ALIENFLIGHTNGF7/config.c

@@ -72,7 +72,6 @@ void targetConfiguration(void)
         rxConfigMutable()->spektrum_sat_bind_autoreset = 1;
     } else {
         rxConfigMutable()->serialrx_provider = SERIALRX_SBUS;
-        rxConfigMutable()->sbus_inversion = 0;
         serialConfigMutable()->portConfigs[3].functionMask = FUNCTION_TELEMETRY_FRSKY;
         telemetryConfigMutable()->telemetry_inversion = 0;
         featureConfigMutable()->enabledFeatures |= (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY);

src/main/target/SPRACINGF3NEO/config.c

@@ -24,8 +24,6 @@
 
 #include "io/serial.h"
 
-#include "rx/rx.h"
-
 #include "sensors/sensors.h"
 #include "sensors/compass.h"
 #include "sensors/barometer.h"
@@ -36,7 +34,6 @@ void targetConfiguration(void)
 {
     barometerConfigMutable()->baro_hardware = BARO_AUTODETECT;
     compassConfigMutable()->mag_hardware = MAG_AUTODETECT;
-    rxConfigMutable()->sbus_inversion = 1;
     serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; // So Bluetooth users don't have to change anything.
     serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT;
     serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(GPS_UART)].functionMask = FUNCTION_GPS;

src/main/target/SPRACINGF4EVO/config.c

@@ -23,7 +23,6 @@
 #ifdef TARGET_CONFIG
 
 #include "io/serial.h"
-#include "rx/rx.h"
 #include "sensors/barometer.h"
 #include "telemetry/telemetry.h"
 
@@ -31,7 +30,6 @@
 void targetConfiguration(void)
 {
     barometerConfigMutable()->baro_hardware = BARO_BMP280;
-    rxConfigMutable()->sbus_inversion = 1;
     serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; // So SPRacingF3OSD users don't have to change anything.
     serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT;
     telemetryConfigMutable()->telemetry_inversion = true;

src/main/target/common.h

@@ -30,6 +30,7 @@
 #define USE_SERIALRX_SPEKTRUM   // Cheap and fairly common protocol
 #define USE_SERIALRX_SBUS       // Very common protocol
 #define USE_SERIALRX_IBUS       // Cheap FlySky & Turnigy receivers
+#define USE_SERIALRX_FPORT
 
 #define COMMON_DEFAULT_FEATURES (FEATURE_TX_PROF_SEL)
 
@@ -91,6 +92,7 @@
 #define USE_TELEMETRY_MAVLINK
 #define USE_TELEMETRY_SMARTPORT
 #define USE_TELEMETRY_CRSF
+#define USE_MSP_OVER_TELEMETRY
 // These are rather exotic serial protocols
 #define USE_RX_MSP
 #define USE_SERIALRX_SUMD

src/main/telemetry/frsky.c

@@ -395,7 +395,7 @@ static void sendVoltageAmp(void)
     } else {
         uint16_t voltage = (vbat * 11) / 21;
         uint16_t vfasVoltage;
-        if (telemetryConfig()->frsky_vfas_cell_voltage) {
+        if (telemetryConfig()->report_cell_voltage) {
             vfasVoltage = voltage / getBatteryCellCount();
         } else {
             vfasVoltage = voltage;

src/main/telemetry/ltm.c

@@ -81,7 +81,6 @@
 #define LTM_CYCLETIME   100
 #define LTM_SCHEDULE_SIZE (1000/LTM_CYCLETIME)
 
-extern uint16_t rssi;           // FIXME dependency on mw.c
 static serialPort_t *ltmPort;
 static serialPortConfig_t *portConfig;
 static bool ltmEnabled;
@@ -187,7 +186,7 @@ void ltm_sframe(sbuf_t *dst)
     sbufWriteU8(dst, 'S');
     sbufWriteU16(dst, getBatteryVoltage() * 10);    //vbat converted to mv
     sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF));    // current mAh (65535 mAh max)
-    sbufWriteU8(dst, (uint8_t)((rssi * 254) / 1023));        // scaled RSSI (uchar)
+    sbufWriteU8(dst, (uint8_t)((getRSSI() * 254) / 1023));        // scaled RSSI (uchar)
 #if defined(USE_PITOT)
     sbufWriteU8(dst, sensors(SENSOR_PITOT) ? pitot.airSpeed / 100.0f : 0);  // in m/s
 #else

src/main/telemetry/mavlink.c

@@ -83,8 +83,6 @@
 #define TELEMETRY_MAVLINK_MAXRATE       50
 #define TELEMETRY_MAVLINK_DELAY         ((1000 * 1000) / TELEMETRY_MAVLINK_MAXRATE)
 
-extern uint16_t rssi; // FIXME dependency on mw.c
-
 static serialPort_t *mavlinkPort = NULL;
 static serialPortConfig_t *portConfig;
 
@@ -272,7 +270,7 @@ void mavlinkSendRCChannelsAndRSSI(void)
         // chan8_raw RC channel 8 value, in microseconds
         (rxRuntimeConfig.channelCount >= 8) ? rcData[7] : 0,
         // rssi Receive signal strength indicator, 0: 0%, 255: 100%
-        scaleRange(rssi, 0, 1023, 0, 255));
+        scaleRange(getRSSI(), 0, 1023, 0, 255));
 
     mavlinkSendMessage();
 }

src/main/telemetry/msp_shared.c

@@ -0,0 +1,225 @@
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+#if defined(USE_MSP_OVER_TELEMETRY)
+
+#include "build/build_config.h"
+
+#include "common/utils.h"
+
+#include "fc/fc_msp.h"
+
+#include "msp/msp.h"
+
+#include "telemetry/crsf.h"
+#include "telemetry/msp_shared.h"
+#include "telemetry/smartport.h"
+
+#define TELEMETRY_MSP_VERSION    1
+#define TELEMETRY_MSP_VER_SHIFT  5
+#define TELEMETRY_MSP_VER_MASK   (0x7 << TELEMETRY_MSP_VER_SHIFT)
+#define TELEMETRY_MSP_ERROR_FLAG (1 << 5)
+#define TELEMETRY_MSP_START_FLAG (1 << 4)
+#define TELEMETRY_MSP_SEQ_MASK   0x0F
+#define TELEMETRY_MSP_RES_ERROR (-10)
+
+enum {
+    TELEMETRY_MSP_VER_MISMATCH=0,
+    TELEMETRY_MSP_CRC_ERROR=1,
+    TELEMETRY_MSP_ERROR=2
+};
+
+STATIC_UNIT_TESTED uint8_t checksum = 0;
+STATIC_UNIT_TESTED mspPackage_t mspPackage;
+static mspRxBuffer_t mspRxBuffer;
+static mspTxBuffer_t mspTxBuffer;
+static mspPacket_t mspRxPacket;
+static mspPacket_t mspTxPacket;
+
+void initSharedMsp(void)
+{
+    mspPackage.requestBuffer = (uint8_t *)&mspRxBuffer;
+    mspPackage.requestPacket = &mspRxPacket;
+    mspPackage.requestPacket->buf.ptr = mspPackage.requestBuffer;
+    mspPackage.requestPacket->buf.end = mspPackage.requestBuffer;
+
+    mspPackage.responseBuffer = (uint8_t *)&mspTxBuffer;
+    mspPackage.responsePacket = &mspTxPacket;
+    mspPackage.responsePacket->buf.ptr = mspPackage.responseBuffer;
+    mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
+}
+
+static void processMspPacket(void)
+{
+    mspPackage.responsePacket->cmd = 0;
+    mspPackage.responsePacket->result = 0;
+    mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
+
+    mspPostProcessFnPtr mspPostProcessFn = NULL;
+    if (mspFcProcessCommand(mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) {
+        sbufWriteU8(&mspPackage.responsePacket->buf, TELEMETRY_MSP_ERROR);
+    }
+    if (mspPostProcessFn) {
+        mspPostProcessFn(NULL);
+    }
+
+    sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
+}
+
+void sendMspErrorResponse(uint8_t error, int16_t cmd)
+{
+    mspPackage.responsePacket->cmd = cmd;
+    mspPackage.responsePacket->result = 0;
+    mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
+
+    sbufWriteU8(&mspPackage.responsePacket->buf, error);
+    mspPackage.responsePacket->result = TELEMETRY_MSP_RES_ERROR;
+    sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
+}
+
+bool handleMspFrame(uint8_t *frameStart, int frameLength)
+{
+    static uint8_t mspStarted = 0;
+    static uint8_t lastSeq = 0;
+
+    if (sbufBytesRemaining(&mspPackage.responsePacket->buf) > 0) {
+        mspStarted = 0;
+    }
+
+    if (mspStarted == 0) {
+        initSharedMsp();
+    }
+
+    mspPacket_t *packet = mspPackage.requestPacket;
+    sbuf_t *frameBuf = sbufInit(&mspPackage.requestFrame, frameStart, frameStart + (uint8_t)frameLength);
+    sbuf_t *rxBuf = &mspPackage.requestPacket->buf;
+    const uint8_t header = sbufReadU8(frameBuf);
+    const uint8_t seqNumber = header & TELEMETRY_MSP_SEQ_MASK;
+    const uint8_t version = (header & TELEMETRY_MSP_VER_MASK) >> TELEMETRY_MSP_VER_SHIFT;
+
+    if (version != TELEMETRY_MSP_VERSION) {
+        sendMspErrorResponse(TELEMETRY_MSP_VER_MISMATCH, 0);
+        return true;
+    }
+
+    if (header & TELEMETRY_MSP_START_FLAG) {
+        // first packet in sequence
+        uint8_t mspPayloadSize = sbufReadU8(frameBuf);
+
+        packet->cmd = sbufReadU8(frameBuf);
+        packet->result = 0;
+        packet->buf.ptr = mspPackage.requestBuffer;
+        packet->buf.end = mspPackage.requestBuffer + mspPayloadSize;
+
+        checksum = mspPayloadSize ^ packet->cmd;
+        mspStarted = 1;
+    } else if (!mspStarted) {
+        // no start packet yet, throw this one away
+        return false;
+    } else if (((lastSeq + 1) & TELEMETRY_MSP_SEQ_MASK) != seqNumber) {
+        // packet loss detected!
+        mspStarted = 0;
+        return false;
+    }
+
+    const uint8_t bufferBytesRemaining = sbufBytesRemaining(rxBuf);
+    const uint8_t frameBytesRemaining = sbufBytesRemaining(frameBuf);
+    uint8_t payload[frameBytesRemaining];
+
+    if (bufferBytesRemaining >= frameBytesRemaining) {
+        sbufReadData(frameBuf, payload, frameBytesRemaining);
+        sbufAdvance(frameBuf, frameBytesRemaining);
+        sbufWriteData(rxBuf, payload, frameBytesRemaining);
+        lastSeq = seqNumber;
+
+        return false;
+    } else {
+        sbufReadData(frameBuf, payload, bufferBytesRemaining);
+        sbufAdvance(frameBuf, bufferBytesRemaining);
+        sbufWriteData(rxBuf, payload, bufferBytesRemaining);
+        sbufSwitchToReader(rxBuf, mspPackage.requestBuffer);
+        while (sbufBytesRemaining(rxBuf)) {
+            checksum ^= sbufReadU8(rxBuf);
+        }
+
+        if (checksum != *frameBuf->ptr) {
+            mspStarted = 0;
+            sendMspErrorResponse(TELEMETRY_MSP_CRC_ERROR, packet->cmd);
+            return true;
+        }
+    }
+
+    mspStarted = 0;
+    sbufSwitchToReader(rxBuf, mspPackage.requestBuffer);
+    processMspPacket();
+    return true;
+}
+
+bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn)
+{
+    static uint8_t checksum = 0;
+    static uint8_t seq = 0;
+
+    uint8_t payloadOut[payloadSize];
+    sbuf_t payload;
+    sbuf_t *payloadBuf = sbufInit(&payload, payloadOut, payloadOut + payloadSize);
+    sbuf_t *txBuf = &mspPackage.responsePacket->buf;
+
+    // detect first reply packet
+    if (txBuf->ptr == mspPackage.responseBuffer) {
+
+        // header
+        uint8_t head = TELEMETRY_MSP_START_FLAG | (seq++ & TELEMETRY_MSP_SEQ_MASK);
+        if (mspPackage.responsePacket->result < 0) {
+            head |= TELEMETRY_MSP_ERROR_FLAG;
+        }
+        sbufWriteU8(payloadBuf, head);
+
+        uint8_t size = sbufBytesRemaining(txBuf);
+        sbufWriteU8(payloadBuf, size);
+    } else {
+        // header
+        sbufWriteU8(payloadBuf, (seq++ & TELEMETRY_MSP_SEQ_MASK));
+    }
+
+    const uint8_t bufferBytesRemaining = sbufBytesRemaining(txBuf);
+    const uint8_t payloadBytesRemaining = sbufBytesRemaining(payloadBuf);
+    uint8_t frame[payloadBytesRemaining];
+
+    if (bufferBytesRemaining >= payloadBytesRemaining) {
+
+        sbufReadData(txBuf, frame, payloadBytesRemaining);
+        sbufAdvance(txBuf, payloadBytesRemaining);
+        sbufWriteData(payloadBuf, frame, payloadBytesRemaining);
+        responseFn(payloadOut);
+
+        return true;
+
+    } else {
+
+        sbufReadData(txBuf, frame, bufferBytesRemaining);
+        sbufAdvance(txBuf, bufferBytesRemaining);
+        sbufWriteData(payloadBuf, frame, bufferBytesRemaining);
+        sbufSwitchToReader(txBuf, mspPackage.responseBuffer);
+
+        checksum = sbufBytesRemaining(txBuf) ^ mspPackage.responsePacket->cmd;
+
+        while (sbufBytesRemaining(txBuf)) {
+            checksum ^= sbufReadU8(txBuf);
+        }
+        sbufWriteU8(payloadBuf, checksum);
+
+        while (sbufBytesRemaining(payloadBuf)>1) {
+            sbufWriteU8(payloadBuf, 0);
+        }
+
+    }
+
+    responseFn(payloadOut);
+    return false;
+}
+
+#endif

src/main/telemetry/msp_shared.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include "common/streambuf.h"
+#include "telemetry/crsf.h"
+#include "telemetry/smartport.h"
+
+typedef void (*mspResponseFnPtr)(uint8_t *payload);
+
+struct mspPacket_s;
+typedef struct mspPackage_s {
+    sbuf_t requestFrame;
+    uint8_t *requestBuffer;
+    uint8_t *responseBuffer;
+    struct mspPacket_s *requestPacket;
+    struct mspPacket_s *responsePacket;
+} mspPackage_t;
+
+typedef union mspRxBuffer_u {
+    uint8_t smartPortMspRxBuffer[SMARTPORT_MSP_RX_BUF_SIZE];
+#if 0
+    uint8_t crsfMspRxBuffer[CRSF_MSP_RX_BUF_SIZE];
+#endif
+} mspRxBuffer_t;
+
+typedef union mspTxBuffer_u {
+    uint8_t smartPortMspTxBuffer[SMARTPORT_MSP_TX_BUF_SIZE];
+#if 0
+    uint8_t crsfMspTxBuffer[CRSF_MSP_TX_BUF_SIZE];
+#endif
+} mspTxBuffer_t;
+
+void initSharedMsp(void);
+bool handleMspFrame(uint8_t *frameStart, int frameLength);
+bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);

src/main/telemetry/smartport.c

@@ -5,6 +5,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdlib.h>
+#include <string.h>
 #include <math.h>
 
 #include "platform.h"
@@ -17,65 +18,49 @@
 #include "common/utils.h"
 
 #include "config/feature.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
 
+#include "drivers/accgyro/accgyro.h"
+#include "drivers/compass/compass.h"
+#include "drivers/sensor.h"
 #include "drivers/time.h"
-#include "drivers/serial.h"
 
 #include "fc/config.h"
+#include "fc/controlrate_profile.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
-#include "fc/controlrate_profile.h"
-#include "fc/fc_msp.h"
 
+#include "flight/failsafe.h"
 #include "flight/imu.h"
+#include "flight/mixer.h"
+#include "flight/pid.h"
 
+#include "io/beeper.h"
 #include "io/gps.h"
-#include "io/ledstrip.h"
 #include "io/serial.h"
 
 #include "navigation/navigation.h"
 
-#include "rx/rx.h"
+#include "sensors/boardalignment.h"
-#include "rx/msp.h"
+#include "sensors/sensors.h"
-
-#include "sensors/acceleration.h"
 #include "sensors/battery.h"
+#include "sensors/acceleration.h"
 #include "sensors/barometer.h"
+#include "sensors/compass.h"
+#include "sensors/gyro.h"
 #include "sensors/pitotmeter.h"
 
+#include "rx/rx.h"
+
 #include "telemetry/telemetry.h"
 #include "telemetry/smartport.h"
+#include "telemetry/msp_shared.h"
 
+#define SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500
 
-enum
+// these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky_hub.h
-{
-    SPSTATE_UNINITIALIZED,
-    SPSTATE_INITIALIZED,
-    SPSTATE_WORKING,
-};
-
-enum
-{
-    FSSP_START_STOP = 0x7E,
-
-    FSSP_DLE        = 0x7D,
-    FSSP_DLE_XOR    = 0x20,
-
-    FSSP_DATA_FRAME = 0x10,
-    FSSP_MSPC_FRAME = 0x30, // MSP client frame
-    FSSP_MSPS_FRAME = 0x32, // MSP server frame
-
-    // ID of sensor. Must be something that is polled by FrSky RX
-    FSSP_SENSOR_ID1 = 0x1B,
-    FSSP_SENSOR_ID2 = 0x0D,
-    FSSP_SENSOR_ID3 = 0x34,
-    FSSP_SENSOR_ID4 = 0x67,
-    // there are 32 ID's polled by smartport master
-    // remaining 3 bits are crc (according to comments in openTx code)
-};
-
-// these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky.h
 enum
 {
     FSSP_DATAID_SPEED      = 0x0830 ,
@@ -87,6 +72,7 @@ enum
     FSSP_DATAID_ADC1       = 0xF102 ,
     FSSP_DATAID_ADC2       = 0xF103 ,
     FSSP_DATAID_LATLONG    = 0x0800 ,
+    FSSP_DATAID_CAP_USED   = 0x0600 ,
     FSSP_DATAID_VARIO      = 0x0110 ,
     FSSP_DATAID_CELLS      = 0x0300 ,
     FSSP_DATAID_CELLS_LAST = 0x030F ,
@@ -100,10 +86,10 @@ enum
     FSSP_DATAID_GPS_ALT    = 0x0820 ,
     FSSP_DATAID_ASPD       = 0x0A00 ,
     FSSP_DATAID_A3         = 0x0900 ,
-    FSSP_DATAID_A4         = 0x0910 ,
+    FSSP_DATAID_A4         = 0x0910
 };
 
-const uint16_t smartPortDataIdTable[] = {
+const uint16_t frSkyDataIdTable[] = {
     FSSP_DATAID_SPEED     ,
     FSSP_DATAID_VFAS      ,
     FSSP_DATAID_CURRENT   ,
@@ -114,6 +100,7 @@ const uint16_t smartPortDataIdTable[] = {
     //FSSP_DATAID_ADC2      ,
     FSSP_DATAID_LATLONG   ,
     FSSP_DATAID_LATLONG   , // twice
+    //FSSP_DATAID_CAP_USED  ,
     FSSP_DATAID_VARIO     ,
     //FSSP_DATAID_CELLS     ,
     //FSSP_DATAID_CELLS_LAST,
@@ -126,526 +113,317 @@ const uint16_t smartPortDataIdTable[] = {
     FSSP_DATAID_HOME_DIST ,
     FSSP_DATAID_GPS_ALT   ,
     FSSP_DATAID_ASPD      ,
-    //FSSP_DATAID_A3        ,
+    FSSP_DATAID_A3        ,
     FSSP_DATAID_A4        ,
+    0
 };
 
 #define __USE_C99_MATH // for roundf()
 #define SMARTPORT_BAUD 57600
 #define SMARTPORT_UART_MODE MODE_RXTX
 #define SMARTPORT_SERVICE_TIMEOUT_MS 1 // max allowed time to find a value to send
-#define SMARTPORT_DATA_LENGTH (sizeof(smartPortDataIdTable) / sizeof(smartPortDataIdTable[0]))
 
 static serialPort_t *smartPortSerialPort = NULL; // The 'SmartPort'(tm) Port.
 static serialPortConfig_t *portConfig;
 
-static bool smartPortTelemetryEnabled =  false;
 static portSharing_e smartPortPortSharing;
 
-char smartPortState = SPSTATE_UNINITIALIZED;
+enum
-static bool smartPortHasRequest = false;
+{
+    TELEMETRY_STATE_UNINITIALIZED,
+    TELEMETRY_STATE_INITIALIZED_SERIAL,
+    TELEMETRY_STATE_INITIALIZED_EXTERNAL,
+};
+
+static uint8_t telemetryState = TELEMETRY_STATE_UNINITIALIZED;
 static uint8_t smartPortIdCnt = 0;
-static uint32_t smartPortLastRequestTime = 0;
 
 typedef struct smartPortFrame_s {
     uint8_t  sensorId;
-    uint8_t  frameId;
+    smartPortPayload_t payload;
-    uint16_t valueId;
-    uint32_t data;
     uint8_t  crc;
 } __attribute__((packed)) smartPortFrame_t;
 
-#define SMARTPORT_FRAME_SIZE  sizeof(smartPortFrame_t)
+#define SMARTPORT_MSP_PAYLOAD_SIZE (sizeof(smartPortPayload_t) - sizeof(uint8_t))
-#define SMARTPORT_TX_BUF_SIZE 256
 
-#define SMARTPORT_PAYLOAD_OFFSET offsetof(smartPortFrame_t, valueId)
+static smartPortWriteFrameFn *smartPortWriteFrame;
-#define SMARTPORT_PAYLOAD_SIZE   (SMARTPORT_FRAME_SIZE - SMARTPORT_PAYLOAD_OFFSET - 1)
 
-static smartPortFrame_t smartPortRxBuffer;
+#if defined(USE_MSP_OVER_TELEMETRY)
-static uint8_t smartPortRxBytes = 0;
-static bool smartPortFrameReceived = false;
-
-#define SMARTPORT_MSP_VERSION    1
-#define SMARTPORT_MSP_VER_SHIFT  5
-#define SMARTPORT_MSP_VER_MASK   (0x7 << SMARTPORT_MSP_VER_SHIFT)
-#define SMARTPORT_MSP_VERSION_S  (SMARTPORT_MSP_VERSION << SMARTPORT_MSP_VER_SHIFT)
-
-#define SMARTPORT_MSP_ERROR_FLAG (1 << 5)
-#define SMARTPORT_MSP_START_FLAG (1 << 4)
-#define SMARTPORT_MSP_SEQ_MASK   0x0F
-
-#define SMARTPORT_MSP_RX_BUF_SIZE 64
-
-static uint8_t smartPortMspTxBuffer[SMARTPORT_TX_BUF_SIZE];
-static mspPacket_t smartPortMspReply;
 static bool smartPortMspReplyPending = false;
+#endif
 
-#define SMARTPORT_MSP_RES_ERROR (-10)
+smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool useChecksum)
-
-enum {
-    SMARTPORT_MSP_VER_MISMATCH = 0,
-    SMARTPORT_MSP_CRC_ERROR    = 1,
-    SMARTPORT_MSP_ERROR        = 2,
-};
-
-static void smartPortDataReceive(uint16_t c)
 {
+    static uint8_t rxBuffer[sizeof(smartPortPayload_t)];
+    static uint8_t smartPortRxBytes = 0;
     static bool skipUntilStart = true;
+    static bool awaitingSensorId = false;
     static bool byteStuffing = false;
     static uint16_t checksum = 0;
 
-    uint32_t now = millis();
-
     if (c == FSSP_START_STOP) {
+        *clearToSend = false;
         smartPortRxBytes = 0;
-        smartPortHasRequest = false;
+        awaitingSensorId = true;
         skipUntilStart = false;
-        return;
-    } else if (skipUntilStart)
-        return;
 
-    uint8_t* rxBuffer = (uint8_t*)&smartPortRxBuffer;
+        return NULL;
-    if (smartPortRxBytes == 0) {
+    } else if (skipUntilStart) {
-        if ((c == FSSP_SENSOR_ID1) && (serialRxBytesWaiting(smartPortSerialPort) == 0)) {
+        return NULL;
-
-            // our slot is starting...
-            smartPortLastRequestTime = now;
-            smartPortHasRequest = true;
-        } else if (c == FSSP_SENSOR_ID2) {
-            rxBuffer[smartPortRxBytes++] = c;
-            checksum = 0;
-        } else
-            skipUntilStart = true;
     }
-    else {
 
+    if (awaitingSensorId) {
+        awaitingSensorId = false;
+        if ((c == FSSP_SENSOR_ID1) && checkQueueEmpty()) {
+            // our slot is starting, no need to decode more
+            *clearToSend = true;
+            skipUntilStart = true;
+        } else if (c == FSSP_SENSOR_ID2) {
+            checksum = 0;
+        } else {
+            skipUntilStart = true;
+        }
+    } else {
         if (c == FSSP_DLE) {
             byteStuffing = true;
-            return;
-        }
 
-        if (byteStuffing) {
+            return NULL;
+        } else if (byteStuffing) {
             c ^= FSSP_DLE_XOR;
             byteStuffing = false;
         }
 
-        rxBuffer[smartPortRxBytes++] = c;
+        if (smartPortRxBytes < sizeof(smartPortPayload_t)) {
-
+            rxBuffer[smartPortRxBytes++] = (uint8_t)c;
-        if (smartPortRxBytes == SMARTPORT_FRAME_SIZE) {
-            if (c == (0xFF - checksum))
-                smartPortFrameReceived = true;
-            skipUntilStart = true;
-        } else if (smartPortRxBytes < SMARTPORT_FRAME_SIZE) {
             checksum += c;
-            checksum += checksum >> 8;
+
-            checksum &= 0x00FF;
+            if (!useChecksum && (smartPortRxBytes == sizeof(smartPortPayload_t))) {
+                skipUntilStart = true;
+
+                return (smartPortPayload_t *)&rxBuffer;
+            }
+        } else {
+            skipUntilStart = true;
+
+            checksum += c;
+            checksum = (checksum & 0xFF) + (checksum >> 8);
+            if (checksum == 0xFF) {
+                return (smartPortPayload_t *)&rxBuffer;
+            }
         }
     }
+
+    return NULL;
 }
 
-static void smartPortSendByte(uint8_t c, uint16_t *crcp)
+void smartPortSendByte(uint8_t c, uint16_t *checksum, serialPort_t *port)
 {
     // smart port escape sequence
     if (c == FSSP_DLE || c == FSSP_START_STOP) {
-        serialWrite(smartPortSerialPort, FSSP_DLE);
+        serialWrite(port, FSSP_DLE);
-        serialWrite(smartPortSerialPort, c ^ FSSP_DLE_XOR);
+        serialWrite(port, c ^ FSSP_DLE_XOR);
-    } else
+    } else {
-        serialWrite(smartPortSerialPort, c);
+        serialWrite(port, c);
+    }
 
-    if (crcp == NULL)
+    if (checksum != NULL) {
-        return;
+        *checksum += c;
-
+    }
-    uint16_t crc = *crcp;
-    crc += c;
-    crc += crc >> 8;
-    crc &= 0x00FF;
-    *crcp = crc;
 }
 
-static void smartPortSendPackageEx(uint8_t frameId, uint8_t* data)
+void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *port, uint16_t checksum)
 {
-    uint16_t crc = 0;
+    uint8_t *data = (uint8_t *)payload;
-    smartPortSendByte(frameId, &crc);
+    for (unsigned i = 0; i < sizeof(smartPortPayload_t); i++) {
-    for (unsigned i = 0; i < SMARTPORT_PAYLOAD_SIZE; i++)
+        smartPortSendByte(*data++, &checksum, port);
-        smartPortSendByte(*data++, &crc);
+    }
-    smartPortSendByte(0xFF - (uint8_t)crc, NULL);
+    checksum = 0xff - ((checksum & 0xff) + (checksum >> 8));
+    smartPortSendByte((uint8_t)checksum, NULL, port);
+}
+
+static void smartPortWriteFrameInternal(const smartPortPayload_t *payload)
+{
+    smartPortWriteFrameSerial(payload, smartPortSerialPort, 0);
 }
 
 static void smartPortSendPackage(uint16_t id, uint32_t val)
 {
-    uint8_t payload[SMARTPORT_PAYLOAD_SIZE];
+    smartPortPayload_t payload;
-    uint8_t *dst = payload;
+    payload.frameId = FSSP_DATA_FRAME;
-    *dst++ = id & 0xFF;
+    payload.valueId = id;
-    *dst++ = id >> 8;
+    payload.data = val;
-    *dst++ = val & 0xFF;
-    *dst++ = (val >> 8) & 0xFF;
-    *dst++ = (val >> 16) & 0xFF;
-    *dst++ = (val >> 24) & 0xFF;
 
-    smartPortSendPackageEx(FSSP_DATA_FRAME,payload);
+    smartPortWriteFrame(&payload);
-    smartPortHasRequest = false;
 }
 
-void initSmartPortTelemetry(void)
+bool initSmartPortTelemetry(void)
 {
-    portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT);
+    if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) {
-    smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT);
+        portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT);
+        if (portConfig) {
+            smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT);
+
+            smartPortWriteFrame = smartPortWriteFrameInternal;
+
+            telemetryState = TELEMETRY_STATE_INITIALIZED_SERIAL;
+        }
+
+        return true;
+    }
+
+    return false;
 }
 
-void freeSmartPortTelemetryPort(void)
+bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameExternal)
+{
+    if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) {
+        smartPortWriteFrame = smartPortWriteFrameExternal;
+
+        telemetryState = TELEMETRY_STATE_INITIALIZED_EXTERNAL;
+
+        return true;
+    }
+
+    return false;
+}
+
+static void freeSmartPortTelemetryPort(void)
 {
     closeSerialPort(smartPortSerialPort);
     smartPortSerialPort = NULL;
-
-    smartPortState = SPSTATE_UNINITIALIZED;
-    smartPortTelemetryEnabled = false;
 }
 
-void configureSmartPortTelemetryPort(void)
+static void configureSmartPortTelemetryPort(void)
 {
-    portOptions_t portOptions;
+    if (portConfig) {
+        portOptions_t portOptions = (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (telemetryConfig()->telemetry_inversion ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
 
-    if (!portConfig)
+        smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions);
-        return;
+    }
-
-    if (telemetryConfig()->smartportUartUnidirectional)
-        portOptions = SERIAL_UNIDIR;
-    else
-        portOptions = SERIAL_BIDIR;
-
-    if (telemetryConfig()->telemetry_inversion)
-        portOptions |= SERIAL_INVERTED;
-
-    smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions);
-
-    if (!smartPortSerialPort)
-        return;
-
-    smartPortState = SPSTATE_INITIALIZED;
-    smartPortTelemetryEnabled = true;
-    smartPortLastRequestTime = millis();
-}
-
-bool canSendSmartPortTelemetry(void)
-{
-    return smartPortSerialPort && (smartPortState == SPSTATE_INITIALIZED || smartPortState == SPSTATE_WORKING);
 }
 
 void checkSmartPortTelemetryState(void)
 {
-    bool newTelemetryEnabledValue = telemetryDetermineEnabledState(smartPortPortSharing);
+    if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL) {
+        bool enableSerialTelemetry = telemetryDetermineEnabledState(smartPortPortSharing);
 
-    if (newTelemetryEnabledValue == smartPortTelemetryEnabled)
+        if (enableSerialTelemetry && !smartPortSerialPort) {
-        return;
+            configureSmartPortTelemetryPort();
-
+        } else if (!enableSerialTelemetry && smartPortSerialPort) {
-    if (newTelemetryEnabledValue)
+            freeSmartPortTelemetryPort();
-        configureSmartPortTelemetryPort();
+        }
-    else
+    }
-        freeSmartPortTelemetryPort();
 }
 
-static void initSmartPortMspReply(int16_t cmd)
+#if defined(USE_MSP_OVER_TELEMETRY)
-{
+static void smartPortSendMspResponse(uint8_t *data) {
-    smartPortMspReply.buf.ptr    = smartPortMspTxBuffer;
+    smartPortPayload_t payload;
-    smartPortMspReply.buf.end    = ARRAYEND(smartPortMspTxBuffer);
+    payload.frameId = FSSP_MSPS_FRAME;
+    memcpy(&payload.valueId, data, SMARTPORT_MSP_PAYLOAD_SIZE);
 
-    smartPortMspReply.cmd    = cmd;
+    smartPortWriteFrame(&payload);
-    smartPortMspReply.result = 0;
 }
+#endif
 
-static void processMspPacket(mspPacket_t* packet)
+void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const uint32_t *requestTimeout)
 {
-    initSmartPortMspReply(0);
+    if (payload) {
-
-    if (mspFcProcessCommand(packet, &smartPortMspReply, NULL) == MSP_RESULT_ERROR)
-        sbufWriteU8(&smartPortMspReply.buf, SMARTPORT_MSP_ERROR);
-
-    // change streambuf direction
-    sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer);
-    smartPortMspReplyPending = true;
-}
-
-/**
- * Request frame format:
- * - Header: 1 byte
- *   - Reserved: 2 bits (future use)
- *   - Error-flag: 1 bit
- *   - Start-flag: 1 bit
- *   - CSeq: 4 bits
- *
- * - MSP payload:
- *   - if Error-flag == 0:
- *     - size: 1 byte
- *     - payload
- *     - CRC (request type included)
- *   - if Error-flag == 1:
- *     - size: 1 byte (== 1)
- *     - error: 1 Byte
- *       - 0: Version mismatch (type=0)
- *       - 1: Sequence number error
- *       - 2: MSP error
- *     - CRC (request type included)
- */
-bool smartPortSendMspReply(void)
-{
-    static uint8_t checksum = 0;
-    static uint8_t seq = 0;
-
-    uint8_t packet[SMARTPORT_PAYLOAD_SIZE];
-    uint8_t* p = packet;
-    uint8_t* end = p + SMARTPORT_PAYLOAD_SIZE;
-
-    sbuf_t* txBuf = &smartPortMspReply.buf;
-
-    // detect first reply packet
-    if (txBuf->ptr == smartPortMspTxBuffer) {
-
-        // header
-        uint8_t head = SMARTPORT_MSP_START_FLAG | (seq++ & SMARTPORT_MSP_SEQ_MASK);
-        if (smartPortMspReply.result < 0)
-            head |= SMARTPORT_MSP_ERROR_FLAG;
-        *p++ = head;
-
-        uint8_t size = sbufBytesRemaining(txBuf);
-        *p++ = size;
-
-        checksum = size ^ smartPortMspReply.cmd;
-    } else {
-        // header
-        *p++ = (seq++ & SMARTPORT_MSP_SEQ_MASK);
-    }
-
-    while ((p < end) && (sbufBytesRemaining(txBuf) > 0)) {
-        *p = sbufReadU8(txBuf);
-        checksum ^= *p++; // MSP checksum
-    }
-
-    // to be continued...
-    if (p == end) {
-        smartPortSendPackageEx(FSSP_MSPS_FRAME, packet);
-        return true;
-    }
-
-    // nothing left in txBuf,
-    // append the MSP checksum
-    *p++ = checksum;
-
-    // pad with zeros
-    while (p < end)
-        *p++ = 0;
-
-    smartPortSendPackageEx(FSSP_MSPS_FRAME, packet);
-    return false;
-}
-
-void smartPortSendErrorReply(uint8_t error, int16_t cmd)
-{
-    initSmartPortMspReply(cmd);
-    sbufWriteU8(&smartPortMspReply.buf, error);
-    smartPortMspReply.result = SMARTPORT_MSP_RES_ERROR;
-
-    sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer);
-    smartPortMspReplyPending = true;
-}
-
-/**
- * Request frame format:
- * - Header: 1 byte
- *   - Version: 3 bits
- *   - Start-flag: 1 bit
- *   - CSeq: 4 bits
- *
- * - MSP payload:
- *   - Size: 1 Byte
- *   - Type: 1 Byte
- *   - payload...
- *   - CRC
- */
-void handleSmartPortMspFrame(smartPortFrame_t* sp_frame)
-{
-    static uint8_t mspBuffer[SMARTPORT_MSP_RX_BUF_SIZE];
-    static uint8_t mspStarted = 0;
-    static uint8_t lastSeq = 0;
-    static uint8_t checksum = 0;
-    static mspPacket_t cmd;
-
-    // re-assemble MSP frame & forward to MSP port when complete
-    uint8_t* p = ((uint8_t*)sp_frame) + SMARTPORT_PAYLOAD_OFFSET;
-    uint8_t* end = p + SMARTPORT_PAYLOAD_SIZE;
-
-    uint8_t head = *p++;
-    uint8_t seq = head & SMARTPORT_MSP_SEQ_MASK;
-    uint8_t version = (head & SMARTPORT_MSP_VER_MASK) >> SMARTPORT_MSP_VER_SHIFT;
-
-    if (version != SMARTPORT_MSP_VERSION) {
-        mspStarted = 0;
-        smartPortSendErrorReply(SMARTPORT_MSP_VER_MISMATCH,0);
-        return;
-    }
-
-    // check start-flag
-    if (head & SMARTPORT_MSP_START_FLAG) {
-
-        //TODO: if (p_size > SMARTPORT_MSP_RX_BUF_SIZE) error!
-        uint8_t p_size = *p++;
-        cmd.cmd = *p++;
-        cmd.result = 0;
-
-        cmd.buf.ptr = mspBuffer;
-        cmd.buf.end = mspBuffer + p_size;
-
-        checksum = p_size ^ cmd.cmd;
-        mspStarted = 1;
-    } else if (!mspStarted)
-        return; // no start packet yet, throw this one away
-    else if (((lastSeq + 1) & SMARTPORT_MSP_SEQ_MASK) != seq) {
-        // packet loss detected!
-        mspStarted = 0;
-        return;
-    }
-
-    // copy payload bytes
-    while ((p < end) && sbufBytesRemaining(&cmd.buf)) {
-        checksum ^= *p;
-        sbufWriteU8(&cmd.buf,*p++);
-    }
-
-    // reached end of smart port frame
-    if (p == end) {
-        lastSeq = seq;
-        return;
-    }
-
-    // last byte must be the checksum
-    if (checksum != *p) {
-        mspStarted = 0;
-        smartPortSendErrorReply(SMARTPORT_MSP_CRC_ERROR,cmd.cmd);
-        return;
-    }
-
-    // end of MSP packet reached
-    mspStarted = 0;
-    sbufSwitchToReader(&cmd.buf,mspBuffer);
-
-    processMspPacket(&cmd);
-}
-
-void handleSmartPortTelemetry(void)
-{
-    uint32_t smartPortLastServiceTime = millis();
-
-    if (!smartPortTelemetryEnabled)
-        return;
-
-    if (!canSendSmartPortTelemetry())
-        return;
-
-    while (serialRxBytesWaiting(smartPortSerialPort) > 0) {
-        uint8_t c = serialRead(smartPortSerialPort);
-        smartPortDataReceive(c);
-    }
-
-    if (smartPortFrameReceived) {
-        smartPortFrameReceived = false;
         // do not check the physical ID here again
         // unless we start receiving other sensors' packets
-        if (smartPortRxBuffer.frameId == FSSP_MSPC_FRAME)
+
-            handleSmartPortMspFrame(&smartPortRxBuffer); // Pass only the payload: skip sensorId & frameId
+#if defined(USE_MSP_OVER_TELEMETRY)
+        if (payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT) {
+            // Pass only the payload: skip frameId
+            uint8_t *frameStart = (uint8_t *)&payload->valueId;
+            smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE);
+        }
+#endif
     }
 
-    while (smartPortHasRequest) {
+    bool doRun = true;
+    while (doRun && *clearToSend) {
         // Ensure we won't get stuck in the loop if there happens to be nothing available to send in a timely manner - dump the slot if we loop in there for too long.
-        if ((millis() - smartPortLastServiceTime) > SMARTPORT_SERVICE_TIMEOUT_MS) {
+        if (requestTimeout) {
-            smartPortHasRequest = false;
+            if (millis() >= *requestTimeout) {
-            return;
+                *clearToSend = false;
+
+                return;
+            }
+        } else {
+            doRun = false;
         }
 
+#if defined(USE_MSP_OVER_TELEMETRY)
         if (smartPortMspReplyPending) {
-            smartPortMspReplyPending = smartPortSendMspReply();
+            smartPortMspReplyPending = sendMspReply(SMARTPORT_MSP_PAYLOAD_SIZE, &smartPortSendMspResponse);
-            smartPortHasRequest = 0;
+            *clearToSend = false;
+
             return;
         }
+#endif
 
         // we can send back any data we want, our table keeps track of the order and frequency of each data type we send
+        uint16_t id = frSkyDataIdTable[smartPortIdCnt];
+        if (id == 0) { // end of table reached, loop back
+            smartPortIdCnt = 0;
+            id = frSkyDataIdTable[smartPortIdCnt];
+        }
         smartPortIdCnt++;
-        if (smartPortIdCnt >= SMARTPORT_DATA_LENGTH)
-            smartPortIdCnt = 0; // end of table reached, loop back
-        uint16_t id = smartPortDataIdTable[smartPortIdCnt];
 
         switch (id) {
-#ifdef USE_GPS
-            case FSSP_DATAID_SPEED      :
-                //convert to knots: 1cm/s = 0.0194384449 knots
-                //Speed should be sent in knots/1000 (GPS speed is in cm/s)
-                if (sensors(SENSOR_GPS) && STATE(GPS_FIX))
-                    smartPortSendPackage(id, (uint32_t)gpsSol.groundSpeed * 1944 / 100);
-                break;
-#endif
             case FSSP_DATAID_VFAS       :
-                if (feature(FEATURE_VBAT)) {
+                if (isBatteryVoltageConfigured()) {
-                    uint16_t vfasVoltage;
+                    uint16_t vfasVoltage = telemetryConfig()->report_cell_voltage ? getBatteryAverageCellVoltage() : getBatteryVoltage();
-                    uint16_t vbat = getBatteryVoltage();
-                    if (telemetryConfig()->frsky_vfas_cell_voltage)
-                        vfasVoltage = vbat / getBatteryCellCount();
-                    else
-                        vfasVoltage = vbat;
                     smartPortSendPackage(id, vfasVoltage);
+                    *clearToSend = false;
                 }
                 break;
             case FSSP_DATAID_CURRENT    :
-                if (feature(FEATURE_CURRENT_METER))
+                if (isAmperageConfigured()) {
                     smartPortSendPackage(id, getAmperage() / 10); // given in 10mA steps, unknown requested unit
+                    *clearToSend = false;
+                }
                 break;
             //case FSSP_DATAID_RPM        :
             case FSSP_DATAID_ALTITUDE   :
-                if (sensors(SENSOR_BARO))
+                if (sensors(SENSOR_BARO)) {
                     smartPortSendPackage(id, getEstimatedActualPosition(Z)); // unknown given unit, requested 100 = 1 meter
+                    *clearToSend = false;
+                }
                 break;
             case FSSP_DATAID_FUEL       :
-                if (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_PERCENT)
+                if (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_PERCENT) {
                     smartPortSendPackage(id, calculateBatteryPercentage()); // Show remaining battery % if smartport_fuel_percent=ON
-                else if (feature(FEATURE_CURRENT_METER))
+                    *clearToSend = false;
+                } else if (isAmperageConfigured()) {
                     smartPortSendPackage(id, (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_MAH ? getMAhDrawn() : getMWhDrawn()));
+                    *clearToSend = false;
+                }
                 break;
             //case FSSP_DATAID_ADC1       :
             //case FSSP_DATAID_ADC2       :
-#ifdef USE_GPS
+            //case FSSP_DATAID_CAP_USED   :
-            case FSSP_DATAID_LATLONG    :
-                if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
-                    uint32_t tmpui = 0;
-                    // the same ID is sent twice, one for longitude, one for latitude
-                    // the MSB of the sent uint32_t helps FrSky keep track
-                    // the even/odd bit of our counter helps us keep track
-                    if (smartPortIdCnt & 1) {
-                        tmpui = abs(gpsSol.llh.lon);  // now we have unsigned value and one bit to spare
-                        tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000;  // 6/100 = 1.5/25, division by power of 2 is fast
-                        if (gpsSol.llh.lon < 0)
-                            tmpui |= 0x40000000;
-                    } else {
-                        tmpui = abs(gpsSol.llh.lat);  // now we have unsigned value and one bit to spare
-                        tmpui = (tmpui + tmpui / 2) / 25;  // 6/100 = 1.5/25, division by power of 2 is fast
-                        if (gpsSol.llh.lat < 0)
-                            tmpui |= 0x40000000;
-                    }
-                    smartPortSendPackage(id, tmpui);
-                }
-                break;
-#endif
             case FSSP_DATAID_VARIO      :
-                if (sensors(SENSOR_BARO))
+                if (sensors(SENSOR_BARO)) {
                     smartPortSendPackage(id, lrintf(getEstimatedActualVelocity(Z))); // unknown given unit but requested in 100 = 1m/s
+                    *clearToSend = false;
+                }
                 break;
             case FSSP_DATAID_HEADING    :
                 smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg
+                *clearToSend = false;
                 break;
             case FSSP_DATAID_ACCX       :
                 smartPortSendPackage(id, lrintf(100 * acc.accADCf[X]));
+                *clearToSend = false;
                 break;
             case FSSP_DATAID_ACCY       :
                 smartPortSendPackage(id, lrintf(100 * acc.accADCf[Y]));
+                *clearToSend = false;
                 break;
             case FSSP_DATAID_ACCZ       :
                 smartPortSendPackage(id, lrintf(100 * acc.accADCf[Z]));
+                *clearToSend = false;
                 break;
             case FSSP_DATAID_T1         :
                 {
@@ -692,7 +470,8 @@ void handleSmartPortTelemetry(void)
                     if (FLIGHT_MODE(FAILSAFE_MODE))
                         tmpi += 40000;
 
-                    smartPortSendPackage(id, tmpi);
+                    smartPortSendPackage(id, (uint32_t)tmpi);
+                    *clearToSend = false;
                     break;
                 }
             case FSSP_DATAID_T2         :
@@ -715,36 +494,103 @@ void handleSmartPortTelemetry(void)
                         tmpi += 4000;
 
                     smartPortSendPackage(id, tmpi);
+                    *clearToSend = false;
 #endif
-                } else if (feature(FEATURE_GPS))
+                } else if (feature(FEATURE_GPS)) {
                     smartPortSendPackage(id, 0);
+                    *clearToSend = false;
+                }
                 break;
 #ifdef USE_GPS
+            case FSSP_DATAID_SPEED      :
+                if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
+                    //convert to knots: 1cm/s = 0.0194384449 knots
+                    //Speed should be sent in knots/1000 (GPS speed is in cm/s)
+                    uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100;
+                    smartPortSendPackage(id, tmpui);
+                    *clearToSend = false;
+                }
+                break;
+            case FSSP_DATAID_LATLONG    :
+                if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
+                    uint32_t tmpui = 0;
+                    // the same ID is sent twice, one for longitude, one for latitude
+                    // the MSB of the sent uint32_t helps FrSky keep track
+                    // the even/odd bit of our counter helps us keep track
+                    if (smartPortIdCnt & 1) {
+                        tmpui = abs(gpsSol.llh.lon);  // now we have unsigned value and one bit to spare
+                        tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000;  // 6/100 = 1.5/25, division by power of 2 is fast
+                        if (gpsSol.llh.lon < 0) tmpui |= 0x40000000;
+                    }
+                    else {
+                        tmpui = abs(gpsSol.llh.lat);  // now we have unsigned value and one bit to spare
+                        tmpui = (tmpui + tmpui / 2) / 25;  // 6/100 = 1.5/25, division by power of 2 is fast
+                        if (gpsSol.llh.lat < 0) tmpui |= 0x40000000;
+                    }
+                    smartPortSendPackage(id, tmpui);
+                    *clearToSend = false;
+                }
+                break;
             case FSSP_DATAID_HOME_DIST  :
-                if (sensors(SENSOR_GPS) && STATE(GPS_FIX))
+                if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
                     smartPortSendPackage(id, GPS_distanceToHome);
+                     *clearToSend = false;
+                }
                 break;
             case FSSP_DATAID_GPS_ALT    :
-                if (sensors(SENSOR_GPS) && STATE(GPS_FIX))
+                if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
                     smartPortSendPackage(id, gpsSol.llh.alt); // cm
+                    *clearToSend = false;
+                }
                 break;
 #endif
-#ifdef USE_PITOT
-            case FSSP_DATAID_ASPD    :
-                if (sensors(SENSOR_PITOT))
-                    smartPortSendPackage(id, pitot.airSpeed * 0.194384449f); // cm/s to knots*10
-                break;
-#endif
-            //case FSSP_DATAID_A3         :
             case FSSP_DATAID_A4         :
-                if (feature(FEATURE_VBAT))
+                if (isBatteryVoltageConfigured()) {
                     smartPortSendPackage(id, getBatteryAverageCellVoltage());
+                    *clearToSend = false;
+                }
+                break;
+            case FSSP_DATAID_ASPD       :
+#ifdef USE_PITOT
+                if (sensors(SENSOR_PITOT)) {
+                    smartPortSendPackage(id, pitot.airSpeed * 0.194384449f); // cm/s to knots*1
+                    *clearToSend = false;
+                }
+#endif
                 break;
             default:
                 break;
-                // if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just loop back to the start
+                // if nothing is sent, hasRequest isn't cleared, we already incremented the counter, just loop back to the start
         }
     }
 }
 
+static bool serialCheckQueueEmpty(void)
+{
+    return (serialRxBytesWaiting(smartPortSerialPort) == 0);
+}
+
+void handleSmartPortTelemetry(void)
+{
+    static bool clearToSend = false;
+    static volatile timeUs_t lastTelemetryFrameReceivedUs;
+    static smartPortPayload_t *payload = NULL;
+
+    const uint32_t requestTimeout = millis() + SMARTPORT_SERVICE_TIMEOUT_MS;
+
+    if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL && smartPortSerialPort) {
+        while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) {
+            uint8_t c = serialRead(smartPortSerialPort);
+            payload = smartPortDataReceive(c, &clearToSend, serialCheckQueueEmpty, true);
+            if (payload) {
+                lastTelemetryFrameReceivedUs = micros();
+            }
+        }
+
+        if (cmpTimeUs(micros(), lastTelemetryFrameReceivedUs) >= SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) {
+            processSmartPortTelemetry(payload, &clearToSend, &requestTimeout);
+            payload = NULL;
+        }
+    }
+}
 #endif

src/main/telemetry/smartport.h

@@ -7,10 +7,51 @@
 
 #pragma once
 
-void initSmartPortTelemetry(void);
+#include <stdbool.h>
+#include <stdint.h>
+
+#define SMARTPORT_MSP_TX_BUF_SIZE 256
+#define SMARTPORT_MSP_RX_BUF_SIZE 64
+
+enum
+{
+    FSSP_START_STOP = 0x7E,
+
+    FSSP_DLE        = 0x7D,
+    FSSP_DLE_XOR    = 0x20,
+
+    FSSP_DATA_FRAME = 0x10,
+    FSSP_MSPC_FRAME_SMARTPORT = 0x30, // MSP client frame
+    FSSP_MSPC_FRAME_FPORT = 0x31, // MSP client frame
+    FSSP_MSPS_FRAME = 0x32, // MSP server frame
+
+    // ID of sensor. Must be something that is polled by FrSky RX
+    FSSP_SENSOR_ID1 = 0x1B,
+    FSSP_SENSOR_ID2 = 0x0D,
+    FSSP_SENSOR_ID3 = 0x34,
+    FSSP_SENSOR_ID4 = 0x67
+    // there are 32 ID's polled by smartport master
+    // remaining 3 bits are crc (according to comments in openTx code)
+};
+
+typedef struct smartPortPayload_s {
+    uint8_t  frameId;
+    uint16_t valueId;
+    uint32_t data;
+} __attribute__((packed)) smartPortPayload_t;
+
+typedef void smartPortWriteFrameFn(const smartPortPayload_t *payload);
+typedef bool smartPortCheckQueueEmptyFn(void);
+
+bool initSmartPortTelemetry(void);
+void checkSmartPortTelemetryState(void);
+bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameExternal);
 
 void handleSmartPortTelemetry(void);
-void checkSmartPortTelemetryState(void);
+void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *hasRequest, const uint32_t *requestTimeout);
 
-void configureSmartPortTelemetryPort(void);
+smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool withChecksum);
-void freeSmartPortTelemetryPort(void);
+
+struct serialPort_s;
+void smartPortWriteFrameSerial(const smartPortPayload_t *payload, struct serialPort_s *port, uint16_t checksum);
+void smartPortSendByte(uint8_t c, uint16_t *checksum, struct serialPort_s *port);

src/main/telemetry/telemetry.c

@@ -65,12 +65,13 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
     .frsky_coordinate_format = FRSKY_FORMAT_DMS,
     .frsky_unit = FRSKY_UNIT_METRICS,
     .frsky_vfas_precision = 0,
-    .frsky_vfas_cell_voltage = 0,
+    .report_cell_voltage = 0,
     .hottAlarmSoundInterval = 5,
     .smartportUartUnidirectional = 0,
     .smartportFuelUnit = SMARTPORT_FUEL_UNIT_MAH,
     .ibusTelemetryType = 0,
-    .ltmUpdateRate = LTM_RATE_NORMAL
+    .ltmUpdateRate = LTM_RATE_NORMAL,
+    .halfDuplex = 1,
 );
 
 void telemetryInit(void)

src/main/telemetry/telemetry.h

@@ -24,7 +24,10 @@
 
 #pragma once
 
+#include "common/time.h"
+
 #include "config/parameter_group.h"
+
 #include "io/serial.h"
 
 
@@ -58,12 +61,13 @@ typedef struct telemetryConfig_s {
     frskyGpsCoordFormat_e frsky_coordinate_format;
     frskyUnit_e frsky_unit;
     uint8_t frsky_vfas_precision;
-    uint8_t frsky_vfas_cell_voltage;
+    uint8_t report_cell_voltage;
     uint8_t hottAlarmSoundInterval;
     uint8_t smartportUartUnidirectional;
     smartportFuelUnit_e smartportFuelUnit;
     uint8_t ibusTelemetryType;
     uint8_t ltmUpdateRate;
+    uint8_t halfDuplex;
 } telemetryConfig_t;
 
 PG_DECLARE(telemetryConfig_t, telemetryConfig);