Added support for the FrSky FPort protocol.

make/source.mk

@@ -111,10 +111,12 @@ FC_SRC = \
             rx/rx_spi.c \
             rx/crsf.c \
             rx/sbus.c \
+            rx/sbus_channels.c \
             rx/spektrum.c \
             rx/sumd.c \
             rx/sumh.c \
             rx/xbus.c \
+            rx/fport.c \
             sensors/acceleration.c \
             sensors/boardalignment.c \
             sensors/compass.c \
@@ -222,9 +224,11 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
             rx/rx_spi.c \
             rx/crsf.c \
             rx/sbus.c \
+            rx/sbus_channels.c \
             rx/spektrum.c \
             rx/sumd.c \
             rx/xbus.c \
+            rx/fport.c \
             scheduler/scheduler.c \
             sensors/acceleration.c \
             sensors/boardalignment.c \

src/main/build/debug.c

@@ -53,5 +53,7 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "FRSKY_D_RX",
     "GYRO_RAW",
     "MAX7456_SIGNAL",
-    "MAX7456_SPICLOCK"
+    "MAX7456_SPICLOCK",
+    "SBUS",
+    "FPORT",
 };

src/main/build/debug.h

@@ -72,6 +72,8 @@ typedef enum {
     DEBUG_GYRO_RAW,
     DEBUG_MAX7456_SIGNAL,
     DEBUG_MAX7456_SPICLOCK,
+    DEBUG_SBUS,
+    DEBUG_FPORT,
     DEBUG_COUNT
 } debugType_e;
 

src/main/fc/settings.c

@@ -180,7 +180,9 @@ static const char * const lookupTableSerialRX[] = {
     "IBUS",
     "JETIEXBUS",
     "CRSF",
-    "SRXL"
+    "SRXL",
+    "CUSTOM",
+    "FPORT",
 };
 #endif
 

src/main/rx/fport.c

