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.

---
Added delay to rxfail detection

All channels are monitored for bad (out of valid range) pulses.
On bad pulses channel data will HOLD the last value for a period of
MAX_INVALID_PULS_TIME (300ms) before starting rxfail substitution.
This should prevent a too aggressive reaction to small dropouts.

---
Init ARM switch rc channel to OFF for safety

Initialize ARM switch to OFF position when arming via switch is defined.
To prevent arming during init when RX is disconnected and/or when RX is
connected but TX is still off.

---
Modified rx_rx_unittest.cc

Adapted because rxInit() parameters changed.
Added tests for ARM switch initialization.
No further tests added.

---
Move smoothing of rcData to rcCommand

Commit from @borisbstyle pr #1418
rc_smoothing function has changed to leave rcData unchanged in #1418

src/main/main.c

@@ -112,7 +112,7 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers, servoMixe
 void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers);
 #endif
 void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
-void rxInit(rxConfig_t *rxConfig);
+void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationConditions);
 void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
 void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
 void imuInit(void);
@@ -406,7 +406,7 @@ void init(void)
 
     failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 
-    rxInit(&masterConfig.rxConfig);
+    rxInit(&masterConfig.rxConfig, currentProfile->modeActivationConditions);
 
 #ifdef GPS
     if (feature(FEATURE_GPS)) {

src/main/mw.c

@@ -682,8 +682,8 @@ void filterRc(void){
 
     if (isRXDataNew) {
         for (int channel=0; channel < 4; channel++) {
-        	deltaRC[channel] = rcData[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
+        	deltaRC[channel] = rcCommand[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
-            lastCommand[channel] = rcData[channel];
+            lastCommand[channel] = rcCommand[channel];
         }
 
         isRXDataNew = false;
@@ -692,10 +692,10 @@ void filterRc(void){
         factor--;
     }
 
-    // Interpolate steps of rcData
+    // Interpolate steps of rcCommand
     if (factor > 0) {
         for (int channel=0; channel < 4; channel++) {
-            rcData[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
+            rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
          }
     } else {
         factor = 0;
@@ -768,11 +768,12 @@ void loop(void)
             }
         }
 
+        annexCode();
+
         if (masterConfig.rxConfig.rcSmoothing) {
             filterRc();
         }
 
-        annexCode();
 #if defined(BARO) || defined(SONAR)
         haveProcessedAnnexCodeOnce = true;
 #endif

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;
@@ -78,7 +81,9 @@ static uint8_t  skipRxSamples = 0;
 
 int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
 int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
+uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 
+#define MAX_INVALID_PULS_TIME    300
 #define PPM_AND_PWM_SAMPLE_COUNT 3
 
 #define DELAY_50_HZ (1000000 / 50)
@@ -145,19 +150,36 @@ void resetAllRxChannelRangeConfigurations(rxChannelRangeConfiguration_t *rxChann
     }
 }
 
-void rxInit(rxConfig_t *rxConfig)
+void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationConditions)
 {
     uint8_t i;
+    uint16_t value;
 
     useRxConfig(rxConfig);
     rcSampleIndex = 0;
 
     for (i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
         rcData[i] = rxConfig->midrc;
+        rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME;
     }
 
     rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig->midrc : rxConfig->rx_min_usec;
 
+    // Initialize ARM switch to OFF position when arming via switch is defined
+    for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
+        modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[i];
+        if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) {
+            // ARM switch is defined, determine an OFF value
+            if (modeActivationCondition->range.startStep > 0) {
+                value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationCondition->range.startStep - 1));
+            } else {
+                value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationCondition->range.endStep + 1));
+            }
+            // Initialize ARM AUX channel to OFF value
+            rcData[modeActivationCondition->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = value;
+        }
+    }
+
 #ifdef SERIAL_RX
     if (feature(FEATURE_RX_SERIAL)) {
         serialRxInit(rxConfig);
@@ -272,9 +294,7 @@ static void resetRxSignalReceivedFlagIfNeeded(uint32_t currentTime)
 
     if (((int32_t)(currentTime - needRxSignalBefore) >= 0)) {
         rxSignalReceived = false;
-#ifdef DEBUG_RX_SIGNAL_LOSS
+        rxSignalReceivedNotDataDriven = false;
-        debug[0]++;
-#endif
     }
 }
 
@@ -307,7 +327,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 +339,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 +355,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;
         }
     }
@@ -441,15 +465,23 @@ static void detectAndApplySignalLossBehaviour(void)
     uint16_t sample;
     bool useValueFromRx = true;
     bool rxIsDataDriven = isRxDataDriven();
+    uint32_t currentMilliTime = millis();
 
-    if (!rxSignalReceived) {
+    if (!rxIsDataDriven) {
-        if (rxIsDataDriven && rxDataReceived) {
+        rxSignalReceived = rxSignalReceivedNotDataDriven;
-            // use the values from the RX
+        rxIsInFailsafeMode = rxIsInFailsafeModeNotDataDriven;
-        } else {
-            useValueFromRx = false;
-        }
     }
 
+    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++) {
@@ -459,11 +491,16 @@ static void detectAndApplySignalLossBehaviour(void)
         bool validPulse = isPulseValid(sample);
 
         if (!validPulse) {
-            sample = getRxfailValue(channel);
+            if (currentMilliTime < rcInvalidPulsPeriod[channel]) {
+                sample = rcData[channel];           // hold channel for MAX_INVALID_PULS_TIME
+            } else {
+                sample = getRxfailValue(channel);   // after that apply rxfail value
+                rxUpdateFlightChannelStatus(channel, validPulse);
+            }
+        } else {
+            rcInvalidPulsPeriod[channel] = currentMilliTime + MAX_INVALID_PULS_TIME;
         }
 
-        rxUpdateFlightChannelStatus(channel, validPulse);
-
         if (rxIsDataDriven) {
             rcData[channel] = sample;
         } else {
@@ -476,7 +513,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 +521,9 @@ static void detectAndApplySignalLossBehaviour(void)
         }
     }
 
+#ifdef DEBUG_RX_SIGNAL_LOSS
+    debug[3] = rcData[THROTTLE];
+#endif
 }
 
 void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime)

src/test/unit/rx_rx_unittest.cc

@@ -24,10 +24,12 @@ extern "C" {
     #include "platform.h"
 
     #include "rx/rx.h"
+    #include "io/rc_controls.h"
+    #include "common/maths.h"
 
     uint32_t rcModeActivationMask;
 
-    void rxInit(rxConfig_t *rxConfig);
+    void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationConditions);
     void rxResetFlightChannelStatus(void);
     bool rxHaveValidFlightChannels(void);
     bool isPulseValid(uint16_t pulseDuration);
@@ -54,16 +56,28 @@ TEST(RxTest, TestValidFlightChannels)
 
     // and
     rxConfig_t rxConfig;
+    modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
 
     memset(&rxConfig, 0, sizeof(rxConfig));
     rxConfig.rx_min_usec = 1000;
     rxConfig.rx_max_usec = 2000;
 
-    // when
+    memset(&modeActivationConditions, 0, sizeof(modeActivationConditions));
-    rxInit(&rxConfig);
+    modeActivationConditions[0].auxChannelIndex = 0;
+    modeActivationConditions[0].modeId = BOXARM;
+    modeActivationConditions[0].range.startStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MIN);
+    modeActivationConditions[0].range.endStep = CHANNEL_VALUE_TO_STEP(1600);
 
+    // when
+    rxInit(&rxConfig, modeActivationConditions);
+
+    // then (ARM channel should be positioned just 1 step above active range to init to OFF)
+    EXPECT_EQ(1625, rcData[modeActivationConditions[0].auxChannelIndex +  NON_AUX_CHANNEL_COUNT]);
+
+    // given
     rxResetFlightChannelStatus();
 
+    // and
     for (uint8_t channelIndex = 0; channelIndex < MAX_SUPPORTED_RC_CHANNEL_COUNT; channelIndex++) {
         bool validPulse = isPulseValid(1500);
         rxUpdateFlightChannelStatus(channelIndex, validPulse);
@@ -80,20 +94,29 @@ TEST(RxTest, TestInvalidFlightChannels)
 
     // and
     rxConfig_t rxConfig;
+    modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
 
     memset(&rxConfig, 0, sizeof(rxConfig));
     rxConfig.rx_min_usec = 1000;
     rxConfig.rx_max_usec = 2000;
 
+    memset(&modeActivationConditions, 0, sizeof(modeActivationConditions));
+    modeActivationConditions[0].auxChannelIndex = 0;
+    modeActivationConditions[0].modeId = BOXARM;
+    modeActivationConditions[0].range.startStep = CHANNEL_VALUE_TO_STEP(1400);
+    modeActivationConditions[0].range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
+
     // and
     uint16_t channelPulses[MAX_SUPPORTED_RC_CHANNEL_COUNT];
     memset(&channelPulses, 1500, sizeof(channelPulses));
 
     // and
-    rxInit(&rxConfig);
+    rxInit(&rxConfig, modeActivationConditions);
+
+    // then (ARM channel should be positioned just 1 step below active range to init to OFF)
+    EXPECT_EQ(1375, rcData[modeActivationConditions[0].auxChannelIndex +  NON_AUX_CHANNEL_COUNT]);
 
     // and
-
     for (uint8_t stickChannelIndex = 0; stickChannelIndex < STICK_CHANNEL_COUNT; stickChannelIndex++) {
 
         // given