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

Update navigation.c

Browse source
This commit is contained in:
breadoven 2022-12-31 10:43:26 +00:00
parent 89d8e91472
commit fc099d470a
1 changed files with 10 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1806,6 +1806,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PR
{
UNUSED(previousState);
// Reset target position if too far away for some reason, e.g. GPS recovered since start landing.
if (posControl.flags.estPosStatus >= EST_USABLE
float targetPosLimit = STATE(MULTIROTOR) ? 2000.0f : navConfig()->fw.loiter_radius * 2.0f;
if (calculateDistanceToDestination(&posControl.desiredState.pos) > targetPosLimit) {
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
}
}
if (STATE(LANDING_DETECTED)) {
return NAV_FSM_EVENT_SUCCESS;
}
@ -3543,6 +3551,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
// If we are disarmed, abort forced RTH or Emergency Landing
posControl.flags.forcedRTHActivated = false;
posControl.flags.forcedEmergLandingActivated = false;
posControl.flags.manualEmergLandActive = false;
// ensure WP missions always restart from first waypoint after disarm
posControl.activeWaypointIndex = posControl.startWpIndex;
// Reset RTH trackback
@ -3621,7 +3630,7 @@ static bool isWaypointMissionValid(void)
static bool isManualEmergencyLandingActivated(void)
{
// Emergency landing initiated manually by toggling Poshold 4 times within 2 seconds
// Emergency landing initiated manually by toggling Poshold 4 times within 3 seconds
static timeMs_t timeout = 0;
static int8_t counter = 0;
static bool toggle;