Plane: Quadaplane: add param for min QRTL altitude when close to home

ArduPlane/mode_qrtl.cpp

@@ -23,8 +23,9 @@ bool ModeQRTL::_enter()
         const float radius = MAX(fabsf(plane.aparm.loiter_radius), fabsf(plane.g.rtl_radius));
 
         // Climb at least to a cone around home of hight of QRTL alt and radius of 1.5*radius
-        // Always climb up to at least land final alt
-        const float target_alt = MAX(quadplane.qrtl_alt * (dist / MAX(1.5*radius, dist)), quadplane.land_final_alt);
+        // Always climb up to at least Q_RTL_ALT_MIN, constrain Q_RTL_ALT_MIN between Q_LAND_FINAL_ALT and Q_RTL_ALT
+        const float min_climb = constrain_float(quadplane.qrtl_alt_min, quadplane.land_final_alt, quadplane.qrtl_alt);
+        const float target_alt = MAX(quadplane.qrtl_alt * (dist / MAX(1.5*radius, dist)), min_climb);
 
 
 #if AP_TERRAIN_AVAILABLE

ArduPlane/quadplane.cpp

@@ -495,6 +495,15 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
     // @User: Standard
     AP_SUBGROUPINFO(command_model_pilot, "PLT_Y_", 33, QuadPlane, AC_CommandModel),
 
+    // @Param: RTL_ALT_MIN
+    // @DisplayName: QRTL minimum altitude
+    // @Description: If VTOL motors are active QRTL mode will VTOL climb to at least this altitude before returning home. If outside 150% the larger of WP_LOITER_RAD and RTL_RADIUS the vehicle will VTOL climb to Q_RTL_ALT. This parameter has no effect if the vehicle is in forward flight. Should be between Q_LAND_FINAL_ALT and Q_RTL_ALT
+    // @Units: m
+    // @Range: 1 200
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("RTL_ALT_MIN", 34, QuadPlane, qrtl_alt_min, 10),
+
     AP_GROUPEND
 };
 

ArduPlane/quadplane.h

@@ -337,6 +337,7 @@ private:
 
     // QRTL start altitude, meters
     AP_Int16 qrtl_alt;
+    AP_Int16 qrtl_alt_min;
     
     // alt to switch to QLAND_FINAL
     AP_Float land_final_alt;