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