Plane: Add climb slope minimum height parameter

Slope treatment for a flight leg was conditioned to a minimum height of
20m (which was a hardcoded value and only documented in a comment in the
code), below which an immediate altitude change was performed for that
leg. This caused unpredictable results for flight plans with waypoints
at exactly 20m followed by a climb, as the aircraft could, depending on
chance, be above or below the height demand, resulting in very different
trajectories.

This commit addresses that issue with the following changes:
- Added a parameter to control the minimum height at which a gradual
  climb slope can be performed. This documents the original feature and
  retains the safety aspect of it.
- Changed the behavior when below said minimum height to only
  immediately climb up to the safe height, regaining the intended climb
  slope afterwards. This leverages existing code for recalculating climb
  slopes, provides a good balance between safety and following the
  flight plan as intended, and fixes the core issue of different
  trajectories being taken based on random external factors.

ArduPlane/Parameters.cpp

@@ -1309,7 +1309,16 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("RNGFND_LND_ORNT", 36, ParametersG2, rangefinder_land_orient, ROTATION_PITCH_270),
 #endif
-
+
+    // @Param: CLIMB_SLOPE_HGT
+    // @DisplayName: Climb slope mininum height
+    // @Description: Sets the minimum height above home at which the aircraft will apply a climb slope between waypoints. Below it, the aircraft will ascend immediately, and will only resume the requested trajectory upon reaching this height. This prevents unsafe behavior such as attempting to slowly gain altitude near obstacles. The default value of 25 m. ensures safe operations in most environments, but it can be adjusted based on specific terrain or operational needs.
+    // @Units: m
+    // @Range: 0 50
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("CLIMB_SLOPE_HGT", 37, ParametersG2, waypoint_climb_slope_height_min, 25),
+
     AP_GROUPEND
 };
 

ArduPlane/Parameters.h

@@ -525,6 +525,8 @@ class ParametersG2 {
 
     AP_Int32 flight_options;
 
+    AP_Int16 waypoint_climb_slope_height_min;
+
     AP_Int8 takeoff_throttle_accel_count;
     AP_Int8 takeoff_timeout;
 

ArduPlane/altitude.cpp

@@ -79,25 +79,16 @@ void Plane::setup_glide_slope(void)
             reset_offset_altitude();
         }
         break;
-
     case Mode::Number::AUTO:
-
-        //climb without doing glide slope if option is enabled
+        // climb without doing glide slope if option is enabled
         if (!above_location_current(next_WP_loc) && plane.flight_option_enabled(FlightOptions::IMMEDIATE_CLIMB_IN_AUTO)) {
             reset_offset_altitude();
             break;
         }
 
-        // we only do glide slide handling in AUTO when above 20m or
-        // when descending. The 20 meter threshold is arbitrary, and
-        // is basically to prevent situations where we try to slowly
-        // gain height at low altitudes, potentially hitting
-        // obstacles.
-        if (adjusted_relative_altitude_cm() > 2000 || above_location_current(next_WP_loc)) {
-            set_offset_altitude_location(prev_WP_loc, next_WP_loc);
-        } else {
-            reset_offset_altitude();
-        }
+        // otherwise we set up an altitude slope for this leg
+        set_offset_altitude_location(prev_WP_loc, next_WP_loc);
+
         break;
     default:
         reset_offset_altitude();
@@ -289,6 +280,7 @@ void Plane::change_target_altitude(int32_t change_cm)
     }
 #endif
 }
+
 /*
   change target altitude by a proportion of the target altitude offset
   (difference in height to next WP from previous WP). proportion
@@ -302,10 +294,25 @@ void Plane::change_target_altitude(int32_t change_cm)
  */
 void Plane::set_target_altitude_proportion(const Location &loc, float proportion)
 {
+    // We only do altitude slope handling when above CLIMB_SLOPE_HGT or when
+    // descending. This is meant to prevent situations where we try to slowly
+    // gain height at low altitudes, potentially hitting obstacles.
+    if (target_altitude.offset_cm > 0 &&
+        adjusted_relative_altitude_cm() < (g2.waypoint_climb_slope_height_min * 100)) {
+        Location min_loc = loc;
+        min_loc.set_alt_cm(
+            g2.waypoint_climb_slope_height_min * 100, Location::AltFrame::ABOVE_HOME);
+        set_target_altitude_location(min_loc);
+
+        return;
+    }
+
     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
+
+    // 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);