From 44542861659c32e5de883983dae9ebaa46e00bc9 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Wed, 18 Jan 2023 13:44:50 +1200 Subject: [PATCH] GPS Rescue Bugfix, add a failsafe debug, refactor stick deflection (#12195) * simple failsafe debug * simplify areSticksActive use getRcDeflectionAbs and fix unitTest * bugfix to require stick input to re-take control * small refactor --- src/main/build/debug.c | 1 + src/main/build/debug.h | 1 + src/main/fc/core.c | 13 +------ src/main/flight/failsafe.c | 41 ++++++++++++--------- src/main/flight/failsafe.h | 1 + src/main/rx/rx.c | 1 + src/test/unit/arming_prevention_unittest.cc | 2 +- 7 files changed, 30 insertions(+), 30 deletions(-) diff --git a/src/main/build/debug.c b/src/main/build/debug.c index aedc6555d2..6bfe05a099 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -107,4 +107,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "ATTITUDE", "VTX_MSP", "GPS_DOP", + "FAILSAFE", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 9a4e847fe8..f43cebd117 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -105,6 +105,7 @@ typedef enum { DEBUG_ATTITUDE, DEBUG_VTX_MSP, DEBUG_GPS_DOP, + DEBUG_FAILSAFE, DEBUG_COUNT } debugType_e; diff --git a/src/main/fc/core.c b/src/main/fc/core.c index dcd9eef608..1010285e59 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -683,18 +683,7 @@ static bool canUpdateVTX(void) bool areSticksActive(uint8_t stickPercentLimit) { for (int axis = FD_ROLL; axis <= FD_YAW; axis ++) { - const uint8_t deadband = axis == FD_YAW ? rcControlsConfig()->yaw_deadband : rcControlsConfig()->deadband; - uint8_t stickPercent = 0; - if ((rcData[axis] >= PWM_RANGE_MAX) || (rcData[axis] <= PWM_RANGE_MIN)) { - stickPercent = 100; - } else { - if (rcData[axis] > (rxConfig()->midrc + deadband)) { - stickPercent = ((rcData[axis] - rxConfig()->midrc - deadband) * 100) / (PWM_RANGE_MAX - rxConfig()->midrc - deadband); - } else if (rcData[axis] < (rxConfig()->midrc - deadband)) { - stickPercent = ((rxConfig()->midrc - deadband - rcData[axis]) * 100) / (rxConfig()->midrc - deadband - PWM_RANGE_MIN); - } - } - if (stickPercent >= stickPercentLimit) { + if (getRcDeflectionAbs(axis) * 100.f >= stickPercentLimit) { return true; } } diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 8779e8fcf4..94aa551e18 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -104,6 +104,7 @@ void failsafeReset(void) failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod; failsafeState.phase = FAILSAFE_IDLE; failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN; + failsafeState.failsafeSwitchWasOn = false; } void failsafeInit(void) @@ -217,17 +218,19 @@ uint32_t failsafeFailurePeriodMs(void) } FAST_CODE_NOINLINE void failsafeUpdateState(void) -// triggered directly, and ONLY, by the cheduler, at 10ms = PERIOD_RXDATA_FAILURE - intervals +// triggered directly, and ONLY, by the scheduler, at 10ms = PERIOD_RXDATA_FAILURE - intervals { if (!failsafeIsMonitoring()) { return; } bool receivingRxData = failsafeIsReceivingRxData(); - // true when FAILSAFE_RXLINK_UP - // FAILSAFE_RXLINK_UP is set in failsafeOnValidDataReceived + // returns state of FAILSAFE_RXLINK_UP + // FAILSAFE_RXLINK_UP is set in failsafeOnValidDataReceived, after the various Stage 1 and recovery delays // failsafeOnValidDataReceived runs from detectAndApplySignalLossBehaviour + DEBUG_SET(DEBUG_FAILSAFE, 2, receivingRxData); // from Rx alone, not considering switch + bool armed = ARMING_FLAG(ARMED); bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE); beeperMode_e beeperMode = BEEPER_SILENCE; @@ -238,7 +241,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) receivingRxData = false; } - // Beep RX lost only if we are not seeing data and we have been armed earlier + // Beep RX lost only if we are not seeing data and are armed or have been armed earlier if (!receivingRxData && (armed || ARMING_FLAG(WAS_EVER_ARMED))) { beeperMode = BEEPER_RX_LOST; } @@ -250,12 +253,13 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) switch (failsafeState.phase) { case FAILSAFE_IDLE: + failsafeState.failsafeSwitchWasOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE); if (armed) { // Track throttle command below minimum time if (calculateThrottleStatus() != THROTTLE_LOW) { failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; } - if (failsafeSwitchIsOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL)) { + if (failsafeState.failsafeSwitchWasOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL)) { // Failsafe switch is configured as KILL switch and is switched ON failsafeState.active = true; failsafeState.events++; @@ -286,8 +290,8 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) reprocessState = true; } } else { - // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode) - if (failsafeSwitchIsOn) { + // When NOT armed, enable failsafe mode to show warnings in OSD + if (failsafeState.failsafeSwitchWasOn) { ENABLE_FLIGHT_MODE(FAILSAFE_MODE); } else { DISABLE_FLIGHT_MODE(FAILSAFE_MODE); @@ -308,8 +312,6 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) // Enter Stage 2 with settings for landing mode ENABLE_FLIGHT_MODE(FAILSAFE_MODE); failsafeState.phase = FAILSAFE_LANDING; - failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod; - // allow re-arming 1 second after Rx recovery failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND; break; @@ -317,21 +319,20 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) ENABLE_FLIGHT_MODE(FAILSAFE_MODE); failsafeState.phase = FAILSAFE_LANDED; // go directly to FAILSAFE_LANDED - failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod; - // allow re-arming 1 second after Rx recovery break; #ifdef USE_GPS_RESCUE case FAILSAFE_PROCEDURE_GPS_RESCUE: ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE); failsafeState.phase = FAILSAFE_GPS_RESCUE; - failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod; - // allow re-arming 1 second after Rx recovery break; #endif } - if (failsafeSwitchIsOn) { + if (failsafeState.failsafeSwitchWasOn) { failsafeState.receivingRxDataPeriodPreset = 0; - // allow immediate recovery if failsafe is triggered by a switch + // recover immediately if failsafe was triggered by a switch + } else { + failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod; + // recover from true link loss failsafe 1 second after RC Link recovers } } reprocessState = true; @@ -358,8 +359,10 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) #ifdef USE_GPS_RESCUE case FAILSAFE_GPS_RESCUE: if (receivingRxData) { - if (areSticksActive(failsafeConfig()->failsafe_stick_threshold) || !failsafeSwitchIsOn) { - // this test requires stick inputs to be received during GPS Rescue see PR #7936 for rationale + if (areSticksActive(failsafeConfig()->failsafe_stick_threshold) || failsafeState.failsafeSwitchWasOn) { + // exits the rescue immediately if failsafe was initiated by switch, otherwise + // requires stick input to exit the rescue after a true Rx loss failsafe + // NB this test requires stick inputs to be received during GPS Rescue see PR #7936 for rationale failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; reprocessState = true; } @@ -414,6 +417,10 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) default: break; } + + DEBUG_SET(DEBUG_FAILSAFE, 0, failsafeState.failsafeSwitchWasOn); + DEBUG_SET(DEBUG_FAILSAFE, 3, failsafeState.phase); + } while (reprocessState); if (beeperMode != BEEPER_SILENCE) { diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index e68c75a3d4..350c9e2df2 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -90,6 +90,7 @@ typedef struct failsafeState_s { uint32_t receivingRxDataPeriodPreset; // preset for the required period of valid rxData failsafePhase_e phase; failsafeRxLinkState_e rxLinkState; + bool failsafeSwitchWasOn; } failsafeState_t; void failsafeInit(void); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index a3b6e4d146..2c117a40ab 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -564,6 +564,7 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current } } + DEBUG_SET(DEBUG_FAILSAFE, 1, rxSignalReceived); DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 0, rxSignalReceived); } diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index c36f38742e..6c1a8d672c 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -1150,5 +1150,5 @@ extern "C" { UNUSED(pitchLpf); return 0.0f; } - + void getRcDeflectionAbs(void) {} }