Plane: rebuild the glide slope if we are above it and already climbing

(controlled by GLIDE_SLOPE_THR)

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;

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

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;
         }
     }