Copter: add RTL_CLIMB_MIN

Vehicle climbs at least this many cm when entering RTL

ArduCopter/Parameters.cpp

@@ -196,6 +196,15 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(rtl_alt_final,  "RTL_ALT_FINAL", RTL_ALT_FINAL),
 
+    // @Param: RTL_CLIMB_MIN
+    // @DisplayName: RTL minimum climb
+    // @Description: The vehicle will climb this many cm during the initial climb portion of the RTL
+    // @Units: Centimeters
+    // @Range: 0 3000
+    // @Increment: 10
+    // @User: Standard
+    GSCALAR(rtl_climb_min,  "RTL_CLIMB_MIN",    RTL_CLIMB_MIN_DEFAULT),
+
     // @Param: RSSI_PIN
     // @DisplayName: Receiver RSSI sensing pin
     // @Description: This selects an analog pin for the receiver RSSI voltage. It assumes the voltage is RSSI_RANGE for max rssi, 0V for minimum

ArduCopter/Parameters.h

@@ -335,7 +335,8 @@ public:
         k_param_autotune_axis_bitmask,
         k_param_autotune_aggressiveness,
         k_param_pi_vel_xy,
-        k_param_fs_ekf_action, // 248
+        k_param_fs_ekf_action,
+        k_param_rtl_climb_min,  // 249
 
         // 254,255: reserved
     };
@@ -370,6 +371,7 @@ public:
     AP_Int8         compass_enabled;
     AP_Int8         super_simple;
     AP_Int16        rtl_alt_final;
+    AP_Int16        rtl_climb_min;              // rtl minimum climb in cm
 
     AP_Int8         rssi_pin;
     AP_Float        rssi_range;                 // allows to set max voltage for rssi pin such as 5.0, 3.3 etc. 

ArduCopter/config.h

@@ -494,6 +494,10 @@
  # define RTL_ALT_MIN               200     // min height above ground for RTL (i.e 2m)
 #endif
 
+#ifndef RTL_CLIMB_MIN_DEFAULT
+ # define RTL_CLIMB_MIN_DEFAULT     0       // vehicle will always climb this many cm as first stage of RTL
+#endif
+
 #ifndef RTL_LOITER_TIME
  # define RTL_LOITER_TIME           5000    // Time (in milliseconds) to loiter above home before begining final descent
 #endif

ArduCopter/control_rtl.cpp

@@ -389,8 +389,8 @@ void Copter::rtl_land_run()
 //      altitude is in cm above home
 float Copter::get_RTL_alt()
 {
-    // maximum of current altitude and rtl altitude
-    float rtl_alt = max(current_loc.alt, g.rtl_altitude);
+    // maximum of current altitude + climb_min and rtl altitude
+    float rtl_alt = max(current_loc.alt + max(0, g.rtl_climb_min), g.rtl_altitude);
     rtl_alt = max(rtl_alt, RTL_ALT_MIN);
 
 #if AC_FENCE == ENABLED