diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 602de21729..c16aa048eb 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -103,7 +103,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .disarm_on_landing = 0, .rth_allow_landing = NAV_RTH_ALLOW_LANDING_ALWAYS, .auto_overrides_motor_stop = 1, - .rth_alt_control_override = 0, //set using nav_rth_alt_control_override }, // General navigation parameters @@ -222,8 +221,6 @@ static navigationFSMEvent_t nextForNonGeoStates(void); void initializeRTHSanityChecker(const fpVector3_t * pos); bool validateRTHSanityChecker(void); -static void OverRideRTHAtitudePreset(void); - /*************************************************************************************************/ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navigationFSMState_t previousState); @@ -1085,8 +1082,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING(nav static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState) { navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); - - posControl.flags.CanOverRideRTHAlt = false; //prevent unwanted override if AltHold selected at RTH initialize if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || (posControl.flags.estPosStatus != EST_TRUSTED) || !STATE(GPS_FIX_HOME)) { // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing @@ -1150,29 +1145,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati } } -static void OverRideRTHAtitudePreset(void) -{ - if (!navConfig()->general.flags.rth_alt_control_override) { - return; - } - - if (!IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) { - posControl.flags.CanOverRideRTHAlt = true; - } - else { - if (posControl.flags.CanOverRideRTHAlt && !posControl.flags.forcedRTHActivated) { - posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; - posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; - posControl.flags.CanOverRideRTHAlt = false; - } - } -} - static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState) { UNUSED(previousState); - - OverRideRTHAtitudePreset(); if ((posControl.flags.estHeadingStatus == EST_NONE)) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; @@ -1247,8 +1222,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState) { UNUSED(previousState); - - OverRideRTHAtitudePreset(); if ((posControl.flags.estHeadingStatus == EST_NONE)) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;