From 83a41565f6ae8715235231623694a80d532bc39a Mon Sep 17 00:00:00 2001
From: ProDrone <me@prodrone.com>
Date: Tue, 29 Sep 2015 17:50:31 +0200
Subject: [PATCH] Improved RX failsafe detection & handling

modified debug output (currently disabled)

To solve problem as indicated here:
https://github.com/cleanflight/cleanflight/issues/1266#issuecomment-135640133

and here:
https://github.com/cleanflight/cleanflight/pull/1340

and here:
https://github.com/cleanflight/cleanflight/pull/1342

Tested on FrSKY X4RSB with latest CPPM firmware (non-EU version).
Firmware filename: X4R-X4RSB_cppm_non-EU_150630

In both SBUS and CPPM mode.
---
 src/main/rx/rx.c | 42 +++++++++++++++++++++++++++++-------------
 1 file changed, 29 insertions(+), 13 deletions(-)

diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index 7a46d60a03..ba6c7c183f 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -69,7 +69,10 @@ uint16_t rssi = 0;                  // range: [0;1023]
 
 static bool rxDataReceived = false;
 static bool rxSignalReceived = false;
+static bool rxSignalReceivedNotDataDriven = false;
 static bool rxFlightChannelsValid = false;
+static bool rxIsInFailsafeMode = true;
+static bool rxIsInFailsafeModeNotDataDriven = true;
 
 static uint32_t rxUpdateAt = 0;
 static uint32_t needRxSignalBefore = 0;
@@ -272,9 +275,7 @@ static void resetRxSignalReceivedFlagIfNeeded(uint32_t currentTime)
 
     if (((int32_t)(currentTime - needRxSignalBefore) >= 0)) {
         rxSignalReceived = false;
-#ifdef DEBUG_RX_SIGNAL_LOSS
-        debug[0]++;
-#endif
+        rxSignalReceivedNotDataDriven = false;
     }
 }
 
@@ -307,7 +308,8 @@ void updateRx(uint32_t currentTime)
 
         if (frameStatus & SERIAL_RX_FRAME_COMPLETE) {
             rxDataReceived = true;
-            rxSignalReceived = (frameStatus & SERIAL_RX_FRAME_FAILSAFE) == 0;
+            rxIsInFailsafeMode = (frameStatus & SERIAL_RX_FRAME_FAILSAFE) != 0;
+            rxSignalReceived = !rxIsInFailsafeMode;
             needRxSignalBefore = currentTime + DELAY_10_HZ;
         }
     }
@@ -318,13 +320,15 @@ void updateRx(uint32_t currentTime)
 
         if (rxDataReceived) {
             rxSignalReceived = true;
+            rxIsInFailsafeMode = false;
             needRxSignalBefore = currentTime + DELAY_5_HZ;
         }
     }
 
     if (feature(FEATURE_RX_PPM)) {
         if (isPPMDataBeingReceived()) {
-            rxSignalReceived = true;
+            rxSignalReceivedNotDataDriven = true;
+            rxIsInFailsafeModeNotDataDriven = false;
             needRxSignalBefore = currentTime + DELAY_10_HZ;
             resetPPMDataReceivedState();
         }
@@ -332,7 +336,8 @@ void updateRx(uint32_t currentTime)
 
     if (feature(FEATURE_RX_PARALLEL_PWM)) {
         if (isPWMDataBeingReceived()) {
-            rxSignalReceived = true;
+            rxSignalReceivedNotDataDriven = true;
+            rxIsInFailsafeModeNotDataDriven = false;
             needRxSignalBefore = currentTime + DELAY_10_HZ;
         }
     }
@@ -442,14 +447,21 @@ static void detectAndApplySignalLossBehaviour(void)
     bool useValueFromRx = true;
     bool rxIsDataDriven = isRxDataDriven();
 
-    if (!rxSignalReceived) {
-        if (rxIsDataDriven && rxDataReceived) {
-            // use the values from the RX
-        } else {
-            useValueFromRx = false;
-        }
+    if (!rxIsDataDriven) {
+        rxSignalReceived = rxSignalReceivedNotDataDriven;
+        rxIsInFailsafeMode = rxIsInFailsafeModeNotDataDriven;
     }
 
+    if (!rxSignalReceived || rxIsInFailsafeMode) {
+        useValueFromRx = false;
+    }
+
+#ifdef DEBUG_RX_SIGNAL_LOSS
+    debug[0] = rxSignalReceived;
+    debug[1] = rxIsInFailsafeMode;
+    debug[2] = rcReadRawFunc(&rxRuntimeConfig, 0);
+#endif
+
     rxResetFlightChannelStatus();
 
     for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
@@ -476,7 +488,7 @@ static void detectAndApplySignalLossBehaviour(void)
     if ((rxFlightChannelsValid) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
         failsafeOnValidDataReceived();
     } else {
-        rxSignalReceived = false;
+        rxIsInFailsafeMode = rxIsInFailsafeModeNotDataDriven = true;
         failsafeOnValidDataFailed();
 
         for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
@@ -484,6 +496,10 @@ static void detectAndApplySignalLossBehaviour(void)
         }
     }
 
+#ifdef DEBUG_RX_SIGNAL_LOSS
+    debug[3] = rcData[THROTTLE];
+#endif
+
 }
 
 void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime)