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

GPS Rescue Bugfix, add a failsafe debug, refactor stick deflection (#12195)

Browse source 
* simple failsafe debug

* simplify areSticksActive

use getRcDeflectionAbs and fix unitTest

* bugfix to require stick input to re-take control

* small refactor
This commit is contained in:
ctzsnooze 2023-01-18 13:44:50 +12:00 • committed by GitHub
parent e5c53597a3
commit 4454286165
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 30 additions and 30 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/build/debug.c
View file

@ -107,4 +107,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"ATTITUDE",
"VTX_MSP",
"GPS_DOP",
"FAILSAFE",
};

1
src/main/build/debug.h
View file

@ -105,6 +105,7 @@ typedef enum {
DEBUG_ATTITUDE,
DEBUG_VTX_MSP,
DEBUG_GPS_DOP,
DEBUG_FAILSAFE,
DEBUG_COUNT
} debugType_e;

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

@ -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;
}
}

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

@ -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) {

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

@ -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);

1
src/main/rx/rx.c
View file

@ -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);
}

2
src/test/unit/arming_prevention_unittest.cc
View file

@ -1150,5 +1150,5 @@ extern "C" {
UNUSED(pitchLpf);
return 0.0f;
}
void getRcDeflectionAbs(void) {}
}