Fixed Spektrum implementation inconsistency.

src/main/rx/spektrum.c

@@ -36,7 +36,6 @@
 #include "io/serial.h"
 
 #include "fc/config.h"
-#include "fc/dispatch.h"
 
 #include "io/spektrum_rssi.h"
 #include "io/spektrum_vtx_control.h"
@@ -53,6 +52,8 @@
 
 // driver for spektrum satellite receiver / sbus
 
+#define SPEKTRUM_TELEMETRY_FRAME_DELAY_US 1000 // Gap between received Rc frame and transmited TM frame
+
 bool srxlEnabled = false;
 int32_t resolution;
 uint8_t rssi_channel;
@@ -70,9 +71,6 @@ static serialPort_t *serialPort;
 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
 static uint8_t telemetryBuf[SRXL_FRAME_SIZE_MAX];
 static uint8_t telemetryBufLen = 0;
-
-void srxlRxSendTelemetryDataDispatch(dispatchEntry_t *self);
-static dispatchEntry_t srxlTelemetryDispatch = { .dispatch = srxlRxSendTelemetryDataDispatch};
 #endif
 
 // Receive ISR callback
@@ -109,10 +107,14 @@ static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
     UNUSED(rxRuntimeConfig);
 
-    if (!rcFrameComplete) {
+    static timeUs_t telemetryFrameRequestedUs = 0;
-        return RX_FRAME_PENDING;
-    }
 
+    uint8_t result = RX_FRAME_PENDING;
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
+    timeUs_t currentTimeUs = micros();
+#endif
+
+    if (rcFrameComplete) {
         rcFrameComplete = false;
 
 #if defined(USE_SPEKTRUM_REAL_RSSI) || defined(USE_SPEKTRUM_FAKE_RSSI)
@@ -148,21 +150,23 @@ static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
         }
 
 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
-    if (srxlEnabled) {
+        if (srxlEnabled && telemetryBufLen && (spekFrame[2] & 0x80) == 0) {
-        /* Only dispatch for transmission if there are some data in buffer AND servos in phase 0 */
+            telemetryFrameRequestedUs = currentTimeUs;
-        if (telemetryBufLen && (spekFrame[2] & 0x80) == 0) {
-            dispatchAdd(&srxlTelemetryDispatch, SPEKTRUM_TELEMETRY_FRAME_DELAY);
-        }
-
-        /* Trigger tm data collection if buffer has been sent and is empty, 
-           so data will be ready to transmit in the next phase 0 */
-        if (telemetryBufLen == 0) {
-            srxlCollectTelemetryNow();
-        }
         }
 #endif
 
-    return RX_FRAME_COMPLETE;
+        result = RX_FRAME_COMPLETE;
+    }
+
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
+    if (telemetryFrameRequestedUs && cmpTimeUs(currentTimeUs, telemetryFrameRequestedUs) >= SPEKTRUM_TELEMETRY_FRAME_DELAY_US) {
+        telemetryFrameRequestedUs = 0;
+
+        result = (result & ~RX_FRAME_PENDING) | RX_FRAME_PROCESSING_REQUIRED;
+    }
+#endif
+
+    return result;
 }
 
 static uint16_t spektrumReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
@@ -303,6 +307,30 @@ void spektrumBind(rxConfig_t *rxConfig)
 }
 #endif // USE_SPEKTRUM_BIND
 
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
+static bool spektrumProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    UNUSED(rxRuntimeConfig);
+
+    // if there is telemetry data to write
+    if (telemetryBufLen > 0) {
+        serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
+        telemetryBufLen = 0; // reset telemetry buffer
+
+        srxlCollectTelemetryNow();
+    }
+
+    return true;
+}
+
+void srxlRxWriteTelemetryData(const void *data, int len)
+{
+    len = MIN(len, (int)sizeof(telemetryBuf));
+    memcpy(telemetryBuf, data, len);
+    telemetryBufLen = len;
+}
+#endif
+
 bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 {
     rxRuntimeConfigPtr = rxRuntimeConfig;
@@ -347,6 +375,9 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
 
     rxRuntimeConfig->rcReadRawFn = spektrumReadRawRC;
     rxRuntimeConfig->rcFrameStatusFn = spektrumFrameStatus;
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
+    rxRuntimeConfig->rcProcessFrameFn = spektrumProcessFrame;
+#endif
 
     serialPort = openSerialPort(portConfig->identifier,
         FUNCTION_RX_SERIAL,
@@ -367,31 +398,9 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
         rssi_channel = 0;
     }
 
-    if (serialPort && srxlEnabled) {
-        dispatchEnable();
-    }
     return serialPort != NULL;
 }
 
-#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
-void srxlRxWriteTelemetryData(const void *data, int len)
-{
-    len = MIN(len, (int)sizeof(telemetryBuf));
-    memcpy(telemetryBuf, data, len);
-    telemetryBufLen = len;
-}
-
-void srxlRxSendTelemetryDataDispatch(dispatchEntry_t* self)
-{
-    UNUSED(self);
-    // if there is telemetry data to write
-    if (telemetryBufLen > 0) {
-        serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
-        telemetryBufLen = 0; // reset telemetry buffer
-    }
-}
-#endif
-
 bool srxlRxIsActive(void)
 {
     return serialPort != NULL;

src/main/rx/spektrum.h

@@ -32,7 +32,6 @@
 #define SRXL_FRAME_SIZE_MAX (SPEK_FRAME_SIZE + SRXL_FRAME_OVERHEAD)
 
 #define SPEKTRUM_NEEDED_FRAME_INTERVAL     5000
-#define SPEKTRUM_TELEMETRY_FRAME_DELAY     1000   // Gap between received Rc frame and transmited TM frame, uS
 
 #define SPEKTRUM_BAUDRATE                115200