From 309342931a98e04a729f8978a87f0ef70d5d5fef Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Tue, 22 Dec 2015 10:58:21 +1000 Subject: [PATCH] NAV: Instantly switch to emergency landing if RTH state entered without home fix (Failsafe RTH) --- src/main/flight/navigation_rewrite.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c index 6b1ba71ff..8087bbaac 100755 --- a/src/main/flight/navigation_rewrite.c +++ b/src/main/flight/navigation_rewrite.c @@ -695,8 +695,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati return NAV_FSM_EVENT_SWITCH_TO_RTH_2D; } } - /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */ - else if (checkForPositionSensorTimeout()) { + /* No HOME set or position sensor failure timeout - land */ + else if (!STATE(GPS_FIX_HOME) || checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } /* No valid POS sensor but still within valid timeout - wait */ @@ -755,8 +755,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_GPS_FAILING(navi if (posControl.flags.hasValidPositionSensor && STATE(GPS_FIX_HOME)) { return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_2D_HEAD_HOME } - /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */ - else if (checkForPositionSensorTimeout()) { + /* No HOME set or position sensor failure timeout - land */ + else if (!STATE(GPS_FIX_HOME) || checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } /* No valid POS sensor but still within valid timeout - wait */ -- 2.11.4.GIT