diff --git a/src/main/config/config.c b/src/main/config/config.c index f0fd7aeaa..438f9cd66 100755 --- a/src/main/config/config.c +++ b/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 diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 11682c81f..5c8872fa2 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -18,7 +18,7 @@ #include #include -#include "platform.h" +#include #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 - failsafeActivate(); + switch (failsafeConfig->failsafe_procedure) { + 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; diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 7d841dd11..501c16845 100644 --- a/src/main/flight/failsafe.h +++ b/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); - diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 2b2893361..b780f1a47 100644 --- a/src/main/io/serial_cli.c +++ b/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 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index a4bb84ca2..00698f378 100644 --- a/src/main/io/serial_msp.c +++ b/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(); - read8(); + masterConfig.failsafeConfig.failsafe_procedure = read8(); break; case MSP_SET_RXFAIL_CONFIG: