Merge pull request #1374 from ProDrone/improved_rx_failsafe_detection

RX - Improved RX failsafe detection & handling

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