Merge remote-tracking branch 'prodrone/improved_rx_failsafe_detection' into betaflight

Conflicts:
	src/main/drivers/accgyro_mpu6500.c
	src/main/drivers/accgyro_spi_mpu6000.c
	src/main/drivers/system.c
	src/main/mw.c
	src/main/rx/rx.c
	src/main/sensors/initialisation.c

Makefile

@@ -540,6 +540,7 @@ COLIBRI_RACE_SRC = \
 		   drivers/accgyro_mpu.c \
 		   drivers/accgyro_mpu6500.c \
 		   drivers/accgyro_spi_mpu6500.c \
+		   drivers/accgyro_mpu6500.c \
 		   drivers/barometer_ms5611.c \
 		   drivers/compass_ak8975.c \
 		   drivers/compass_hmc5883l.c \

src/main/main.c

@@ -113,7 +113,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 displayInit(rxConfig_t *intialRxConfig);
@@ -407,7 +407,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/rx/rx.c

@@ -81,9 +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]
-int8_t  rcInvalidPulsCounter[MAX_SUPPORTED_RC_CHANNEL_COUNT];
+uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 
-#define MAX_INVALID_PULS_COUNTS  20
+#define MAX_INVALID_PULS_TIME    300
 #define PPM_AND_PWM_SAMPLE_COUNT 3
 
 #define DELAY_50_HZ (1000000 / 50)
@@ -150,20 +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;
-        rcInvalidPulsCounter[i] = MAX_INVALID_PULS_COUNTS;
+        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);
@@ -449,6 +465,7 @@ static void detectAndApplySignalLossBehaviour(void)
     uint16_t sample;
     bool useValueFromRx = true;
     bool rxIsDataDriven = isRxDataDriven();
+    uint32_t currentMilliTime = millis();
 
     if (!rxIsDataDriven) {
         rxSignalReceived = rxSignalReceivedNotDataDriven;
@@ -474,15 +491,14 @@ static void detectAndApplySignalLossBehaviour(void)
         bool validPulse = isPulseValid(sample);
 
         if (!validPulse) {
-            if (rcInvalidPulsCounter[channel]) {
+            if (currentMilliTime < rcInvalidPulsPeriod[channel]) {
-                rcInvalidPulsCounter[channel]--;
+                sample = rcData[channel];           // hold channel for MAX_INVALID_PULS_TIME
-                sample = rcData[channel];           // hold channel for MAX_INVALID_PULS_COUNTS
             } else {
                 sample = getRxfailValue(channel);   // after that apply rxfail value
                 rxUpdateFlightChannelStatus(channel, validPulse);
             }
         } else {
-            rcInvalidPulsCounter[channel] = MAX_INVALID_PULS_COUNTS;
+            rcInvalidPulsPeriod[channel] = currentMilliTime + MAX_INVALID_PULS_TIME;
         }
 
         if (rxIsDataDriven) {
@@ -508,7 +524,6 @@ 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