From 5950237aafd91e0a56ea06363554c01f05cb1a95 Mon Sep 17 00:00:00 2001
From: "Konstantin Sharlaimov (DigitalEntity)"
 <konstantin.sharlaimov@gmail.com>
Date: Sat, 30 Jun 2018 18:16:01 +0200
Subject: [PATCH] Don't allow data that RC receiver sends as failsafe values to
 pollute RC filter and rcData. Only meaningful for digital receivers that
 signal the link loss explicitly

---
 src/main/fc/fc_msp.c |  2 +-
 src/main/rx/rx.c     | 17 ++++++++++++++---
 src/main/rx/rx.h     |  1 +
 3 files changed, 16 insertions(+), 4 deletions(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index aad30560fd0..80d3e7b9f6d 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -484,7 +484,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 
     case MSP_RC:
         for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
-            sbufWriteU16(dst, rcData[i]);
+            sbufWriteU16(dst, rcRaw[i]);
         }
         break;
 
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index 7f0c85ebc4e..3dbe47ebb38 100755
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -469,6 +469,7 @@ void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
 
+    int16_t rcStaging[MAX_SUPPORTED_RC_CHANNEL_COUNT];
     const timeMs_t currentTimeMs = millis();
 
     if (auxiliaryProcessingRequired) {
@@ -519,11 +520,21 @@ void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
             rcInvalidPulsPeriod[channel] = currentTimeMs + MAX_INVALID_PULS_TIME;
         }
 
-        // Update rcData channel value
+        // Save channel value
+        rcStaging[channel] = sample;
+    }
+
+    // Update rcData channel value if receiver is not in failsafe mode
+    // If receiver is in failsafe (not receiving signal or sending invalid channel values) - last good rcData values are retained
+    if (rxFlightChannelsValid && rxSignalReceived) {
         if (rxRuntimeConfig.requireFiltering) {
-            rcData[channel] = applyChannelFiltering(channel, sample);
+            for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
+                rcData[channel] = applyChannelFiltering(channel, rcStaging[channel]);
+            }
         } else {
-            rcData[channel] = sample;
+            for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
+                rcData[channel] = rcStaging[channel];
+            }
         }
     }
 
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
index 6ee9da5b556..26da0bbaab0 100644
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@@ -93,6 +93,7 @@ typedef enum {
 
 extern const char rcChannelLetters[];
 
+extern int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT];        // interval [1000;2000]
 extern int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];       // interval [1000;2000]
 
 #define MAX_MAPPABLE_RX_INPUTS 4