From 3bab5f60ca0422dccea50c8f048f87a589b96294 Mon Sep 17 00:00:00 2001 From: Tony Cabello Date: Wed, 19 Dec 2018 20:09:04 +0100 Subject: [PATCH] Sanity checks flyaway/stalled only done in crosstrack phase --- src/main/flight/gps_rescue.c | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index ba70ccfd53..60ac040d49 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -353,17 +353,19 @@ static void performSanityChecks() previousTimeUs = currentTimeUs; - secondsStalled = constrain(secondsStalled + ((rescueState.sensor.groundSpeed < 150) ? 1 : -1), 0, 20); + if (rescueState.phase == RESCUE_CROSSTRACK) { + secondsStalled = constrain(secondsStalled + ((rescueState.sensor.groundSpeed < 150) ? 1 : -1), 0, 20); - if (secondsStalled == 20) { - rescueState.failure = RESCUE_STALLED; - } + if (secondsStalled == 20) { + rescueState.failure = RESCUE_STALLED; + } - secondsFlyingAway = constrain(secondsFlyingAway + ((lastDistanceToHomeM < rescueState.sensor.distanceToHomeM) ? 1 : -1), 0, 10); - lastDistanceToHomeM = rescueState.sensor.distanceToHomeM; + secondsFlyingAway = constrain(secondsFlyingAway + ((lastDistanceToHomeM < rescueState.sensor.distanceToHomeM) ? 1 : -1), 0, 10); + lastDistanceToHomeM = rescueState.sensor.distanceToHomeM; - if (secondsFlyingAway == 10) { - rescueState.failure = RESCUE_FLYAWAY; + if (secondsFlyingAway == 10) { + rescueState.failure = RESCUE_FLYAWAY; + } } secondsLowSats = constrain(secondsLowSats + ((rescueState.sensor.numSat < gpsRescueConfig()->minSats) ? 1 : -1), 0, 10);