Fix failsafe & navigation clash

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;

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

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