@@ -0,0 +1,366 @@
+/*
+ * 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_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;
+
+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 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)
+{
+    static timeUs_t frameStartAt = 0;
+    static bool frameStarting = false;
+    static bool escapedCharacter = false;
+    static uint8_t bufferPosition = 0;
+    static timeUs_t lastFrameReceivedUs = 0;
+    static bool telemetryFrame = false;
+
+    const timeUs_t currentTimeUs = micros();
+
+    if (bufferPosition > 0 && cmpTimeUs(currentTimeUs, frameStartAt) > FPORT_TIME_NEEDED_PER_FRAME_US + 500) {
+         reportFrameError(DEBUG_FPORT_ERROR_TIMEOUT);
+
+         bufferPosition = 0;
+         clearToSend = false;
+     }
+
+    uint8_t val = (uint8_t)c;
+
+    if (val == FPORT_FRAME_MARKER) {
+        frameStarting = true;
+        escapedCharacter = false;
+        frameStartAt = currentTimeUs;
+
+        if (bufferPosition > 0) {
+            uint8_t nextWriteIndex = (rxBufferWriteIndex + 1) % NUM_RX_BUFFERS;
+            if (nextWriteIndex != rxBufferReadIndex) {
+                rxBuffer[rxBufferWriteIndex].length = bufferPosition;
+                rxBufferWriteIndex = nextWriteIndex;
+            }
+            bufferPosition = 0;
+            if (telemetryFrame) {
+                clearToSend = true;
+                lastTelemetryFrameReceivedUs = currentTimeUs;
+            }
+
+            if (lastFrameReceivedUs) {
+                DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_INTERVAL, currentTimeUs - lastFrameReceivedUs);
+            }
+            lastFrameReceivedUs = currentTimeUs;
+        } else {
+            clearToSend = false;
+        }
+        telemetryFrame = false;
+    } else {
+        if ((frameStarting || bufferPosition > 0) && bufferPosition < BUFFER_SIZE) {
+            if (escapedCharacter) {
+                val = val ^ FPORT_ESCAPE_MASK;
+                escapedCharacter = false;
+            } else if (val == FPORT_ESCAPE_CHAR) {
+                escapedCharacter = true;
+
+                return;
+            }
+
+            if (bufferPosition == 1 && val == FPORT_FRAME_TYPE_TELEMETRY_REQUEST) {
+                telemetryFrame = true;
+            }
+
+            frameStarting = false;
+            rxBuffer[rxBufferWriteIndex].data[bufferPosition++] = val;
+        } else if (bufferPosition == BUFFER_SIZE) {
+            bufferPosition = 0;
+
+            reportFrameError(DEBUG_FPORT_ERROR_OVERSIZE);
+        }
+
+    }
+}
+
+#if defined(USE_TELEMETRY_SMARTPORT)
+static void smartPortWriteFrameFport(const smartPortPayload_t *payload)
+{
+    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(void)
+{
+    static smartPortPayload_t payloadBuffer;
+    static smartPortPayload_t *mspPayload = NULL;
+    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(&frame->data.controlData.channels);
+
+                        processRssi(constrain(frame->data.controlData.rssi, 0, 100));
+                    }
+
+                    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;
+                        }
+
+                        mspPayload = NULL;
+                        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 defined(USE_TELEMETRY_SMARTPORT)
+    } else {
+        timeUs_t currentTimeUs = micros();
+        if (clearToSend && cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) {
+            if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US) {
+                clearToSend = false;
+            }
+
+            if (clearToSend) {
+                DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_DELAY, currentTimeUs - lastTelemetryFrameReceivedUs);
+
+                if (hasTelemetryRequest || mspPayload) {
+                    processSmartPortTelemetry(mspPayload, &clearToSend, NULL);
+                }
+
+                if (clearToSend) {
+                    smartPortWriteFrameFport(&emptySmartPortFrame);
+                    clearToSend = false;
+                }
+
+            }
+        }
+#endif
+    }
+
+    return result;
+}
+
+bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    sbusChannelsInit(rxConfig);
+
+    rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
+    rxRuntimeConfig->rxRefreshRate = 11000;
+
+    rxRuntimeConfig->rcReadRawFn = sbusChannelsReadRawRC;
+    rxRuntimeConfig->rcFrameStatusFn = fportFrameStatus;
+
+    const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
+    if (!portConfig) {
+        return false;
+    }
+
+    fportPort = openSerialPort(portConfig->identifier,
+        FUNCTION_RX_SERIAL,
+        fportDataReceive,
+        FPORT_BAUDRATE,
+        MODE_RXTX,
+        FPORT_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0: SERIAL_INVERTED) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
+    );
+
+    if (fportPort) {
+#if defined(USE_TELEMETRY_SMARTPORT)
+        telemetryEnabled = initSmartPortTelemetryExternal(smartPortWriteFrameFport);
+#endif
+    }
+
+    return fportPort != NULL;
+}
+#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/rx.c

@@ -49,6 +49,7 @@
 
 #include "rx/rx.h"
 #include "rx/pwm.h"
+#include "rx/fport.h"
 #include "rx/sbus.h"
 #include "rx/spektrum.h"
 #include "rx/sumd.h"
@@ -275,6 +276,11 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
     case SERIALRX_TARGET_CUSTOM:
         enabled = targetCustomSerialRxInit(rxConfig, rxRuntimeConfig);
         break;
+#endif
+#ifdef USE_SERIALRX_FPORT
+    case SERIALRX_FPORT:
+        enabled = fportRxInit(rxConfig, rxRuntimeConfig);
+        break;
 #endif
     default:
         enabled = false;

src/main/rx/rx.h

@@ -18,8 +18,11 @@
 #pragma once
 
 #include "common/time.h"
+
 #include "config/parameter_group.h"
 
+#include "drivers/io_types.h"
+
 #define STICK_CHANNEL_COUNT 4
 
 #define PWM_RANGE_ZERO 0 // FIXME should all usages of this be changed to use PWM_RANGE_MIN?
@@ -57,6 +60,7 @@ typedef enum {
     SERIALRX_CRSF = 9,
     SERIALRX_SRXL = 10,
     SERIALRX_TARGET_CUSTOM = 11,
+    SERIALRX_FPORT = 12,
 } SerialRXType;
 
 #define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT          12

src/main/rx/sbus.c

@@ -23,6 +23,8 @@
 
 #ifdef USE_SERIAL_RX
 
+#include "build/debug.h"
+
 #include "common/utils.h"
 
 #include "drivers/time.h"
@@ -32,8 +34,10 @@
 #ifdef USE_TELEMETRY
 #include "telemetry/telemetry.h"
 #endif
+
 #include "rx/rx.h"
 #include "rx/sbus.h"
+#include "rx/sbus_channels.h"
 
 /*
  * Observations
@@ -49,20 +53,10 @@
 
 #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
 
@@ -75,35 +69,19 @@ static uint16_t sbusStateFlags = 0;
 #define SBUS_DIGITAL_CHANNEL_MIN 173
 #define SBUS_DIGITAL_CHANNEL_MAX 1812
 
+enum {
+    DEBUG_SBUS_FRAME_FLAGS = 0,
+    DEBUG_SBUS_STATE_FLAGS,
+    DEBUG_SBUS_FRAME_TIME,
+};
+
+static uint16_t sbusStateFlags = 0;
+
 static bool sbusFrameDone = false;
 
-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.
      *
@@ -147,9 +125,7 @@ static void sbusDataReceive(uint16_t c)
             sbusFrameDone = false;
         } else {
             sbusFrameDone = true;
-#ifdef DEBUG_SBUS_PACKETS
+            DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_TIME, sbusFrameTime);
-        debug[2] = sbusFrameTime;
-#endif
         }
     }
 }
@@ -161,80 +137,31 @@ static uint8_t sbusFrameStatus(void)
     }
     sbusFrameDone = false;
 
-#ifdef DEBUG_SBUS_PACKETS
     sbusStateFlags = 0;
-    debug[1] = sbusFrame.frame.flags;
+    DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_FLAGS, sbusFrame.frame.channels.flags);
-#endif
 
-    sbusChannelData[0] = sbusFrame.frame.chan0;
+    if (sbusFrame.frame.channels.flags & SBUS_FLAG_SIGNAL_LOSS) {
-    sbusChannelData[1] = sbusFrame.frame.chan1;
-    sbusChannelData[2] = sbusFrame.frame.chan2;
-    sbusChannelData[3] = sbusFrame.frame.chan3;
-    sbusChannelData[4] = sbusFrame.frame.chan4;
-    sbusChannelData[5] = sbusFrame.frame.chan5;
-    sbusChannelData[6] = sbusFrame.frame.chan6;
-    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) {
-        sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MAX;
-    } else {
-        sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MIN;
-    }
-
-    if (sbusFrame.frame.flags & SBUS_FLAG_SIGNAL_LOSS) {
-#ifdef DEBUG_SBUS_PACKETS
         sbusStateFlags |= SBUS_STATE_SIGNALLOSS;
-        debug[0] = sbusStateFlags;
+        DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusStateFlags);
-#endif
     }
-    if (sbusFrame.frame.flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
+    if (sbusFrame.frame.channels.flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
-        // internal failsafe enabled and rx failsafe flag set
-#ifdef DEBUG_SBUS_PACKETS
         sbusStateFlags |= SBUS_STATE_FAILSAFE;
-        debug[0] = sbusStateFlags;
+        DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, 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_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusStateFlags);
-    debug[0] = sbusStateFlags;
-#endif
-    return RX_FRAME_COMPLETE;
-}
 
-static uint16_t sbusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
+    return sbusChannelsDecode(&sbusFrame.frame.channels);
-{
-    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 (5 * sbusChannelData[chan] / 8) + 880;
 }
 
 bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 {
-    for (int b = 0; b < SBUS_MAX_CHANNEL; b++) {
+    sbusChannelsInit(rxConfig);
-        sbusChannelData[b] = (16 * rxConfig->midrc) / 10 - 1408;
-    }
 
     rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
     rxRuntimeConfig->rxRefreshRate = 11000;
 
-    rxRuntimeConfig->rcReadRawFn = sbusReadRawRC;
+    rxRuntimeConfig->rcReadRawFn = sbusChannelsReadRawRC;
     rxRuntimeConfig->rcFrameStatusFn = sbusFrameStatus;
 
     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);

src/main/rx/sbus_channels.c

@@ -0,0 +1,98 @@
+/*
+ * 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
+
+static uint32_t sbusChannelData[SBUS_MAX_CHANNEL];
+
+#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(const sbusChannels_t *channels)
+{
+    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;
+}
+
+uint16_t sbusChannelsReadRawRC(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 (5 * sbusChannelData[chan] / 8) + 880;
+}
+
+void sbusChannelsInit(const rxConfig_t *rxConfig)
+{
+    for (int b = 0; b < SBUS_MAX_CHANNEL; b++) {
+        sbusChannelData[b] = (16 * rxConfig->midrc) / 10 - 1408;
+    }
+}
+
+#endif

src/main/rx/sbus_channels.h

@@ -0,0 +1,55 @@
+/*
+ * 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(const sbusChannels_t *channels);
+
+uint16_t sbusChannelsReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
+
+void sbusChannelsInit(const rxConfig_t *rxConfig);
+

src/main/target/FRSKYF3/target.h

@@ -60,10 +60,8 @@
 #define USE_UART1
 #define USE_UART2
 #define USE_UART3
-#define USE_SOFTSERIAL1
-#define USE_SOFTSERIAL2
 
-#define SERIAL_PORT_COUNT       6
+#define SERIAL_PORT_COUNT       4
 #define UART1_TX_PIN            PA9
 #define UART1_RX_PIN            PA10
 

src/main/target/OMNIBUS/target.h

@@ -17,12 +17,14 @@
 
 #pragma once
 
-#undef USE_TELEMETRY_IBUS   //no space left
+// Removed to make the firmware fit into flash:
-#undef USE_TELEMETRY_HOTT   //no space left
+#undef USE_TELEMETRY_IBUS
-#undef USE_TELEMETRY_JETIEXBUS // no space left
+#undef USE_TELEMETRY_HOTT
-#undef USE_TELEMETRY_MAVLINK   // no space left
+#undef USE_TELEMETRY_JETIEXBUS
-#undef USE_RCDEVICE // no space left
+#undef USE_TELEMETRY_MAVLINK
-#undef USE_RTC_TIME // no space left
+#undef USE_TELEMETRY_LTM
+#undef USE_RCDEVICE
+#undef USE_RTC_TIME
 
 #define TARGET_BOARD_IDENTIFIER "OMNI" // https://en.wikipedia.org/wiki/Omnibus
 

src/main/target/SPRACINGF3EVO/target.h

@@ -88,8 +88,9 @@
 #define USE_UART1
 #define USE_UART2
 #define USE_UART3
-#define USE_SOFTSERIAL1
+// Disabled to make the target fit into flash
-#define USE_SOFTSERIAL2
+//#define USE_SOFTSERIAL1
+//#define USE_SOFTSERIAL2
 
 #define SOFTSERIAL1_RX_PIN      PA6 // PWM 5
 #define SOFTSERIAL1_TX_PIN      PA7 // PWM 6
@@ -97,7 +98,7 @@
 #define SOFTSERIAL2_RX_PIN      PB0 // PWM 7
 #define SOFTSERIAL2_TX_PIN      PB1 // PWM 8
 
-#define SERIAL_PORT_COUNT       6
+#define SERIAL_PORT_COUNT       4
 
 #define USE_ESCSERIAL
 #define ESCSERIAL_TIMER_TX_PIN  PA15  // (HARDARE=0,PPM)

src/main/target/SPRACINGF3NEO/target.h

@@ -95,8 +95,9 @@
 #define VTX_RTC6705
 #define VTX_RTC6705_OPTIONAL    // VTX/OSD board is OPTIONAL
 
-#undef VTX_SMARTAUDIO           // Disabled due to flash size
+// Disabled due to flash size
-#undef VTX_TRAMP                // Disabled due to flash size
+#undef VTX_SMARTAUDIO
+#undef VTX_TRAMP
 
 #define RTC6705_CS_PIN          PF4
 #define RTC6705_SPI_INSTANCE    SPI3

src/main/target/common_fc_pre.h

@@ -119,6 +119,7 @@
 #define USE_RX_MSP
 #define USE_SERIALRX_JETIEXBUS
 #define USE_SENSOR_NAMES
+#define USE_SERIALRX_FPORT      // FrSky FPort
 #define USE_VIRTUAL_CURRENT_METER
 #define VTX_COMMON
 #define VTX_CONTROL

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"
@@ -60,8 +61,8 @@
 enum
 {
     SPSTATE_UNINITIALIZED,
-    SPSTATE_INITIALIZED,
+    SPSTATE_INITIALIZED_SERIAL,
-    SPSTATE_WORKING
+    SPSTATE_INITIALIZED_EXTERNAL,
 };
 
 enum
@@ -72,7 +73,8 @@ enum
     FSSP_DLE_XOR    = 0x20,
 
     FSSP_DATA_FRAME = 0x10,
-    FSSP_MSPC_FRAME = 0x30, // MSP client frame
+    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
@@ -145,154 +147,161 @@ const uint16_t frSkyDataIdTable[] = {
 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;
-static uint8_t smartPortHasRequest = 0;
 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_PAYLOAD_OFFSET offsetof(smartPortFrame_t, valueId)
+static smartPortWriteFrameFn *smartPortWriteFrame;
-#define SMARTPORT_PAYLOAD_SIZE   (SMARTPORT_FRAME_SIZE - SMARTPORT_PAYLOAD_OFFSET - 1)
 
-static smartPortFrame_t smartPortRxBuffer;
-static uint8_t smartPortRxBytes = 0;
-static bool smartPortFrameReceived = false;
 #if defined(USE_MSP_OVER_TELEMETRY)
 static bool smartPortMspReplyPending = false;
 #endif
 
-static void smartPortDataReceive(uint16_t c)
+static smartPortPayload_t *smartPortDataReceiveSerial(uint16_t c, bool *clearToSend)
 {
+    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 = 0;
+        awaitingSensorId = true;
         skipUntilStart = false;
-        return;
+
+        return NULL;
     } else if (skipUntilStart) {
-        return;
+        return NULL;
     }
 
-    uint8_t* rxBuffer = (uint8_t*)&smartPortRxBuffer;
+    if (awaitingSensorId) {
-    if (smartPortRxBytes == 0) {
+        awaitingSensorId = false;
         if ((c == FSSP_SENSOR_ID1) && (serialRxBytesWaiting(smartPortSerialPort) == 0)) {
-
+            // our slot is starting, no need to decode more
-            // our slot is starting...
+            *clearToSend = true;
-            smartPortLastRequestTime = now;
+            skipUntilStart = true;
-            smartPortHasRequest = 1;
         } else if (c == FSSP_SENSOR_ID2) {
-            rxBuffer[smartPortRxBytes++] = c;
             checksum = 0;
-        }
+        } else {
-        else {
             skipUntilStart = true;
         }
-    }
+    } else {
-    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;
+        } else {
-            checksum &= 0x00FF;
+            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(port, c);
-        serialWrite(smartPortSerialPort, 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);
 }
 
-void initSmartPortTelemetry(void)
+bool initSmartPortTelemetry(void)
 {
+    if (smartPortState == SPSTATE_UNINITIALIZED) {
         portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT);
+        if (portConfig) {
             smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT);
+
+            smartPortWriteFrame = smartPortWriteFrameInternal;
+
+            smartPortState = SPSTATE_INITIALIZED_SERIAL;
+        }
+
+        return true;
+    }
+
+    return false;
 }
 
-void freeSmartPortTelemetryPort(void)
+bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameExternal)
+{
+    if (smartPortState == SPSTATE_UNINITIALIZED) {
+        smartPortWriteFrame = smartPortWriteFrameExternal;
+
+        smartPortState = SPSTATE_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)
 {
     if (!portConfig) {
         return;
@@ -301,82 +310,64 @@ void configureSmartPortTelemetryPort(void)
     portOptions_e portOptions = (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
 
     smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, 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 (smartPortState == SPSTATE_INITIALIZED_SERIAL) {
-    if (newTelemetryEnabledValue == smartPortTelemetryEnabled) {
+        bool enableSerialTelemetry = telemetryDetermineEnabledState(smartPortPortSharing);
-        return;
-    }
 
-    if (newTelemetryEnabledValue)
+        if (enableSerialTelemetry && !smartPortSerialPort) {
             configureSmartPortTelemetryPort();
-    else
+        } else if (!enableSerialTelemetry && smartPortSerialPort) {
             freeSmartPortTelemetryPort();
+        }
+    }
 }
 
 #if defined(USE_MSP_OVER_TELEMETRY)
-void smartPortSendMspResponse(uint8_t *payload) {
+static void smartPortSendMspResponse(uint8_t *data) {
-    smartPortSendPackageEx(FSSP_MSPS_FRAME, payload);
+    smartPortPayload_t payload;
+    payload.frameId = FSSP_MSPS_FRAME;
+    memcpy(&payload.valueId, data, SMARTPORT_MSP_PAYLOAD_SIZE);
+
+    smartPortWriteFrame(&payload);
 }
 #endif
 
-void handleSmartPortTelemetry(void)
+void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const uint32_t *requestTimeout)
 {
-    uint32_t smartPortLastServiceTime = millis();
+    if (payload) {
-
-    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 defined(USE_MSP_OVER_TELEMETRY)
-        if (smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) {
+        if (payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT) {
-            // Pass only the payload: skip sensorId & frameId
+            // Pass only the payload: skip frameId
-            uint8_t *frameStart = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET;
+            uint8_t *frameStart = (uint8_t *)&payload->valueId;
-            smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_PAYLOAD_SIZE);
+            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 = 0;
+            if (millis() >= *requestTimeout) {
+                *clearToSend = false;
+
                 return;
             }
+        } else {
+            doRun = false;
+        }
 
 #if defined(USE_MSP_OVER_TELEMETRY)
         if (smartPortMspReplyPending) {
-            smartPortMspReplyPending = sendMspReply(SMARTPORT_PAYLOAD_SIZE, &smartPortSendMspResponse);
+            smartPortMspReplyPending = sendMspReply(SMARTPORT_MSP_PAYLOAD_SIZE, &smartPortSendMspResponse);
-            smartPortHasRequest = 0;
+            *clearToSend = false;
+
             return;
         }
 #endif
@@ -402,7 +393,7 @@ void handleSmartPortTelemetry(void)
                     //Speed should be sent in knots/1000 (GPS speed is in cm/s)
                     uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100;
                     smartPortSendPackage(id, tmpui);
-                    smartPortHasRequest = 0;
+                    *clearToSend = false;
                 }
                 break;
 #endif
@@ -415,26 +406,26 @@ void handleSmartPortTelemetry(void)
                         vfasVoltage = getBatteryVoltage();
                     }
                     smartPortSendPackage(id, vfasVoltage * 10); // given in 0.1V, convert to volts
-                    smartPortHasRequest = 0;
+                    *clearToSend = false;
                 }
                 break;
             case FSSP_DATAID_CURRENT    :
                 if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
                     smartPortSendPackage(id, getAmperage() / 10); // given in 10mA steps, unknown requested unit
-                    smartPortHasRequest = 0;
+                    *clearToSend = false;
                 }
                 break;
             //case FSSP_DATAID_RPM        :
             case FSSP_DATAID_ALTITUDE   :
                 if (sensors(SENSOR_BARO)) {
                     smartPortSendPackage(id, getEstimatedAltitude()); // unknown given unit, requested 100 = 1 meter
-                    smartPortHasRequest = 0;
+                    *clearToSend = false;
                 }
                 break;
             case FSSP_DATAID_FUEL       :
                 if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
                     smartPortSendPackage(id, getMAhDrawn()); // given in mAh, unknown requested unit
-                    smartPortHasRequest = 0;
+                    *clearToSend = false;
                 }
                 break;
             //case FSSP_DATAID_ADC1       :
@@ -457,7 +448,7 @@ void handleSmartPortTelemetry(void)
                         if (gpsSol.llh.lat < 0) tmpui |= 0x40000000;
                     }
                     smartPortSendPackage(id, tmpui);
-                    smartPortHasRequest = 0;
+                    *clearToSend = false;
                 }
                 break;
 #endif
@@ -465,24 +456,24 @@ void handleSmartPortTelemetry(void)
             case FSSP_DATAID_VARIO      :
                 if (sensors(SENSOR_BARO)) {
                     smartPortSendPackage(id, getEstimatedVario()); // unknown given unit but requested in 100 = 1m/s
-                    smartPortHasRequest = 0;
+                    *clearToSend = false;
                 }
                 break;
             case FSSP_DATAID_HEADING    :
                 smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg
-                smartPortHasRequest = 0;
+                *clearToSend = false;
                 break;
             case FSSP_DATAID_ACCX       :
                 smartPortSendPackage(id, 100 * acc.accSmooth[X] / acc.dev.acc_1G); // Multiply by 100 to show as x.xx g on Taranis
-                smartPortHasRequest = 0;
+                *clearToSend = false;
                 break;
             case FSSP_DATAID_ACCY       :
                 smartPortSendPackage(id, 100 * acc.accSmooth[Y] / acc.dev.acc_1G);
-                smartPortHasRequest = 0;
+                *clearToSend = false;
                 break;
             case FSSP_DATAID_ACCZ       :
                 smartPortSendPackage(id, 100 * acc.accSmooth[Z] / acc.dev.acc_1G);
-                smartPortHasRequest = 0;
+                *clearToSend = false;
                 break;
             case FSSP_DATAID_T1         :
                 // we send all the flags as decimal digits for easy reading
@@ -528,18 +519,18 @@ void handleSmartPortTelemetry(void)
                     tmpi += 4000;
 
                 smartPortSendPackage(id, (uint32_t)tmpi);
-                smartPortHasRequest = 0;
+                *clearToSend = false;
                 break;
             case FSSP_DATAID_T2         :
                 if (sensors(SENSOR_GPS)) {
 #ifdef USE_GPS
                     // provide GPS lock status
                     smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + gpsSol.numSat);
-                    smartPortHasRequest = 0;
+                    *clearToSend = false;
 #endif
                 } else if (feature(FEATURE_GPS)) {
                     smartPortSendPackage(id, 0);
-                    smartPortHasRequest = 0;
+                    *clearToSend = false;
                 } else if (telemetryConfig()->pidValuesAsTelemetry) {
                     switch (t2Cnt) {
                         case 0:
@@ -569,28 +560,47 @@ void handleSmartPortTelemetry(void)
                         t2Cnt = 0;
                     }
                     smartPortSendPackage(id, tmp2);
-                    smartPortHasRequest = 0;
+                    *clearToSend = false;
                 }
                 break;
 #ifdef USE_GPS
             case FSSP_DATAID_GPS_ALT    :
                 if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
                     smartPortSendPackage(id, gpsSol.llh.alt * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7)
-                    smartPortHasRequest = 0;
+                    *clearToSend = false;
                 }
                 break;
 #endif
             case FSSP_DATAID_A4         :
                 if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryCellCount() > 0) {
                     smartPortSendPackage(id, getBatteryVoltage() * 10 / getBatteryCellCount()); // given in 0.1V, convert to volts
-                    smartPortHasRequest = 0;
+                    *clearToSend = false;
                 }
                 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
         }
     }
 }
 
+void handleSmartPortTelemetry(void)
+{
+    static bool clearToSend = false;
+
+    const uint32_t requestTimeout = millis() + SMARTPORT_SERVICE_TIMEOUT_MS;
+
+    if (!(smartPortState == SPSTATE_INITIALIZED_SERIAL && smartPortSerialPort)) {
+        return;
+    }
+
+    smartPortPayload_t *payload = NULL;
+    while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) {
+        uint8_t c = serialRead(smartPortSerialPort);
+        payload = smartPortDataReceiveSerial(c, &clearToSend);
+    }
+
+    processSmartPortTelemetry(payload, &clearToSend, &requestTimeout);
+}
+
 #endif

src/main/telemetry/smartport.h

@@ -7,14 +7,25 @@
 
 #pragma once
 
+#include "drivers/serial.h"
+
 #define SMARTPORT_MSP_TX_BUF_SIZE 256
 #define SMARTPORT_MSP_RX_BUF_SIZE 64
 
-void initSmartPortTelemetry(void);
+typedef struct smartPortPayload_s {
+    uint8_t  frameId;
+    uint16_t valueId;
+    uint32_t data;
+} __attribute__((packed)) smartPortPayload_t;
+
+typedef void smartPortWriteFrameFn(const smartPortPayload_t *payload);
+
+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);
-void freeSmartPortTelemetryPort(void);
 
+void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *port, uint16_t checksum);
+void smartPortSendByte(uint8_t c, uint16_t *checksum, serialPort_t *port);