diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 94d88d603ce..807e991173e 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -83,12 +83,14 @@ typedef enum { } failsafeChannelBehavior_e; typedef struct { + bool bypassNavigation; bool forceAngleMode; failsafeChannelBehavior_e channelBehavior[4]; } failsafeProcedureLogic_t; static const failsafeProcedureLogic_t failsafeProcedureLogic[] = { [FAILSAFE_PROCEDURE_AUTO_LANDING] = { + .bypassNavigation = true, .forceAngleMode = true, .channelBehavior = { FAILSAFE_CHANNEL_AUTO, // ROLL @@ -99,6 +101,7 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = { }, [FAILSAFE_PROCEDURE_DROP_IT] = { + .bypassNavigation = true, .forceAngleMode = true, .channelBehavior = { FAILFAFE_CHANNEL_NEUTRAL, // ROLL @@ -109,6 +112,7 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = { }, [FAILSAFE_PROCEDURE_RTH] = { + .bypassNavigation = false, .forceAngleMode = true, .channelBehavior = { FAILFAFE_CHANNEL_NEUTRAL, // ROLL @@ -119,6 +123,7 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = { }, [FAILSAFE_PROCEDURE_NONE] = { + .bypassNavigation = false, .forceAngleMode = false, .channelBehavior = { FAILSAFE_CHANNEL_HOLD, // ROLL @@ -160,6 +165,11 @@ void failsafeInit(void) } #ifdef NAV +bool failsafeBypassNavigation(void) +{ + return failsafeState.active && failsafeState.controlling && failsafeProcedureLogic[failsafeConfig()->failsafe_procedure].bypassNavigation; +} + bool failsafeMayRequireNavigationMode(void) { return failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_RTH; diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index c46178a0276..79c61deb02e 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -112,6 +112,7 @@ void failsafeApplyControlInput(void); bool failsafeRequiresAngleMode(void); bool failsafeRequiresMotorStop(void); bool failsafeShouldApplyControlInput(void); +bool failsafeBypassNavigation(void); void failsafeUpdateRcCommandValues(void); void failsafeOnValidDataReceived(void); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index c4085060265..f69f224e1de 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2342,6 +2342,11 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) //We can switch modes only when ARMED if (ARMING_FLAG(ARMED)) { + // Ask failsafe system if we can use navigation system + if (failsafeBypassNavigation()) { + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } + // Flags if we can activate certain nav modes (check if we have required sensors and they provide valid data) bool canActivateAltHold = canActivateAltHoldMode(); bool canActivatePosHold = canActivatePosHoldMode();