Add failsafe_procedure

src/main/config/config.c

@@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0;
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
 
-static const uint8_t EEPROM_CONF_VERSION = 121;
+static const uint8_t EEPROM_CONF_VERSION = 122;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 {
@@ -514,6 +514,7 @@ static void resetConf(void)
     masterConfig.failsafeConfig.failsafe_throttle = 1000;         // default throttle off.
     masterConfig.failsafeConfig.failsafe_kill_switch = 0;         // default failsafe switch action is identical to rc link loss
     masterConfig.failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition
+    masterConfig.failsafeConfig.failsafe_procedure = 0;           // default full failsafe procedure is 0: auto-landing
 
 #ifdef USE_SERVOS
     // servos

src/main/flight/failsafe.c

@@ -18,7 +18,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#include "platform.h"
+#include <platform.h>
 
 #include "debug.h"
 
@@ -167,7 +167,7 @@ void failsafeUpdateState(void)
         return;
     }
 
-    bool receivingRxData = failsafeIsReceivingRxData();        // FIXME - Original check for received data
+    bool receivingRxData = failsafeIsReceivingRxData();
     bool armed = ARMING_FLAG(ARMED);
     bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
     beeperMode_e beeperMode = BEEPER_SILENCE;
@@ -222,8 +222,20 @@ void failsafeUpdateState(void)
                 if (receivingRxData) {
                     failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
                 } else {
-                    // Stabilize, and set Throttle to specified level
+                    switch (failsafeConfig->failsafe_procedure) {
-                    failsafeActivate();
+                        default:
+                        case FAILSAFE_PROCEDURE_AUTO_LANDING:
+                            // Stabilize, and set Throttle to specified level
+                            failsafeActivate();
+                            break;
+
+                        case FAILSAFE_PROCEDURE_DROP_IT:
+                            // Drop the craft
+                            failsafeActivate();
+                            failsafeState.phase = FAILSAFE_LANDED;      // skip auto-landing procedure
+                            failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData
+                            break;
+                    }
                 }
                 reprocessState = true;
                 break;

src/main/flight/failsafe.h

@@ -33,6 +33,7 @@ typedef struct failsafeConfig_s {
     uint16_t failsafe_throttle;             // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
     uint8_t failsafe_kill_switch;           // failsafe switch action is 0: identical to rc link loss, 1: disarms instantly
     uint16_t failsafe_throttle_low_delay;   // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure".
+    uint8_t failsafe_procedure;             // selected full failsafe procedure is 0: auto-landing, 1: Drop it
 } failsafeConfig_t;
 
 typedef enum {
@@ -49,6 +50,11 @@ typedef enum {
     FAILSAFE_RXLINK_UP
 } failsafeRxLinkState_e;
 
+typedef enum {
+    FAILSAFE_PROCEDURE_AUTO_LANDING = 0,
+    FAILSAFE_PROCEDURE_DROP_IT
+} failsafeProcedure_e;
+
 typedef struct failsafeState_s {
     int16_t events;
     bool monitoring;
@@ -82,4 +88,3 @@ void failsafeOnValidDataFailed(void);
 
 
 
-

src/main/io/serial_cli.c

@@ -648,6 +648,7 @@ const clivalue_t valueTable[] = {
     { "failsafe_throttle",          VAR_UINT16 | MASTER_VALUE,  &masterConfig.failsafeConfig.failsafe_throttle, .config.minmax = { PWM_RANGE_MIN,  PWM_RANGE_MAX } },
     { "failsafe_kill_switch",       VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.failsafeConfig.failsafe_kill_switch, .config.lookup = { TABLE_OFF_ON } },
     { "failsafe_throttle_low_delay",VAR_UINT16 | MASTER_VALUE,  &masterConfig.failsafeConfig.failsafe_throttle_low_delay, .config.minmax = { 0,  300 } },
+    { "failsafe_procedure",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.failsafeConfig.failsafe_procedure, .config.lookup = { TABLE_OFF_ON } },
 
     { "rx_min_usec",                VAR_UINT16 | MASTER_VALUE,  &masterConfig.rxConfig.rx_min_usec, .config.minmax = { PWM_PULSE_MIN,  PWM_PULSE_MAX } },
     { "rx_max_usec",                VAR_UINT16 | MASTER_VALUE,  &masterConfig.rxConfig.rx_max_usec, .config.minmax = { PWM_PULSE_MIN,  PWM_PULSE_MAX } },

src/main/io/serial_msp.c

@@ -1068,7 +1068,7 @@ static bool processOutCommand(uint8_t cmdMSP)
         serialize16(masterConfig.failsafeConfig.failsafe_throttle);
         serialize8(masterConfig.failsafeConfig.failsafe_kill_switch);
         serialize16(masterConfig.failsafeConfig.failsafe_throttle_low_delay);
-        serialize8(1);
+        serialize8(masterConfig.failsafeConfig.failsafe_procedure);
         break;
 
     case MSP_RXFAIL_CONFIG:
@@ -1613,8 +1613,7 @@ static bool processInCommand(void)
         masterConfig.failsafeConfig.failsafe_throttle = read16();
         masterConfig.failsafeConfig.failsafe_kill_switch = read8();
         masterConfig.failsafeConfig.failsafe_throttle_low_delay = read16();
-        //masterConfig.failsafeConfig.failsafe_procedure = read8();
+        masterConfig.failsafeConfig.failsafe_procedure = read8();
-        read8();
         break;
 
     case MSP_SET_RXFAIL_CONFIG: