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

[NAV] Fallback to RTH in case of WP activation with no mission loaded; Fix the bug when RTH will terminate if GPS is briefly lost even if FSM logic accounts for that

Browse source
This commit is contained in:
Konstantin (DigitalEntity) Sharlaimov 2020-07-30 10:09:06 +02:00
parent 9da3327292
commit 4b148920b9
1 changed files with 29 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -3097,6 +3097,11 @@ static bool canActivateNavigationModes(void)
return (posControl.flags.estPosStatus == EST_TRUSTED) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
}
static bool isWaypointMissionValid(void)
{
return posControl.waypointListValid && (posControl.waypointCount > 0);
}
static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
{
static bool canActivateWaypoint = false;
@ -3110,9 +3115,16 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
}
// Flags if we can activate certain nav modes (check if we have required sensors and they provide valid data)
bool canActivateAltHold = canActivateAltHoldMode();
bool canActivatePosHold = canActivatePosHoldMode();
bool canActivateNavigation = canActivateNavigationModes();
const bool canActivateAltHold = canActivateAltHoldMode();
const bool canActivatePosHold = canActivatePosHoldMode();
const bool canActivateNavigation = canActivateNavigationModes();
const bool isExecutingRTH = navGetStateFlags(posControl.navState) & NAV_AUTO_RTH;
// Keep canActivateWaypoint flag at FALSE if there is no mission loaded
// Also block WP mission if we are executing RTH
if (!isWaypointMissionValid() || isExecutingRTH) {
canActivateWaypoint = false;
}
// LAUNCH mode has priority over any other NAV mode
if (STATE(FIXED_WING_LEGACY)) {
@ -3139,21 +3151,32 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
}
// RTH/Failsafe_RTH can override MANUAL
if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
if (posControl.flags.forcedRTHActivated) {
// If we request forced RTH - attempt to activate it no matter what
// This might switch to emergency landing controller if GPS is unavailable
canActivateWaypoint = false; // Block WP mode if we switched to RTH for whatever reason
return NAV_FSM_EVENT_SWITCH_TO_RTH;
}
// Pilot-triggered RTH, also fall-back for WP if there is no mission loaded
if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || (IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint)) {
// Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss
// If don't keep this, loss of any of the canActivatePosHold && canActivateNavigation && canActivateAltHold
// will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back
// logic to kick in (waiting for GPS on airplanes, switch to emergency landing etc)
if (isExecutingRTH || (canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
return NAV_FSM_EVENT_SWITCH_TO_RTH;
}
}
// MANUAL mode has priority over WP/PH/AH
if (IS_RC_MODE_ACTIVE(BOXMANUAL)) {
canActivateWaypoint = false; // Block WP mode if we are in PASSTHROUGH mode
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
// Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded
if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
if ((FLIGHT_MODE(NAV_WP_MODE)) || (canActivateNavigation && canActivateWaypoint && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid && (posControl.waypointCount > 0)))
if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME)))
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
}
else {