From 7e54f8cdc9e3a2ea522ce3954145f6f68532e467 Mon Sep 17 00:00:00 2001
From: Anders Hoglund <AndWho@gmail.com>
Date: Tue, 30 Jan 2018 22:32:02 +0100
Subject: [PATCH] Spektrum protocol corrections, VTX ctrl and srxl tm in
 correct phases.

---
 src/main/io/spektrum_vtx_control.h |  1 +
 src/main/rx/spektrum.c             | 37 ++++++++++++++++--------------
 2 files changed, 21 insertions(+), 17 deletions(-)

diff --git a/src/main/io/spektrum_vtx_control.h b/src/main/io/spektrum_vtx_control.h
index ca97d0b17..ff79bd0f3 100644
--- a/src/main/io/spektrum_vtx_control.h
+++ b/src/main/io/spektrum_vtx_control.h
@@ -25,6 +25,7 @@
 #define SPEKTRUM_VTX_CONTROL_2          (SPEK_FRAME_SIZE - 3)
 #define SPEKTRUM_VTX_CONTROL_3          (SPEK_FRAME_SIZE - 2)
 #define SPEKTRUM_VTX_CONTROL_4          (SPEK_FRAME_SIZE - 1)
+#define SPEKTRUM_VTX_CONTROL_SIZE       4
 
 #define SPEKTRUM_VTX_BAND_MASK          0x00e00000
 #define SPEKTRUM_VTX_CHANNEL_MASK       0x000f0000
diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c
index d27e803bf..d69163545 100644
--- a/src/main/rx/spektrum.c
+++ b/src/main/rx/spektrum.c
@@ -111,8 +111,26 @@ static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
     spektrumHandleRSSI(spekFrame);
 #endif
 
+    // Get the VTX control bytes in a frame
+    uint32_t vtxControl = ((spekFrame[SPEKTRUM_VTX_CONTROL_1] << 24) |
+                           (spekFrame[SPEKTRUM_VTX_CONTROL_2] << 16) |
+                           (spekFrame[SPEKTRUM_VTX_CONTROL_3] <<  8) |
+                           (spekFrame[SPEKTRUM_VTX_CONTROL_4] <<  0) );
+
+    int8_t spektrumRcDataSize;
+    // Handle VTX control frame.
+    if ((vtxControl & SPEKTRUM_VTX_CONTROL_FRAME_MASK) == SPEKTRUM_VTX_CONTROL_FRAME &&
+        (spekFrame[2] & 0x80) == 0 )  {
+#if defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
+      spektrumHandleVtxControl(vtxControl);
+#endif
+      spektrumRcDataSize = SPEK_FRAME_SIZE - SPEKTRUM_VTX_CONTROL_SIZE;
+    } else {
+      spektrumRcDataSize = SPEK_FRAME_SIZE;
+    }
+
     // Get the RC control channel inputs
-    for (int b = 3; b < SPEK_FRAME_SIZE; b += 2) {
+    for (int b = 3; b < spektrumRcDataSize; b += 2) {
         const uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift);
         if (spekChannel < rxRuntimeConfigPtr->channelCount && spekChannel < SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT) {
             if (rssi_channel == 0 || spekChannel != rssi_channel) {
@@ -121,23 +139,8 @@ static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
         }
     }
 
-#if defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
-
-    // Get the VTX control bytes in a frame
-    uint32_t vtxControl = ((spekFrame[SPEKTRUM_VTX_CONTROL_1] << 24) |
-                           (spekFrame[SPEKTRUM_VTX_CONTROL_2] << 16) |
-                           (spekFrame[SPEKTRUM_VTX_CONTROL_3] <<  8) |
-                           (spekFrame[SPEKTRUM_VTX_CONTROL_4] <<  0) );
-
-    // Handle VTX control frame,
-    if ((vtxControl & SPEKTRUM_VTX_CONTROL_FRAME_MASK) == SPEKTRUM_VTX_CONTROL_FRAME) {
-      spektrumHandleVtxControl(vtxControl);
-    }
-
-#endif // USE_SPEKTRUM_VTX_CONTROL && VTX_COMMON
-
     /* only process if srxl enabled, some data in buffer AND servos in phase 0 */
-    if (srxlEnabled && telemetryBufLen && (spekFrame[2] & 0x80)) {
+    if (srxlEnabled && telemetryBufLen && (spekFrame[2] & 0x80) == 0) {
         dispatchAdd(&srxlTelemetryDispatch, SPEKTRUM_TELEMETRY_FRAME_DELAY);
     }
     return RX_FRAME_COMPLETE;