Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 14:55:18 +03:00
Code Issues Wiki Activity

Update navigation.c

Browse source
This commit is contained in:
breadoven 2020-10-13 17:41:16 +01:00 • committed by GitHub
parent d3629ffbf4
commit ca05206cce
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 0 additions and 27 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -103,7 +103,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.disarm_on_landing = 0, .disarm_on_landing = 0,
.rth_allow_landing = NAV_RTH_ALLOW_LANDING_ALWAYS, .rth_allow_landing = NAV_RTH_ALLOW_LANDING_ALWAYS,
.auto_overrides_motor_stop = 1, .auto_overrides_motor_stop = 1,
.rth_alt_control_override = 0, //set using nav_rth_alt_control_override
}, },
// General navigation parameters // General navigation parameters
@ -222,8 +221,6 @@ static navigationFSMEvent_t nextForNonGeoStates(void);
void initializeRTHSanityChecker(const fpVector3_t * pos); void initializeRTHSanityChecker(const fpVector3_t * pos);
bool validateRTHSanityChecker(void); bool validateRTHSanityChecker(void);
static void OverRideRTHAtitudePreset(void);
/*************************************************************************************************/ /*************************************************************************************************/
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(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) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState)
{ {
navigationFSMStateFlags_t prevFlags = navGetStateFlags(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)) { 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 // 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) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState)
{ {
UNUSED(previousState); UNUSED(previousState);
OverRideRTHAtitudePreset();
if ((posControl.flags.estHeadingStatus == EST_NONE)) { if ((posControl.flags.estHeadingStatus == EST_NONE)) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; 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) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState)
{ {
UNUSED(previousState); UNUSED(previousState);
OverRideRTHAtitudePreset();
if ((posControl.flags.estHeadingStatus == EST_NONE)) { if ((posControl.flags.estHeadingStatus == EST_NONE)) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;