Merge pull request #3762 from iNavFlight/de_cruise_failsafe_fix

Fix failsafe RTH not activating from CRUISE mode if sticks were not centered
iNavFlight · Aug 18, 2018 · 1ca4ae3 · 1ca4ae3
2 parents 0630fae + 0bf51de
commit 1ca4ae3
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 2 deletions.
diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c
@@ -154,6 +154,7 @@ void failsafeReset(void)
     failsafeState.receivingRxDataPeriodPreset = 0;
     failsafeState.phase = FAILSAFE_IDLE;
     failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
+    failsafeState.bypassNavigation = true;
 
     failsafeState.lastGoodRcCommand[ROLL] = 0;
     failsafeState.lastGoodRcCommand[PITCH] = 0;
@@ -171,7 +172,7 @@ void failsafeInit(void)
 #ifdef USE_NAV
 bool failsafeBypassNavigation(void)
 {
-    return failsafeState.active && failsafeState.controlling && failsafeProcedureLogic[failsafeConfig()->failsafe_procedure].bypassNavigation;
+    return failsafeState.active && failsafeState.controlling && failsafeState.bypassNavigation;
 }
 
 bool failsafeMayRequireNavigationMode(void)
@@ -227,6 +228,7 @@ static void failsafeActivate(failsafePhase_e newPhase)
     failsafeState.active = true;
     failsafeState.controlling = true;
     failsafeState.phase = newPhase;
+    failsafeState.bypassNavigation = failsafeProcedureLogic[failsafeConfig()->failsafe_procedure].bypassNavigation;
     ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
     failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
 
@@ -469,6 +471,7 @@ void failsafeUpdateState(void)
                             // This shouldn't happen. If RTH was somehow aborted during failsafe - fallback to FAILSAFE_LANDING procedure
                             abortForcedRTH();
                             failsafeActivate(FAILSAFE_LANDING);
+                            failsafeState.bypassNavigation = true;  // Force bypassing navigation
                             reprocessState = true;
                             break;
                     }

diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h
@@ -134,6 +134,7 @@ typedef struct failsafeState_s {
     bool suspended;                         // Failsafe is temporary suspended. This happens when we temporary suspend RX system due to EEPROM write/read
     bool active;                            // Failsafe is active (on RC link loss)
     bool controlling;                       // Failsafe is driving the sticks instead of pilot
+    bool bypassNavigation;
     timeMs_t rxDataFailurePeriod;
     timeMs_t rxDataRecoveryPeriod;
     timeMs_t validRxDataReceivedAt;

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
@@ -365,6 +365,12 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_TIMEOUT]                     = NAV_STATE_CRUISE_2D_ADJUSTING,
             [NAV_FSM_EVENT_ERROR]                       = NAV_STATE_IDLE,
             [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]           = NAV_STATE_ALTHOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]        = NAV_STATE_POSHOLD_3D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D]         = NAV_STATE_CRUISE_3D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_RTH]               = NAV_STATE_RTH_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]          = NAV_STATE_WAYPOINT_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
         }
     },
 
@@ -383,6 +389,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
         }
     },
+
     [NAV_STATE_CRUISE_3D_IN_PROGRESS] = {
         .persistentId = NAV_PERSISTENT_ID_CRUISE_3D_IN_PROGRESS,
         .onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS,
@@ -404,7 +411,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         }
     },
 
-        [NAV_STATE_CRUISE_3D_ADJUSTING] = {
+    [NAV_STATE_CRUISE_3D_ADJUSTING] = {
         .persistentId = NAV_PERSISTENT_ID_CRUISE_3D_ADJUSTING,
         .onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING,
         .timeoutMs = 10,
@@ -417,6 +424,12 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_TIMEOUT]                     = NAV_STATE_CRUISE_3D_ADJUSTING,
             [NAV_FSM_EVENT_ERROR]                       = NAV_STATE_IDLE,
             [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]           = NAV_STATE_ALTHOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]        = NAV_STATE_POSHOLD_3D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D]         = NAV_STATE_CRUISE_2D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_RTH]               = NAV_STATE_RTH_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]          = NAV_STATE_WAYPOINT_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
         }
     },