From bf9d49b72daf7a61e64d79fbdabe468418ea6356 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Fri, 13 Jan 2023 00:35:09 +0000
Subject: [PATCH] Plane: Quadaplane: add param for min QRTL altitude when close
 to home

---
 ArduPlane/mode_qrtl.cpp | 5 +++--
 ArduPlane/quadplane.cpp | 9 +++++++++
 ArduPlane/quadplane.h   | 1 +
 3 files changed, 13 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/mode_qrtl.cpp b/ArduPlane/mode_qrtl.cpp
index 64804abf9e..97d4bb02c5 100644
--- a/ArduPlane/mode_qrtl.cpp
+++ b/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
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 6ecbef914b..14ade88789 100644
--- a/ArduPlane/quadplane.cpp
+++ b/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
 };
 
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index 45495d71c4..9b6f4759a6 100644
--- a/ArduPlane/quadplane.h
+++ b/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;