From f9830404014d1e251cbcc26f41c6c7fe779e9b2e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 4 Aug 2014 16:55:15 +1000
Subject: [PATCH] Plane: more fixes for glide slope calculations

---
 ArduPlane/altitude.pde       | 8 ++++++--
 ArduPlane/commands.pde       | 2 +-
 ArduPlane/commands_logic.pde | 2 +-
 3 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/ArduPlane/altitude.pde b/ArduPlane/altitude.pde
index ea42c9fc0b..0ffe28e7f0 100644
--- a/ArduPlane/altitude.pde
+++ b/ArduPlane/altitude.pde
@@ -349,11 +349,15 @@ static bool above_location_current(const Location &loc)
         if (!loc.flags.relative_alt) {
             loc_alt -= home.alt*0.01f;
         }
-        return terrain_alt > loc.alt;
+        return terrain_alt > loc_alt;
     }
 #endif
 
-    return current_loc.alt > loc.alt;
+    float loc_alt_cm = loc.alt;
+    if (!loc.flags.relative_alt) {
+        loc_alt_cm -= home.alt;
+    }
+    return current_loc.alt > loc_alt_cm;
 }
 
 /*
diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde
index 9da6a7192f..34636394d7 100644
--- a/ArduPlane/commands.pde
+++ b/ArduPlane/commands.pde
@@ -48,7 +48,7 @@ static void set_next_WP(const struct Location& loc)
 
     // used to control FBW and limit the rate of climb
     // -----------------------------------------------
-    set_target_altitude_current();
+    set_target_altitude_location(next_WP_loc);
 
     // zero out our loiter vals to watch for missed waypoints
     loiter_angle_reset();
diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde
index dbf0d78d82..a44a446868 100644
--- a/ArduPlane/commands_logic.pde
+++ b/ArduPlane/commands_logic.pde
@@ -250,10 +250,10 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
 
 static void do_RTL(void)
 {
-    control_mode    = RTL;
     prev_WP_loc = current_loc;
     next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude());
     setup_terrain_target_alt(next_WP_loc);
+    set_target_altitude_location(next_WP_loc);
 
     if (g.loiter_radius < 0) {
         loiter.direction = -1;