Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +03:00
Code Issues Wiki Activity

Fix failsafe RTH not activating from CRUISE mode if sticks were not centered

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2018-08-17 22:29:38 +02:00
parent 3c56215dce
commit 0bf51debd4
3 changed files with 19 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

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

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

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

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

13
src/main/navigation/navigation.c
View file

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