Moved telemetry processing into the frame processing handler.

src/main/fc/core.c

@@ -1135,7 +1135,6 @@ static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs)
     }
 
     processRcCommand();
-
 }
 
 // Function for loop trigger

src/main/rx/cc2500_frsky_common.h

@@ -24,4 +24,5 @@
 
 bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntimeConfig);
 rx_spi_received_e frSkySpiDataReceived(uint8_t *packet);
+rx_spi_received_e frSkySpiProcessFrame(uint8_t *packet);
 void frSkySpiSetRcData(uint16_t *rcData, const uint8_t *payload);

src/main/rx/cc2500_frsky_shared.c

@@ -62,9 +62,11 @@ static uint8_t bindIdx;
 static int8_t bindOffset;
 
 typedef uint8_t handlePacketFn(uint8_t * const packet, uint8_t * const protocolState);
+typedef rx_spi_received_e processFrameFn(uint8_t * const packet);
 typedef void setRcDataFn(uint16_t *rcData, const uint8_t *payload);
 
 static handlePacketFn *handlePacket;
+static processFrameFn *processFrame;
 static setRcDataFn *setRcData;
 
 static void initialise() {
@@ -381,6 +383,15 @@ rx_spi_received_e frSkySpiDataReceived(uint8_t *packet)
     return ret;
 }
 
