Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 13:25:30 +03:00
Code Issues Wiki Activity

GPS Rescue procedure can be aborted by moving sticks, with a configurable delay after recovering rx

Browse source
This commit is contained in:
Tony Cabello 2019-04-09 06:30:35 +02:00
parent b4286c6ac9
commit d829563179
7 changed files with 36 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/cli/settings.c
View file

@ -746,6 +746,8 @@ const clivalue_t valueTable[] = {
{ "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.minmaxUnsigned = { 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) },
{ "failsafe_recovery_delay", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_recovery_delay) },
{ "failsafe_stick_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_stick_threshold) },
// PG_BOARDALIGNMENT_CONFIG
{ "align_board_roll", VAR_INT16 | MASTER_VALUE, .config.minmax = { -180, 360 }, PG_BOARD_ALIGNMENT, offsetof(boardAlignment_t, rollDegrees) },

5
src/main/fc/core.c
View file

@ -579,7 +579,7 @@ static bool canUpdateVTX(void)
}
#endif
#ifdef USE_RUNAWAY_TAKEOFF
#if defined(USE_RUNAWAY_TAKEOFF) || defined(USE_GPS_RESCUE)
// determine if the R/P/Y stick deflection exceeds the specified limit - integer math is good enough here.
bool areSticksActive(uint8_t stickPercentLimit)
{
@ -601,8 +601,9 @@ bool areSticksActive(uint8_t stickPercentLimit)
}
return false;
}
#endif
#ifdef USE_RUNAWAY_TAKEOFF
// allow temporarily disabling runaway takeoff prevention if we are connected
// to the configurator and the ARMING_DISABLED_MSP flag is cleared.
void runawayTakeoffTemporaryDisable(uint8_t disableFlag)

5
src/main/fc/core.h
View file

@ -67,6 +67,11 @@ void updateArmingStatus(void);
void taskMainPidLoop(timeUs_t currentTimeUs);
#if defined(USE_RUNAWAY_TAKEOFF) || defined(USE_GPS_RESCUE)
// determine if the R/P/Y stick deflection exceeds the specified limit - integer math is good enough here.
bool areSticksActive(uint8_t stickPercentLimit);
#endif
bool isFlipOverAfterCrashActive(void);
int8_t calculateThrottlePercent(void);
uint8_t calculateThrottlePercentAbs(void);

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

@ -69,7 +69,9 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
.failsafe_delay = 4, // 0,4sec
.failsafe_off_delay = 10, // 1sec
.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
.failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT,// default full failsafe procedure is 0: auto-landing
.failsafe_recovery_delay = 20, // 2 sec of valid rx data (plus 200ms) needed to allow recovering from failsafe procedure
.failsafe_stick_threshold = 30 // 30 percent of stick deflection to exit GPS Rescue procedure
);
const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = {
@ -263,12 +265,10 @@ void failsafeUpdateState(void)
// 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;
case FAILSAFE_PROCEDURE_GPS_RESCUE:
failsafeActivate();
failsafeState.phase = FAILSAFE_GPS_RESCUE;
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS;
break;
}
}
@ -277,8 +277,10 @@ void failsafeUpdateState(void)
case FAILSAFE_LANDING:
if (receivingRxData) {
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true;
if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > (failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_RECOVERY)) {
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true;
}
}
if (armed) {
failsafeApplyControlInput();
@ -292,8 +294,12 @@ void failsafeUpdateState(void)
break;
case FAILSAFE_GPS_RESCUE:
if (receivingRxData) {
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true;
if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > (failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_RECOVERY)) {
if (areSticksActive(failsafeConfig()->failsafe_stick_threshold)) {
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true;
}
}
}
if (armed) {
failsafeApplyControlInput();

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

@ -39,6 +39,8 @@ typedef struct failsafeConfig_s {
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_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
uint16_t failsafe_recovery_delay; // Time (in 0.1sec) of valid rx data (plus 200ms) needed to allow recovering from failsafe procedure
uint8_t failsafe_stick_threshold; // Stick deflection percentage to exit GPS Rescue procedure
} failsafeConfig_t;
PG_DECLARE(failsafeConfig_t, failsafeConfig);
@ -77,9 +79,9 @@ typedef struct failsafeState_s {
int16_t events;
bool monitoring;
bool active;
uint32_t rxDataFailurePeriod;
uint32_t validRxDataReceivedAt;
uint32_t validRxDataFailedAt;
int32_t rxDataFailurePeriod;
int32_t validRxDataReceivedAt;
int32_t validRxDataFailedAt;
uint32_t throttleLowPeriod; // throttle stick must have been below 'min_check' for this period
uint32_t landingShouldBeFinishedAt;
uint32_t receivingRxDataPeriod; // period for the required period of valid rxData

3
src/test/Makefile
View file

@ -127,6 +127,9 @@ flight_failsafe_unittest_SRC := \
$(USER_DIR)/fc/runtime_config.c \
$(USER_DIR)/flight/failsafe.c
flight_failsafe_unittest_DEFINES := \
USE_GPS_RESCUE=
flight_imu_unittest_SRC := \
$(USER_DIR)/common/bitarray.c \

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

@ -589,6 +589,11 @@ bool isUsingSticksForArming(void)
return isUsingSticksToArm;
}
bool areSticksActive(uint8_t stickPercentLimit) {
UNUSED(stickPercentLimit);
return false;
}
void beeperConfirmationBeeps(uint8_t beepCount) { UNUSED(beepCount); }
bool crashRecoveryModeActive(void) { return false; }