From 69b5f352f6bc44729bd82dbaa99b71e83d61843e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell <tridge@samba.org> Date: Sun, 18 May 2014 16:14:11 +1000 Subject: [PATCH] Plane: prevent a bad glide slope when first entering auto we need to ensure that prev_WP_loc is not used as it could be completely different from current_loc. As a precuation, this also sets up next_WP_loc so that when the set_next_WP() is first called that prev_WP_loc is set to current_loc --- ArduPlane/navigation.pde | 4 ++-- ArduPlane/system.pde | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 4c5f09b7b..4bac5c848 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -256,8 +256,8 @@ static void setup_glide_slope(void) // is basically to prevent situations where we try to slowly // gain height at low altitudes, potentially hitting // obstacles. - if (relative_altitude() > 40 || next_WP_loc.alt < prev_WP_loc.alt) { - offset_altitude_cm = next_WP_loc.alt - prev_WP_loc.alt; + if (relative_altitude() > 40 || next_WP_loc.alt < current_loc.alt) { + offset_altitude_cm = next_WP_loc.alt - current_loc.alt; } else { offset_altitude_cm = 0; } diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 4776c9ade..b4b4cc7ad 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -335,7 +335,7 @@ static void set_mode(enum FlightMode mode) case AUTO: auto_throttle_mode = true; - prev_WP_loc = current_loc; + next_WP_loc = prev_WP_loc = current_loc; // start or resume the mission, based on MIS_AUTORESET mission.start_or_resume(); break; -- GitLab