Plane: implement RTL_CLIMB_MIN parameter

this allows for an initial climb on RTL where roll is limited to
LEVEL_ROLL_LIMIT

ArduPlane/ArduPlane.cpp

@@ -581,7 +581,16 @@ void Plane::update_alt()
             distance_beyond_land_wp = current_loc.get_distance(next_WP_loc);
         }
 
-        SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
+        float target_alt = relative_target_altitude_cm();
+
+        if (control_mode == &mode_rtl && !rtl.done_climb && g2.rtl_climb_min > 0) {
+            // ensure we do the initial climb in RTL. We add an extra
+            // 10m in the demanded height to push TECS to climb
+            // quickly
+            target_alt = MAX(target_alt, prev_WP_loc.alt + (g2.rtl_climb_min+10)*100);
+        }
+
+        SpdHgt_Controller->update_pitch_throttle(target_alt,
                                                  target_airspeed_cm,
                                                  flight_stage,
                                                  distance_beyond_land_wp,

ArduPlane/Parameters.cpp

@@ -1278,6 +1278,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("FS_EKF_THRESH", 26, ParametersG2, fs_ekf_thresh, FS_EKF_THRESHOLD_DEFAULT),
 
+    // @Param: RTL_CLIMB_MIN
+    // @DisplayName: RTL minimum climb
+    // @Description: The vehicle will climb this many m during the initial climb portion of the RTL. During this time the roll will be limited to LEVEL_ROLL_LIMIT degrees.
+    // @Units: m
+    // @Range: 0 30
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("RTL_CLIMB_MIN", 27, ParametersG2, rtl_climb_min, 0),
+
     AP_GROUPEND
 };
 

ArduPlane/Parameters.h

@@ -577,6 +577,9 @@ public:
 #endif
 
     AP_Float        fs_ekf_thresh;
+
+    // min initial climb in RTL
+    AP_Int16        rtl_climb_min;
 };
 
 extern const AP_Param::Info var_info[];

ArduPlane/Plane.h

@@ -726,6 +726,10 @@ private:
         uint32_t last_trim_save;
     } auto_trim;
 
+    struct {
+        bool done_climb;
+    } rtl;
+
     // last time home was updated while disarmed
     uint32_t last_home_update_ms;
 

ArduPlane/mode_rtl.cpp

@@ -8,6 +8,7 @@ bool ModeRTL::_enter()
     plane.auto_navigation_mode = true;
     plane.prev_WP_loc = plane.current_loc;
     plane.do_RTL(plane.get_RTL_altitude());
+    plane.rtl.done_climb = false;
 
     return true;
 }
@@ -17,5 +18,19 @@ void ModeRTL::update()
     plane.calc_nav_roll();
     plane.calc_nav_pitch();
     plane.calc_throttle();
+
+    if (plane.g2.rtl_climb_min > 0) {
+        /*
+          when RTL first starts limit bank angle to LEVEL_ROLL_LIMIT
+          until we have climbed by RTL_CLIMB_MIN meters
+         */
+        if (!plane.rtl.done_climb && (plane.current_loc.alt - plane.prev_WP_loc.alt)*0.01 > plane.g2.rtl_climb_min) {
+            plane.rtl.done_climb = true;
+        }
+        if (!plane.rtl.done_climb) {
+            plane.roll_limit_cd = MIN(plane.roll_limit_cd, plane.g.level_roll_limit*100);
+            plane.nav_roll_cd = constrain_int32(plane.nav_roll_cd, -plane.roll_limit_cd, plane.roll_limit_cd);
+        }
+    }
 }