Merge pull request #6076 from mikeller/fix_frsky_x_spi_dropouts

Fixed overflow FrSky X channel decoding.

src/main/build/debug.c

@@ -72,4 +72,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "ITERM_RELAX",
     "ACRO_TRAINER",
     "RC_SMOOTHING",
+    "RX_SIGNAL_LOSS",    
 };

src/main/build/debug.h

@@ -90,6 +90,7 @@ typedef enum {
     DEBUG_ITERM_RELAX,
     DEBUG_ACRO_TRAINER,
     DEBUG_RC_SMOOTHING,
+    DEBUG_RX_SIGNAL_LOSS,
     DEBUG_COUNT
 } debugType_e;
 

src/main/rx/cc2500_frsky_shared.h

@@ -25,6 +25,9 @@
 #define MAX_MISSING_PKT 100
 
 #define DEBUG_DATA_ERROR_COUNT 0
+#define DEBUG_DATA_MISSING_PACKETS 1
+#define DEBUG_DATA_BAD_FRAME 2
+
 
 #define SYNC_DELAY_MAX 9000
 

src/main/rx/cc2500_frsky_x.c

@@ -153,8 +153,9 @@ static bool telemetryEnabled = false;
 
 static uint16_t calculateCrc(uint8_t *data, uint8_t len) {
     uint16_t crc = 0;
-    for(uint8_t i=0; i < len; i++)
-        crc = (crc<<8) ^ (crcTable[((uint8_t)(crc>>8) ^ *data++) & 0xFF]);
+    for (unsigned i = 0; i < len; i++) {
+        crc = (crc << 8) ^ (crcTable[((uint8_t)(crc >> 8) ^ *data++) & 0xFF]);
+    }
     return crc;
 }
 
@@ -271,7 +272,6 @@ static void frSkyXTelemetryWriteFrame(const smartPortPayload_t *payload)
 void frSkyXSetRcData(uint16_t *rcData, const uint8_t *packet)
 {
     uint16_t c[8];
-
     c[0] = (uint16_t)((packet[10] << 8) & 0xF00) | packet[9];
     c[1] = (uint16_t)((packet[11] << 4) & 0xFF0) | (packet[10] >> 4);
     c[2] = (uint16_t)((packet[13] << 8) & 0xF00) | packet[12];
@@ -281,18 +281,13 @@ void frSkyXSetRcData(uint16_t *rcData, const uint8_t *packet)
     c[6] = (uint16_t)((packet[19] << 8) & 0xF00) | packet[18];
     c[7] = (uint16_t)((packet[20] << 4) & 0xFF0) | (packet[19] >> 4);
 
-    uint8_t j = 0;
-    for(uint8_t i = 0; i < 8; i++) {
-        if(c[i] > 2047) {
-            j = 8;
-            c[i] = c[i] - 2048;
-        } else {
-            j = 0;
-        }
-        int16_t temp = (((c[i] - 64) << 1) / 3 + 860);
-        if ((temp > 800) && (temp < 2200)) {
-            rcData[i+j] = temp;
+    for (unsigned i = 0; i < 8; i++) {
+        bool channelIsShifted = false;
+        if (c[i] & 0x800) {
+            channelIsShifted = true;
         }
+        const uint16_t channelValue = c[i] & 0x7FF;
+        rcData[channelIsShifted ? i + 8 : i] = channelValue * 2.0f / 3 + 860 - 64 * 2 / 3;
     }
 }
 
@@ -313,6 +308,7 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
     static bool frameReceived;
     static timeDelta_t receiveDelayUs;
     static uint8_t channelsToSkip = 1;
+    static uint32_t packetErrors = 0;
 
     static telemetryBuffer_t telemetryRxBuffer[TELEMETRY_SEQUENCE_LENGTH];
 
@@ -433,6 +429,10 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
                             frameReceived = true; // no need to process frame again.
                         }
                     }
+                } 
+                if (!frameReceived) {
+                    packetErrors++;
+                    DEBUG_SET(DEBUG_RX_FRSKY_SPI, DEBUG_DATA_BAD_FRAME, packetErrors);
                 }
             }
         }
@@ -500,7 +500,9 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
             }
 #endif
             *protocolState = STATE_RESUME;
-            ret = RX_SPI_RECEIVED_DATA;
+            if (frameReceived) {
+                ret = RX_SPI_RECEIVED_DATA;
+            }
         }
 
         break;
@@ -528,6 +530,8 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
                 break;
             }
             missingPackets++;
+            DEBUG_SET(DEBUG_RX_FRSKY_SPI, DEBUG_DATA_MISSING_PACKETS, missingPackets);
+            
             *protocolState = STATE_DATA;
         }
         break;

src/main/rx/rx.c

@@ -68,8 +68,6 @@
 #include "rx/targetcustomserial.h"
 
 
-//#define DEBUG_RX_SIGNAL_LOSS
-
 const char rcChannelLetters[] = "AERT12345678abcdefgh";
 
 static uint16_t rssi = 0;                  // range: [0;1023]
@@ -492,11 +490,8 @@ static void detectAndApplySignalLossBehaviour(void)
 
     const bool useValueFromRx = rxSignalReceived && !rxIsInFailsafeMode;
 
-#ifdef DEBUG_RX_SIGNAL_LOSS
-    debug[0] = rxSignalReceived;
-    debug[1] = rxIsInFailsafeMode;
-    debug[2] = rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, 0);
-#endif
+    DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 0, rxSignalReceived);
+    DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 1, rxIsInFailsafeMode);
 
     rxFlightChannelsValid = true;
     for (int channel = 0; channel < rxChannelCount; channel++) {
@@ -533,10 +528,7 @@ static void detectAndApplySignalLossBehaviour(void)
             rcData[channel] = getRxfailValue(channel);
         }
     }
-
-#ifdef DEBUG_RX_SIGNAL_LOSS
-    debug[3] = rcData[THROTTLE];
-#endif
+    DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 3, rcData[THROTTLE]);
 }
 
 bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)

src/test/unit/rx_ranges_unittest.cc

@@ -22,7 +22,7 @@
 
 extern "C" {
     #include "platform.h"
-
+    #include "build/debug.h"
     #include "drivers/io.h"
     #include "common/maths.h"
     #include "pg/pg.h"
@@ -38,6 +38,8 @@ extern "C" {
 
 extern "C" {
 boxBitmask_t rcModeActivationMask;
+int16_t debug[DEBUG16_VALUE_COUNT];
+uint8_t debugMode = 0;
 
 extern uint16_t applyRxChannelRangeConfiguraton(int sample, const rxChannelRangeConfig_t *range);
 }

src/test/unit/rx_rx_unittest.cc

@@ -24,6 +24,7 @@ extern "C" {
     #include "platform.h"
 
     #include "pg/rx.h"
+    #include "build/debug.h"
     #include "drivers/io.h"
     #include "rx/rx.h"
     #include "fc/rc_modes.h"
@@ -35,6 +36,8 @@ extern "C" {
     #include "io/beeper.h"
 
     boxBitmask_t rcModeActivationMask;
+    int16_t debug[DEBUG16_VALUE_COUNT];
+    uint8_t debugMode = 0;
 
     bool isPulseValid(uint16_t pulseDuration);