From 4b1d71e39001119b7d52cddc47655d8fbcc79370 Mon Sep 17 00:00:00 2001
From: Michael du Breuil <michael@swiftradioplanes>
Date: Sun, 12 Apr 2015 19:20:01 -0700
Subject: [PATCH] Plane: rebuild the glide slope if we are above it and already
 climbing

(controlled by GLIDE_SLOPE_THR)
---
 ArduPlane/Parameters.h   |  6 ++++--
 ArduPlane/Parameters.pde | 13 +++++++++++--
 ArduPlane/altitude.pde   | 15 +++++++++++++--
 3 files changed, 28 insertions(+), 6 deletions(-)

diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index 155068cfc2..423c1070bd 100644
--- a/ArduPlane/Parameters.h
+++ b/ArduPlane/Parameters.h
@@ -119,7 +119,7 @@ public:
         k_param_terrain,
         k_param_terrain_follow,
         k_param_stab_pitch_down_cd_old, // deprecated
-        k_param_glide_slope_threshold,
+        k_param_glide_slope_min,
         k_param_stab_pitch_down,
         k_param_terrain_lookahead,
         k_param_fbwa_tdrag_chan,
@@ -135,6 +135,7 @@ public:
         k_param_trim_rc_at_start,
         k_param_hil_mode,
         k_param_land_disarm_delay,
+        k_param_glide_slope_threshold,
 
         // 100: Arming parameters
         k_param_arming = 100,
@@ -471,7 +472,8 @@ public:
     AP_Int8 terrain_follow;
     AP_Int16 terrain_lookahead;
 #endif
-    AP_Int16 glide_slope_threshold;
+    AP_Int16 glide_slope_min;
+    AP_Float glide_slope_threshold;
     AP_Int8 fbwa_tdrag_chan;
     AP_Int8 rangefinder_landing;
     AP_Int8 flap_slewrate;
diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde
index c1cafc6266..fd1a9b5f82 100644
--- a/ArduPlane/Parameters.pde
+++ b/ArduPlane/Parameters.pde
@@ -94,13 +94,22 @@ const AP_Param::Info var_info[] PROGMEM = {
     GSCALAR(stab_pitch_down, "STAB_PITCH_DOWN",   2.0f),
 
     // @Param: GLIDE_SLOPE_MIN
-    // @DisplayName: Glide slope threshold
+    // @DisplayName: Glide slope minimum
     // @Description: This controls the minimum altitude change for a waypoint before a glide slope will be used instead of an immediate altitude change. The default value is 15 meters, which helps to smooth out waypoint missions where small altitude changes happen near waypoints. If you don't want glide slopes to be used in missions then you can set this to zero, which will disable glide slope calculations. Otherwise you can set it to a minimum number of meters of altitude error to the destination waypoint before a glide slope will be used to change altitude.
     // @Range: 0 1000
     // @Increment: 1
     // @Units: meters
     // @User: Advanced
-    GSCALAR(glide_slope_threshold, "GLIDE_SLOPE_MIN", 15),
+    GSCALAR(glide_slope_min, "GLIDE_SLOPE_MIN", 15),
+
+    // @Param: GLIDE_SLOPE_THR
+    // @DisplayName: Glide slope threshold
+    // @Description: This controls the height above the glide slope the plane may be before rebuilding a glide slope. This is useful for smoothing out an autotakeoff
+    // @Range: 0 100
+    // @Increment: 1
+    // @Units: meters
+    // @User: Advanced
+    GSCALAR(glide_slope_threshold, "GLIDE_SLOPE_THR", 5.0),
 
     // @Param: STICK_MIXING
     // @DisplayName: Stick Mixing
diff --git a/ArduPlane/altitude.pde b/ArduPlane/altitude.pde
index c592820242..cd46561504 100644
--- a/ArduPlane/altitude.pde
+++ b/ArduPlane/altitude.pde
@@ -256,6 +256,17 @@ static void set_target_altitude_proportion(const Location &loc, float proportion
     set_target_altitude_location(loc);
     proportion = constrain_float(proportion, 0.0f, 1.0f);
     change_target_altitude(-target_altitude.offset_cm*proportion);
+    //rebuild the glide slope if we are above it and supposed to be climbing
+    if(g.glide_slope_threshold > 0) {
+        if(target_altitude.offset_cm > 0 && calc_altitude_error_cm() < -100 * g.glide_slope_threshold) {
+            set_target_altitude_location(loc);
+            set_offset_altitude_location(loc);
+            change_target_altitude(-target_altitude.offset_cm*proportion);
+            //adjust the new target offset altitude to reflect that we are partially already done
+            if(proportion > 0.0f)
+                target_altitude.offset_cm = ((float)target_altitude.offset_cm)/proportion;
+        }
+    }
 }
 
 /*
@@ -350,8 +361,8 @@ static void set_offset_altitude_location(const Location &loc)
         // more accurate flight of missions where the aircraft may lose or
         // gain a bit of altitude near waypoint turn points due to local
         // terrain changes
-        if (g.glide_slope_threshold <= 0 ||
-            labs(target_altitude.offset_cm)*0.01f < g.glide_slope_threshold) {
+        if (g.glide_slope_min <= 0 ||
+            labs(target_altitude.offset_cm)*0.01f < g.glide_slope_min) {
             target_altitude.offset_cm = 0;
         }
     }