Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 16:55:36 +03:00
Code Issues Wiki Activity

Failsafe switch choosable behavior #5994

Browse source 
The `failsafe_kill_switch` parameter has been renamed to
`failsafe_switch_mode` and it determines what happens
when the Failsafe mode is activated with an AUX switch.
It takes one of three values:
  0 - simulates RC signal loss - thus activates Stage1 failsafe
      (former behavior when kill switch option was OFF)
  1 - disarms immediately
      (former behavior when kill switch option was ON)
  2 - activates the failsafe procedure (Stage2) immediately (new)
This commit is contained in:
Cleric-K 2018-05-28 23:33:38 +03:00
parent 418fd4beaa
commit e17abc4063
7 changed files with 159 additions and 24 deletions
Download patch file Download diff file Expand all files Collapse all files

8
src/main/flight/failsafe.c
View file

@ -68,7 +68,7 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
.failsafe_throttle_low_delay = 100, // default throttle low delay for "just disarm" on failsafe condition
.failsafe_delay = 4, // 0,4sec
.failsafe_off_delay = 10, // 1sec
.failsafe_kill_switch = 0, // default failsafe switch action is identical to rc link loss
.failsafe_switch_mode = 0, // default failsafe switch action is identical to rc link loss
.failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT // default full failsafe procedure is 0: auto-landing
);
@ -192,6 +192,10 @@ void failsafeUpdateState(void)
bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
beeperMode_e beeperMode = BEEPER_SILENCE;
if (failsafeSwitchIsOn && failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2) {
receivingRxData = false; // force Stage2
}
// Beep RX lost only if we are not seeing data and we have been armed earlier
if (!receivingRxData && ARMING_FLAG(WAS_EVER_ARMED)) {
beeperMode = BEEPER_RX_LOST;
@ -210,7 +214,7 @@ void failsafeUpdateState(void)
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
}
// Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming)
if (failsafeSwitchIsOn && failsafeConfig()->failsafe_kill_switch) {
if (failsafeSwitchIsOn && failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL) {
// KillswitchEvent: failsafe switch is configured as KILL switch and is switched ON
failsafeActivate();
failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure

8
src/main/flight/failsafe.h
View file

@ -37,7 +37,7 @@ typedef struct failsafeConfig_s {
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_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200)
uint8_t failsafe_kill_switch; // failsafe switch action is 0: identical to rc link loss, 1: disarms instantly
uint8_t failsafe_switch_mode; // failsafe switch action is 0: stage1 (identical to rc link loss), 1: disarms instantly, 2: stage2
uint8_t failsafe_procedure; // selected full failsafe procedure is 0: auto-landing, 1: Drop it
} failsafeConfig_t;
@ -64,6 +64,12 @@ typedef enum {
FAILSAFE_PROCEDURE_GPS_RESCUE
} failsafeProcedure_e;
typedef enum {
FAILSAFE_SWITCH_MODE_STAGE1 = 0,
FAILSAFE_SWITCH_MODE_KILL,
FAILSAFE_SWITCH_MODE_STAGE2
} failsafeSwitchMode_e;
typedef struct failsafeState_s {
int16_t events;
bool monitoring;

4
src/main/interface/msp.c
View file

@ -1071,7 +1071,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, failsafeConfig()->failsafe_delay);
sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay);
sbufWriteU16(dst, failsafeConfig()->failsafe_throttle);
sbufWriteU8(dst, failsafeConfig()->failsafe_kill_switch);
sbufWriteU8(dst, failsafeConfig()->failsafe_switch_mode);
sbufWriteU16(dst, failsafeConfig()->failsafe_throttle_low_delay);
sbufWriteU8(dst, failsafeConfig()->failsafe_procedure);
break;
@ -1920,7 +1920,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src);
failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src);
failsafeConfigMutable()->failsafe_kill_switch = sbufReadU8(src);
failsafeConfigMutable()->failsafe_switch_mode = sbufReadU8(src);
failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src);
failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src);
break;

7
src/main/interface/settings.c
View file

@ -288,6 +288,10 @@ static const char * const lookupTableFailsafe[] = {
"AUTO-LAND", "DROP", "GPS-RESCUE"
};
static const char * const lookupTableFailsafeSwitchMode[] = {
"STAGE1", "KILL", "STAGE2"
};
static const char * const lookupTableBusType[] = {
"NONE", "I2C", "SPI", "SLAVE"
};
@ -390,6 +394,7 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableLowpassType),
LOOKUP_TABLE_ENTRY(lookupTableDtermLowpassType),
LOOKUP_TABLE_ENTRY(lookupTableFailsafe),
LOOKUP_TABLE_ENTRY(lookupTableFailsafeSwitchMode),
LOOKUP_TABLE_ENTRY(lookupTableCrashRecovery),
#ifdef USE_CAMERA_CONTROL
LOOKUP_TABLE_ENTRY(lookupTableCameraControlMode),
@ -578,7 +583,7 @@ const clivalue_t valueTable[] = {
{ "failsafe_delay", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_delay) },
{ "failsafe_off_delay", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_off_delay) },
{ "failsafe_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_throttle) },
{ "failsafe_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_kill_switch) },
{ "failsafe_switch_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FAILSAFE_SWITCH_MODE }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_switch_mode) },
{ "failsafe_throttle_low_delay",VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 300 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_throttle_low_delay) },
{ "failsafe_procedure", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FAILSAFE }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_procedure) },

