Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-21 07:15:16 +03:00
Code Issues Wiki Activity

Revert "Revert "Update navigation.c""

Browse source 
This reverts commit 61d1e416ba.
This commit is contained in:
breadoven 2020-10-13 17:04:40 +01:00
parent 61d1e416ba
commit fba427f632
1 changed files with 19 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1084,12 +1084,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING(nav
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState)
{
//AWH xxxxxxxxxxxxxxxxxxxxx
RTHAltHoldOverRideFlag = false;
//xxxxxxxxxxxxxxxxxxxxx
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
// If we are in dead-reckoning mode - also fail, since coordinates may be unreliable
@ -1152,23 +1150,30 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
}
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState)
static void OverRideRTHAtitudePreset(void)
{
//AWH xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
if (!navConfig()->general.flags.rth_alt_control_override) {
return;
}
if (!IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) {
RTHAltHoldOverRideFlag = true;
posControl.flags.CanOverRideRTHAlt = true;
}
else {
if (RTHAltHoldOverRideFlag && !posControl.flags.forcedRTHActivated) {
if (posControl.flags.CanOverRideRTHAlt && !posControl.flags.forcedRTHActivated) {
posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
RTHAltHoldOverRideFlag = false;
posControl.flags.CanOverRideRTHAlt = false;
}
}
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
}
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;
}
@ -1243,6 +1248,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
{
UNUSED(previousState);
OverRideRTHAtitudePreset();
if ((posControl.flags.estHeadingStatus == EST_NONE)) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}