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

When GPS Rescue is engaged after a Failsafe, pilot has to manually sw… (#7936)

Browse source 
When GPS Rescue is engaged after a Failsafe, pilot has to manually sw…
This commit is contained in:
Michael Keller 2019-04-30 08:24:59 +12:00 • committed by GitHub
commit b9fc77db3b
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
8 changed files with 45 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -747,6 +747,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) },

3
src/main/fc/config.c
View file

@ -319,9 +319,12 @@ static void validateAndFixConfig(void)
|| true
#endif
) {
#ifdef USE_GPS_RESCUE
if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE) {
failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT;
}
#endif
if (isModeActivationConditionPresent(BOXGPSRESCUE)) {
removeModeActivationCondition(BOXGPSRESCUE);

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)

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

@ -70,6 +70,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs);
bool isFlipOverAfterCrashActive(void);
int8_t calculateThrottlePercent(void);
uint8_t calculateThrottlePercentAbs(void);
bool areSticksActive(uint8_t stickPercentLimit);
void runawayTakeoffTemporaryDisable(uint8_t disableFlag);
bool isAirmodeActivated();
timeUs_t getLastDisarmTimeUs(void);

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

@ -69,13 +69,17 @@ 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] = {
"AUTO-LAND",
"DROP",
"GPS-RESCUE"
#ifdef USE_GPS_RESCUE
"GPS-RESCUE",
#endif
};
/*
@ -84,6 +88,7 @@ const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = {
void failsafeReset(void)
{
failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY + failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.validRxDataReceivedAt = 0;
failsafeState.validRxDataFailedAt = 0;
failsafeState.throttleLowPeriod = 0;
@ -141,11 +146,12 @@ static void failsafeActivate(void)
static void failsafeApplyControlInput(void)
{
#ifdef USE_GPS_RESCUE
if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE) {
ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
return;
}
#endif
for (int i = 0; i < 3; i++) {
rcData[i] = rxConfig()->midrc;
@ -172,7 +178,7 @@ void failsafeOnRxResume(void)
void failsafeOnValidDataReceived(void)
{
failsafeState.validRxDataReceivedAt = millis();
if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > PERIOD_RXDATA_RECOVERY) {
if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > failsafeState.rxDataRecoveryPeriod) {
failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;
unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
}
@ -227,7 +233,11 @@ void failsafeUpdateState(void)
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_1_SECONDS; // require 1 seconds of valid rxData
reprocessState = true;
} else if (!receivingRxData) {
if (millis() > failsafeState.throttleLowPeriod && failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_GPS_RESCUE) {
if (millis() > failsafeState.throttleLowPeriod
#ifdef USE_GPS_RESCUE
&& failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_GPS_RESCUE
#endif
) {
// JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds
failsafeActivate();
failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure
@ -263,13 +273,13 @@ 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;
#ifdef USE_GPS_RESCUE
case FAILSAFE_PROCEDURE_GPS_RESCUE:
failsafeActivate();
failsafeState.phase = FAILSAFE_GPS_RESCUE;
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS;
break;
#endif
}
}
reprocessState = true;
@ -290,10 +300,13 @@ void failsafeUpdateState(void)
reprocessState = true;
}
break;
#ifdef USE_GPS_RESCUE
case FAILSAFE_GPS_RESCUE:
if (receivingRxData) {
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true;
if (areSticksActive(failsafeConfig()->failsafe_stick_threshold)) {
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true;
}
}
if (armed) {
failsafeApplyControlInput();
@ -304,6 +317,7 @@ void failsafeUpdateState(void)
reprocessState = true;
}
break;
#endif
case FAILSAFE_LANDED:
setArmingDisabled(ARMING_DISABLED_FAILSAFE); // To prevent accidently rearming by an intermittent rx link
disarm();

5
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);
@ -61,7 +63,9 @@ typedef enum {
typedef enum {
FAILSAFE_PROCEDURE_AUTO_LANDING = 0,
FAILSAFE_PROCEDURE_DROP_IT,
#ifdef USE_GPS_RESCUE
FAILSAFE_PROCEDURE_GPS_RESCUE,
#endif
FAILSAFE_PROCEDURE_COUNT // must be last
} failsafeProcedure_e;
@ -78,6 +82,7 @@ typedef struct failsafeState_s {
bool monitoring;
bool active;
uint32_t rxDataFailurePeriod;
uint32_t rxDataRecoveryPeriod;
uint32_t validRxDataReceivedAt;
uint32_t validRxDataFailedAt;
uint32_t throttleLowPeriod; // throttle stick must have been below 'min_check' for this period

3
src/test/Makefile
View file

@ -129,6 +129,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; }