1
src/main/interface/settings.h
View file

@ -69,6 +69,7 @@ typedef enum {
TABLE_LOWPASS_TYPE,
TABLE_DTERM_LOWPASS_TYPE,
TABLE_FAILSAFE,
TABLE_FAILSAFE_SWITCH_MODE,
TABLE_CRASH_RECOVERY,
#ifdef USE_CAMERA_CONTROL
TABLE_CAMERA_CONTROL_MODE,

131
src/test/unit/flight_failsafe_unittest.cc
View file

@ -80,11 +80,27 @@ void configureFailsafe(void)
failsafeConfigMutable()->failsafe_delay = 10; // 1 second
failsafeConfigMutable()->failsafe_off_delay = 50; // 5 seconds
failsafeConfigMutable()->failsafe_kill_switch = false;
failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE1;
failsafeConfigMutable()->failsafe_throttle = 1200;
failsafeConfigMutable()->failsafe_throttle_low_delay = 50; // 5 seconds
failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_AUTO_LANDING;
sysTickUptime = 0;
}
void activateBoxFailsafe()
{
boxBitmask_t newMask;
bitArraySet(&newMask, BOXFAILSAFE);
rcModeUpdate(&newMask);
}
void deactivateBoxFailsafe()
{
boxBitmask_t newMask;
memset(&newMask, 0, sizeof(newMask));
rcModeUpdate(&newMask);
}
//
// Stepwise tests
//
@ -300,7 +316,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
}
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent)
TEST(FlightFailsafeTest, TestFailsafeSwitchModeKill)
{
// given
ENABLE_ARMING_FLAG(ARMED);
@ -309,11 +325,9 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent)
// and
throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure
failsafeConfigMutable()->failsafe_kill_switch = true; // configure AUX switch as kill switch
failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_KILL;
boxBitmask_t newMask;
bitArraySet(&newMask, BOXFAILSAFE);
rcModeUpdate(&newMask); // activate BOXFAILSAFE mode
activateBoxFailsafe();
sysTickUptime = 0; // restart time from 0
failsafeOnValidDataReceived(); // set last valid sample at current time
@ -334,8 +348,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent)
sysTickUptime += PERIOD_RXDATA_RECOVERY + 1; // adjust time to point just past the recovery time to
failsafeOnValidDataReceived(); // cause a recovered link
memset(&newMask, 0, sizeof(newMask));
rcModeUpdate(&newMask); // BOXFAILSAFE must be off (kill switch)
deactivateBoxFailsafe();
// when
failsafeUpdateState();
@ -360,6 +373,105 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent)
EXPECT_FALSE(isArmingDisabled());
}
TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Drop)
{
// given
ENABLE_ARMING_FLAG(ARMED);
resetCallCounters();
// and
throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure
failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE2;
failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT;
sysTickUptime = 0; // restart time from 0
activateBoxFailsafe();
failsafeOnValidDataFailed(); // box failsafe causes data to be invalid
// when
failsafeUpdateState(); // should activate stage2 immediately
// then
EXPECT_EQ(true, failsafeIsActive());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// given
sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time
deactivateBoxFailsafe();
failsafeOnValidDataReceived(); // inactive box failsafe gives valid data
// when
failsafeUpdateState();
// then
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
EXPECT_FALSE(isArmingDisabled());
}
TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Land)
{
// given
ENABLE_ARMING_FLAG(ARMED);
resetCallCounters();
// and
throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure
failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE2;
failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_AUTO_LANDING;
sysTickUptime = 0; // restart time from 0
activateBoxFailsafe();
failsafeOnValidDataFailed(); // box failsafe causes data to be invalid
// when
failsafeUpdateState(); // should activate stage2 immediately
// then
EXPECT_EQ(true, failsafeIsActive());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
sysTickUptime += failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND + 1;
// given
failsafeOnValidDataFailed(); // set last invalid sample at current time
// when
failsafeUpdateState();
// then
EXPECT_EQ(true, failsafeIsActive());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// given
sysTickUptime += PERIOD_OF_30_SECONDS + 1; // adjust time to point just past the required additional recovery time
// and
deactivateBoxFailsafe();
failsafeOnValidDataReceived(); // inactive box failsafe gives valid data
// when
failsafeUpdateState();
// then
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
EXPECT_FALSE(isArmingDisabled());
}
/****************************************************************************************/
//
// Additional non-stepwise tests
@ -368,6 +480,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent)
TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected)
{
// given
resetCallCounters();
configureFailsafe();
// and
@ -405,7 +518,7 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected
EXPECT_EQ(true, failsafeIsMonitoring());
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(isArmingDisabled());
// given