From 16ddc9fb8a8c863c7ab7ff921fda9b00a285fed6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 21 Jul 2012 20:32:07 +1000 Subject: [PATCH] APM: fixed a problem with wayoints completing early when a user forces a waypoint jump, we may need to reset prev_WP to prevent us thinking the waypoint is complete before we have started --- ArduPlane/commands.pde | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index 2ef038a5b2..cd5b037a64 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -143,6 +143,16 @@ static void set_next_WP(struct Location *wp) // --------------------- next_WP = *wp; + // are we already past the waypoint? This happens when we jump + // waypoints, and it can cause us to skip a waypoint. If we are + // past the waypoint when we start on a leg, then use the current + // location as the previous waypoint, to prevent immediately + // considering the waypoint complete + if (location_passed_point(current_loc, prev_WP, next_WP)) { + gcs_send_text_P(SEVERITY_LOW, PSTR("Resetting prev_WP")); + prev_WP = current_loc; + } + // used to control FBW and limit the rate of climb // ----------------------------------------------- target_altitude = current_loc.alt;