Spektrum SRXL improvements. Collect TM data in sync with transmission instead of async 30Hz, performancs +50%. B-channel Cap/Cur/temp was visible even though not used, corrected. Incomplete compile conditions corrected. (#5556)

src/main/fc/fc_init.c

@@ -633,7 +633,7 @@ void init(void)
     }
 #endif
 
-#if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY)
+#if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
     // Register the srxl Textgen telemetry sensor as a displayport device
     cmsDisplayPortRegister(displayPortSrxlInit());
 #endif

src/main/io/displayport_srxl.c

@@ -20,7 +20,7 @@
 #include <string.h>
 
 #include "platform.h"
-#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
+#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS) && defined(USE_TELEMETRY_SRXL)
 
 #include "common/utils.h"
 

src/main/rx/spektrum.c

@@ -39,6 +39,7 @@
 #include "io/spektrum_vtx_control.h"
 
 #include "telemetry/telemetry.h"
+#include "telemetry/srxl.h"
 
 #include "rx/rx.h"
 #include "rx/spektrum.h"
@@ -61,10 +62,13 @@ static volatile uint8_t spekFrame[SPEK_FRAME_SIZE];
 static rxRuntimeConfig_t *rxRuntimeConfigPtr;
 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
 static void spektrumDataReceive(uint16_t c, void *data)
@@ -95,7 +99,6 @@ static void spektrumDataReceive(uint16_t c, void *data)
 
 
 uint32_t spekChannelData[SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT];
-static dispatchEntry_t srxlTelemetryDispatch = { .dispatch = srxlRxSendTelemetryDataDispatch};
 
 static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
@@ -139,10 +142,21 @@ static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
         }
     }
 
-    /* only process if srxl enabled, some data in buffer AND servos in phase 0 */
-    if (srxlEnabled && telemetryBufLen && (spekFrame[2] & 0x80) == 0) {
-        dispatchAdd(&srxlTelemetryDispatch, SPEKTRUM_TELEMETRY_FRAME_DELAY);
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
+    if (srxlEnabled) {
+        /* Only dispatch for transmission if there are some data in buffer AND servos in phase 0 */
+        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;
 }
 
@@ -218,12 +232,12 @@ void spektrumBind(rxConfig_t *rxConfig)
         // Take care half-duplex case
         switch (rxConfig->serialrx_provider) {
         case SERIALRX_SRXL:
-#ifdef USE_TELEMETRY
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
             if (feature(FEATURE_TELEMETRY) && !telemetryCheckRxPortShared(portConfig)) {
                 bindPin = txPin;
             }
             break;
-#endif // USE_TELEMETRY
+#endif // USE_TELEMETRY && USE_TELEMETRY_SRXL
 
         default:
             bindPin = rxConfig->halfDuplex ? txPin : rxPin;
@@ -294,7 +308,7 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
     }
 
     srxlEnabled = false;
-#ifdef USE_TELEMETRY
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
     bool portShared = telemetryCheckRxPortShared(portConfig);
 #else
     bool portShared = false;
@@ -302,7 +316,7 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
 
     switch (rxConfig->serialrx_provider) {
     case SERIALRX_SRXL:
-#ifdef USE_TELEMETRY
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
         srxlEnabled = (feature(FEATURE_TELEMETRY) && !portShared);
         FALLTHROUGH;
 #endif
@@ -337,8 +351,7 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
         portShared || srxlEnabled ? MODE_RXTX : MODE_RX,
         (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | ((srxlEnabled || rxConfig->halfDuplex) ? SERIAL_BIDIR : 0)
         );
-
-#ifdef USE_TELEMETRY
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
     if (portShared) {
         telemetrySharedPort = serialPort;
     }
@@ -355,6 +368,7 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
     return serialPort != NULL;
 }
 
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
 void srxlRxWriteTelemetryData(const void *data, int len)
 {
     len = MIN(len, (int)sizeof(telemetryBuf));
@@ -371,6 +385,7 @@ void srxlRxSendTelemetryDataDispatch(dispatchEntry_t* self)
         telemetryBufLen = 0; // reset telemetry buffer
     }
 }
+#endif
 
 bool srxlRxIsActive(void)
 {

src/main/telemetry/srxl.c

@@ -21,7 +21,7 @@
 
 #include "platform.h"
 
-#ifdef USE_TELEMETRY
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
 
 #include "build/version.h"
 
@@ -58,8 +58,6 @@
 #include "io/vtx_tramp.h"
 #include "io/vtx_smartaudio.h"
 
-#define SRXL_CYCLETIME_US           33000 // 33ms, 30 Hz
-
 #define SRXL_ADDRESS_FIRST          0xA5
 #define SRXL_ADDRESS_SECOND         0x80
 #define SRXL_PACKET_LENGTH          0x15
@@ -73,6 +71,13 @@
 
 static bool srxlTelemetryEnabled;
 static uint8_t srxlFrame[SRXL_FRAME_SIZE_MAX];
+static bool srxlTelemetryNow   = false;
+
+void srxlCollectTelemetryNow(void)
+{
+    srxlTelemetryNow   = true;
+}
+
 
 static void srxlInitializeFrame(sbuf_t *dst)
 {
@@ -191,8 +196,8 @@ bool srxlFrameFlightPackCurrent(sbuf_t *dst, timeUs_t currentTimeUs)
         sbufWriteU16(dst, amps);
         sbufWriteU16(dst, mah);
         sbufWriteU16(dst, 0x7fff);            // temp A
-        sbufWriteU16(dst, 0xffff);            // Amps B
-        sbufWriteU16(dst, 0xffff);            // mAH B
+        sbufWriteU16(dst, 0x7fff);            // Amps B
+        sbufWriteU16(dst, 0x7fff);            // mAH B
         sbufWriteU16(dst, 0x7fff);            // temp B
         sbufWriteU16(dst, 0xffff);
 
@@ -507,16 +512,11 @@ bool checkSrxlTelemetryState(void)
  */
 void handleSrxlTelemetry(timeUs_t currentTimeUs)
 {
-    static uint32_t srxlLastCycleTime;
-
-    if (!srxlTelemetryEnabled) {
+    if (!srxlTelemetryNow) {
         return;
     }
 
-    // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
-    if (currentTimeUs >= srxlLastCycleTime + SRXL_CYCLETIME_US) {
-        srxlLastCycleTime = currentTimeUs;
-        processSrxl(currentTimeUs);
-    }
+    srxlTelemetryNow   = false;
+    processSrxl(currentTimeUs);
 }
 #endif

src/main/telemetry/srxl.h

@@ -19,6 +19,7 @@
 
 #include "common/time.h"
 
+void srxlCollectTelemetryNow(void);
 void initSrxlTelemetry(void);
 bool checkSrxlTelemetryState(void);
 void handleSrxlTelemetry(timeUs_t currentTimeUs);