From 95d2e79312a977d58e27b67638c7dc32778df436 Mon Sep 17 00:00:00 2001
From: Michael Day <mday299@yahoo.com>
Date: Sat, 25 Oct 2014 13:43:03 -0700
Subject: [PATCH] Plane: Keep throwing WP in front plane for
 CONTINUE_AND_CHANGE_ALT

---
 ArduPlane/commands_logic.pde | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde
index 72a4d621a2..3d72c711bb 100644
--- a/ArduPlane/commands_logic.pde
+++ b/ArduPlane/commands_logic.pde
@@ -513,7 +513,14 @@ static bool verify_continue_and_change_alt()
     if (abs(adjusted_altitude_cm() - next_WP_loc.alt) <= 500) {
         return true;
     }
-    
+   
+    //Is the next_WP less than 200 m away?
+    if (get_distance(prev_WP_loc, next_WP_loc) < 200.f) {
+        //push another 300 m down the line
+        int32_t next_wp_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
+        location_update(next_WP_loc, next_wp_bearing_cd * 0.01f, 300.f);
+    }
+
     //keep flying the same course
     nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);