diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde
index 4c5f09b7b5dae7f04f713d12359f437ada2d177b..4bac5c8483743b6da126cabeb4f13f6212e2ab1a 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 4776c9adefd2604584697ff600f3239323d39bc5..b4b4cc7adac58287cc7327242fa8bd38ce08fc6f 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;