Failsafe - Allow AUX channels to HOLD their last value or be SET to a

specific value when RX loss is detected.

Tested with SBus, PWM & PPM.

src/main/config/config.c

@@ -404,7 +404,10 @@ static void resetConf(void)
     masterConfig.rxConfig.rx_max_usec = 2115;         // any of first 4 channels above this value will trigger rx loss detection
 
     for (i = 0; i < MAX_AUX_CHANNEL_COUNT; i++) {
-        masterConfig.rxConfig.rx_fail_usec_steps[i] = CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.midrc);
+        rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_aux_channel_configurations[i];
+
+        channelFailsafeConfiguration->mode = RX_FAILSAFE_MODE_HOLD;
+        channelFailsafeConfiguration->step = CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.midrc);
     }
 
     masterConfig.rxConfig.rssi_channel = 0;

src/main/io/serial_cli.c

@@ -571,11 +571,32 @@ static void cliRxFail(char *cmdline)
             cliRxFail(itoa(channel, buf, 10));
         }
     } else {
-        uint16_t value;
         char *ptr = cmdline;
 
         channel = atoi(ptr++);
         if ((channel < MAX_AUX_CHANNEL_COUNT)) {
+
+            rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_aux_channel_configurations[channel];
+
+            uint16_t value;
+            rxFailsafeChannelMode_e mode;
+
+            ptr = strchr(ptr, ' ');
+            if (ptr) {
+                switch (*(++ptr)) {
+                    case 'h':
+                        mode = RX_FAILSAFE_MODE_HOLD;
+                        break;
+
+                    case 's':
+                        mode = RX_FAILSAFE_MODE_SET;
+                        break;
+                    default:
+                        cliShowParseError();
+                        return;
+                }
+            }
+
             ptr = strchr(ptr, ' ');
             if (ptr) {
                 value = atoi(++ptr);
@@ -584,15 +605,21 @@ static void cliRxFail(char *cmdline)
                     cliPrint("Value out of range\r\n");
                     return;
                 }
-                masterConfig.rxConfig.rx_fail_usec_steps[channel] = value;
+
+                channelFailsafeConfiguration->mode = mode;
+                channelFailsafeConfiguration->step = value;
             }
+
+            char modeCharacter = channelFailsafeConfiguration->mode == RX_FAILSAFE_MODE_SET ? 's' : 'h';
             // triple use of printf below
             // 1. acknowledge interpretation on command,
             // 2. query current setting on single item,
             // 3. recursive use for full list.
-            printf("rxfail %u %d\r\n",
+
+            printf("rxfail %u %c %d\r\n",
                 channel,
-                RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.rx_fail_usec_steps[channel])
+                modeCharacter,
+                RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfiguration->step)
             );
         } else {
             printf("channel must be < %u\r\n", MAX_AUX_CHANNEL_COUNT);

src/main/io/serial_msp.c

@@ -1164,9 +1164,10 @@ static bool processOutCommand(uint8_t cmdMSP)
         break;
 
     case MSP_RXFAIL_CONFIG:
-        headSerialReply(2 * (rxRuntimeConfig.channelCount-4));
-        for (i = 4; i < rxRuntimeConfig.channelCount; i++) {
-            serialize16(RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.rx_fail_usec_steps[i-4]));
+        headSerialReply(3 * (rxRuntimeConfig.channelCount - NON_AUX_CHANNEL_COUNT));
+        for (i = NON_AUX_CHANNEL_COUNT; i < rxRuntimeConfig.channelCount; i++) {
+            serialize8(masterConfig.rxConfig.failsafe_aux_channel_configurations[i - NON_AUX_CHANNEL_COUNT].mode);
+            serialize16(RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.failsafe_aux_channel_configurations[i - NON_AUX_CHANNEL_COUNT].step));
         }
         break;
 
@@ -1597,12 +1598,14 @@ static bool processInCommand(void)
 
     case MSP_SET_RXFAIL_CONFIG:
         {
-            uint8_t channelCount = currentPort->dataSize / sizeof(uint16_t);
-            if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT-4) {
+            uint8_t channelCount = currentPort->dataSize / 3;
+            if (channelCount > MAX_AUX_CHANNEL_COUNT) {
                 headSerialError(0);
             } else {
-                for (i = 4; i < channelCount; i++)
-                    masterConfig.rxConfig.rx_fail_usec_steps[i-4] = CHANNEL_VALUE_TO_RXFAIL_STEP(read16());
+                for (i = NON_AUX_CHANNEL_COUNT; i < channelCount; i++) {
+                    masterConfig.rxConfig.failsafe_aux_channel_configurations[i - NON_AUX_CHANNEL_COUNT].mode = read8();
+                    masterConfig.rxConfig.failsafe_aux_channel_configurations[i - NON_AUX_CHANNEL_COUNT].step = CHANNEL_VALUE_TO_RXFAIL_STEP(read16());
+                }
             }
         }
         break;

src/main/rx/rx.c

@@ -319,11 +319,24 @@ static uint16_t getRxfailValue(uint8_t channel)
         case YAW:
             return rxConfig->midrc;
         case THROTTLE:
-            if (feature(FEATURE_3D)) return rxConfig->midrc;
-            else return rxConfig->rx_min_usec;
-        default:
-            return RXFAIL_STEP_TO_CHANNEL_VALUE(rxConfig->rx_fail_usec_steps[channel - NON_AUX_CHANNEL_COUNT]);
+            if (feature(FEATURE_3D))
+                return rxConfig->midrc;
+            else
+                return rxConfig->rx_min_usec;
     }
+
+    rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig->failsafe_aux_channel_configurations[channel - NON_AUX_CHANNEL_COUNT];
+
+    switch(channelFailsafeConfiguration->mode) {
+        default:
+        case RX_FAILSAFE_MODE_HOLD:
+            return rcData[channel];
+
+        case RX_FAILSAFE_MODE_SET:
+            return RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfiguration->step);
+
+    }
+
 }
 
 static void processRxChannels(void)

src/main/rx/rx.h

@@ -81,6 +81,16 @@ extern int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];       // interval [1000;2
 #define RSSI_SCALE_MAX 255
 #define RSSI_SCALE_DEFAULT 30
 
+typedef enum {
+    RX_FAILSAFE_MODE_HOLD = 0,
+    RX_FAILSAFE_MODE_SET
+} rxFailsafeChannelMode_e;
+
+typedef struct rxFailsafeChannelConfiguration_s {
+    uint8_t mode; // See rxFailsafeChannelMode_e
+    uint8_t step;
+} rxFailsafeChannelConfiguration_t;
+
 typedef struct rxConfig_s {
     uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS];  // mapping of radio channels to internal RPYTA+ order
     uint8_t serialrx_provider;              // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first.
@@ -94,7 +104,7 @@ typedef struct rxConfig_s {
 
     uint16_t rx_min_usec;
     uint16_t rx_max_usec;
-    uint8_t rx_fail_usec_steps[MAX_AUX_CHANNEL_COUNT];
+    rxFailsafeChannelConfiguration_t failsafe_aux_channel_configurations[MAX_AUX_CHANNEL_COUNT];
 
 } rxConfig_t;