+rx_spi_received_e frSkySpiProcessFrame(uint8_t *packet)
+{
+    if (processFrame) {
+        return processFrame(packet);
+    }
+
+    return RX_SPI_RECEIVED_NONE;
+}
+
 void frSkySpiSetRcData(uint16_t *rcData, const uint8_t *payload)
 {
     setRcData(rcData, payload);
@@ -430,6 +441,9 @@ bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntime
         rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT_FRSKY_X;
 
         handlePacket = frSkyXHandlePacket;
+#if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT)
+        processFrame = frSkyXProcessFrame;
+#endif
         setRcData = frSkyXSetRcData;
         frSkyXInit(spiProtocol);
 

src/main/rx/cc2500_frsky_x.c

@@ -141,16 +141,21 @@ typedef struct telemetryPayload_s {
 static telemetryData_t telemetryTxBuffer[TELEMETRY_SEQUENCE_LENGTH];
 #endif
 
+static telemetryBuffer_t telemetryRxBuffer[TELEMETRY_SEQUENCE_LENGTH];
+
 static telemetrySequenceMarker_t responseToSend;
 
 #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
 static uint8_t frame[20];
+
 #if defined(USE_TELEMETRY_SMARTPORT)
 static uint8_t telemetryOutWriter;
 
 static uint8_t telemetryOutBuffer[TELEMETRY_OUT_BUFFER_SIZE];
 
 static bool telemetryEnabled = false;
+
+static uint8_t remoteProcessedId = 0;
 #endif
 #endif // USE_RX_FRSKY_SPI_TELEMETRY
 
@@ -320,14 +325,10 @@ bool isValidPacket(const uint8_t *packet)
 rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const protocolState)
 {
     static unsigned receiveTelemetryRetryCount = 0;
-    static timeMs_t pollingTimeMs = 0;
     static bool skipChannels = true;
 
-    static uint8_t remoteProcessedId = 0;
     static uint8_t remoteAckId = 0;
 
-    static uint8_t remoteToProcessIndex = 0;
-
     static timeUs_t packetTimerUs;
 
     static bool frameReceived;
@@ -335,8 +336,6 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
     static uint8_t channelsToSkip = 1;
     static uint32_t packetErrors = 0;
 
-    static telemetryBuffer_t telemetryRxBuffer[TELEMETRY_SEQUENCE_LENGTH];
-
 #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
     static bool telemetryReceived = false;
 #endif
@@ -432,7 +431,9 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
                     }
 
                     if (receiveTelemetryRetryCount >= 5) {
+#if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT)
                          remoteProcessedId = TELEMETRY_SEQUENCE_LENGTH - 1;
+#endif
                          remoteAckId = TELEMETRY_SEQUENCE_LENGTH - 1;
                          for (unsigned i = 0; i < TELEMETRY_SEQUENCE_LENGTH; i++) {
                              telemetryRxBuffer[i].needsProcessing = false;
@@ -483,35 +484,12 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
 
 #if defined(USE_TELEMETRY_SMARTPORT)
             if (telemetryEnabled) {
-                bool clearToSend = false;
+                ret |= RX_SPI_ROCESSING_REQUIRED;
-                timeMs_t now = millis();
-                smartPortPayload_t *payload = NULL;
-                if ((now - pollingTimeMs) > 24) {
-                    pollingTimeMs = now;
-
-                    clearToSend = true;
-                } else {
-                    uint8_t remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH;
-                    while (telemetryRxBuffer[remoteToProcessId].needsProcessing && !payload) {
-                        while (remoteToProcessIndex < telemetryRxBuffer[remoteToProcessId].data.dataLength && !payload) {
-                            payload = smartPortDataReceive(telemetryRxBuffer[remoteToProcessId].data.data[remoteToProcessIndex], &clearToSend, frSkyXCheckQueueEmpty, false);
-                            remoteToProcessIndex = remoteToProcessIndex + 1;
-                        }
-
-                        if (remoteToProcessIndex == telemetryRxBuffer[remoteToProcessId].data.dataLength) {
-                            remoteToProcessIndex = 0;
-                            telemetryRxBuffer[remoteToProcessId].needsProcessing = false;
-                            remoteProcessedId = remoteToProcessId;
-                            remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH;
-                        }
-                    }
-                }
-                processSmartPortTelemetry(payload, &clearToSend, NULL);
             }
 #endif
             *protocolState = STATE_RESUME;
             if (frameReceived) {
-                ret = RX_SPI_RECEIVED_DATA;
+                ret |= RX_SPI_RECEIVED_DATA;
             }
         }
 
@@ -549,6 +527,44 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
     return ret;
 }
 
+#if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT)
+rx_spi_received_e frSkyXProcessFrame(uint8_t * const packet)
+{
+    static timeMs_t pollingTimeMs = 0;
+    static uint8_t remoteToProcessIndex = 0;
+
+    UNUSED(packet);
+
+    bool clearToSend = false;
+    timeMs_t now = millis();
+    smartPortPayload_t *payload = NULL;
+    if ((now - pollingTimeMs) > 24) {
+        pollingTimeMs = now;
+
+        clearToSend = true;
+    } else {
+        uint8_t remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH;
+        while (telemetryRxBuffer[remoteToProcessId].needsProcessing && !payload) {
+            while (remoteToProcessIndex < telemetryRxBuffer[remoteToProcessId].data.dataLength && !payload) {
+                payload = smartPortDataReceive(telemetryRxBuffer[remoteToProcessId].data.data[remoteToProcessIndex], &clearToSend, frSkyXCheckQueueEmpty, false);
+                remoteToProcessIndex = remoteToProcessIndex + 1;
+            }
+
+            if (remoteToProcessIndex == telemetryRxBuffer[remoteToProcessId].data.dataLength) {
+                remoteToProcessIndex = 0;
+                telemetryRxBuffer[remoteToProcessId].needsProcessing = false;
+                remoteProcessedId = remoteToProcessId;
+                remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH;
+            }
+        }
+    }
+
+    processSmartPortTelemetry(payload, &clearToSend, NULL);
+
+    return RX_SPI_RECEIVED_NONE;
+}
+#endif
+
 void frSkyXInit(const rx_spi_protocol_e spiProtocol)
 {
     switch(spiProtocol) {

src/main/rx/cc2500_frsky_x.h

@@ -29,3 +29,4 @@ void frSkyXSetRcData(uint16_t *rcData, const uint8_t *payload);
 
 void frSkyXInit(const rx_spi_protocol_e spiProtocol);
 rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const protocolState);
+rx_spi_received_e frSkyXProcessFrame(uint8_t * const packet);

src/main/rx/rx_spi.c

@@ -57,10 +57,12 @@ STATIC_UNIT_TESTED uint8_t rxSpiNewPacketAvailable; // set true when a new packe
 
 typedef bool (*protocolInitFnPtr)(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntimeConfig);
 typedef rx_spi_received_e (*protocolDataReceivedFnPtr)(uint8_t *payload);
+typedef rx_spi_received_e (*protocolProcessFrameFnPtr)(uint8_t *payload);
 typedef void (*protocolSetRcDataFromPayloadFnPtr)(uint16_t *rcData, const uint8_t *payload);
 
 static protocolInitFnPtr protocolInit;
 static protocolDataReceivedFnPtr protocolDataReceived;
+static protocolProcessFrameFnPtr protocolProcessFrame;
 static protocolSetRcDataFromPayloadFnPtr protocolSetRcDataFromPayload;
 
 STATIC_UNIT_TESTED uint16_t rxSpiReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel)
@@ -137,6 +139,7 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
         protocolInit = frSkySpiInit;
         protocolDataReceived = frSkySpiDataReceived;
         protocolSetRcDataFromPayload = frSkySpiSetRcData;
+        protocolProcessFrame = frSkySpiProcessFrame;
 
         break;
 #endif // USE_RX_FRSKY_SPI
@@ -175,11 +178,39 @@ static uint8_t rxSpiFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
     UNUSED(rxRuntimeConfig);
 
-    if (protocolDataReceived(rxSpiPayload) == RX_SPI_RECEIVED_DATA) {
+    uint8_t status = RX_FRAME_PENDING;
+
+    rx_spi_received_e result = protocolDataReceived(rxSpiPayload);
+
+    if (result & RX_SPI_RECEIVED_DATA) {
         rxSpiNewPacketAvailable = true;
-        return RX_FRAME_COMPLETE;
+        status = RX_FRAME_COMPLETE;
     }
-    return RX_FRAME_PENDING;
+
+    if (result & RX_SPI_ROCESSING_REQUIRED) {
+        status |= RX_FRAME_PROCESSING_REQUIRED;
+    }
+
+    return status;
+}
+
+static bool rxSpiProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    UNUSED(rxRuntimeConfig);
+
+    if (protocolProcessFrame) {
+        rx_spi_received_e result = protocolProcessFrame(rxSpiPayload);
+
+        if (result & RX_SPI_RECEIVED_DATA) {
+            rxSpiNewPacketAvailable = true;
+        }
+
+        if (result & RX_SPI_ROCESSING_REQUIRED) {
+            return false;
+        }
+    }
+
+    return true;
 }
 
 /*
@@ -201,6 +232,7 @@ bool rxSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntimeCon
 
     rxRuntimeConfig->rcReadRawFn = rxSpiReadRawRC;
     rxRuntimeConfig->rcFrameStatusFn = rxSpiFrameStatus;
+    rxRuntimeConfig->rcProcessFrameFn = rxSpiProcessFrame;
 
     return ret;
 }

src/main/rx/rx_spi.h

@@ -47,8 +47,9 @@ typedef enum {
 
 typedef enum {
     RX_SPI_RECEIVED_NONE = 0,
-    RX_SPI_RECEIVED_BIND,
+    RX_SPI_RECEIVED_BIND = (1 << 0),
-    RX_SPI_RECEIVED_DATA
+    RX_SPI_RECEIVED_DATA = (1 << 1),
+    RX_SPI_ROCESSING_REQUIRED = (1 << 2),
 } rx_spi_received_e;
 
 // RC channels in AETR order