From 1aa29d6b3f964bc534adda86607e8cd41e0b73bc Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Thu, 24 Nov 2022 22:50:36 +0000
Subject: [PATCH] Plane: QRTL if RTL is expecting to VTOL land and close home
 with VTOL motors active VO1-272

---
 ArduPlane/mode.h       |  2 +-
 ArduPlane/mode_rtl.cpp | 53 +++++++++++++++++++++++++++---------------
 2 files changed, 35 insertions(+), 20 deletions(-)

diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
index 0dafd9a0eb..8bdd1724f3 100644
--- a/ArduPlane/mode.h
+++ b/ArduPlane/mode.h
@@ -339,7 +339,7 @@ class ModeRTL : public Mode
 private:
 
     // Switch to QRTL if enabled and within radius
-    bool switch_QRTL(bool check_loiter_target = true);
+    bool switch_QRTL();
 };
 
 class ModeStabilize : public Mode
diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp
index a24ea10b65..902749f0fc 100644
--- a/ArduPlane/mode_rtl.cpp
+++ b/ArduPlane/mode_rtl.cpp
@@ -9,15 +9,36 @@ bool ModeRTL::_enter()
 #if HAL_QUADPLANE_ENABLED
     plane.vtol_approach_s.approach_stage = Plane::Landing_ApproachStage::RTL;
 
-    // treat RTL as QLAND if we are in guided wait takeoff state, to cope
-    // with failsafes during GUIDED->AUTO takeoff sequence
-    if (plane.quadplane.guided_wait_takeoff_on_mode_enter) {
-       plane.set_mode(plane.mode_qland, ModeReason::QLAND_INSTEAD_OF_RTL);
-       return true;
-    }
+    // Quadplane specific checks
+    if (plane.quadplane.available()) {
+        // treat RTL as QLAND if we are in guided wait takeoff state, to cope
+        // with failsafes during GUIDED->AUTO takeoff sequence
+        if (plane.quadplane.guided_wait_takeoff_on_mode_enter) {
+            plane.set_mode(plane.mode_qland, ModeReason::QLAND_INSTEAD_OF_RTL);
+            return true;
+        }
 
-    // do not check if we have reached the loiter target if switching from loiter this will trigger as the nav controller has not yet proceeded the new destination
-    switch_QRTL(false);
+        // if Q_RTL_MODE is QRTL always, immediately switch to QRTL mode
+        if (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::QRTL_ALWAYS) {
+            plane.set_mode(plane.mode_qrtl, ModeReason::QRTL_INSTEAD_OF_RTL);
+            return true;
+        }
+
+        // if VTOL landing is expected and quadplane motors are active and within QRTL radius and under QRTL altitude then switch to QRTL
+        const bool vtol_landing = (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::SWITCH_QRTL) || (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::VTOL_APPROACH_QRTL);
+        if (vtol_landing && (quadplane.motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED)) {
+            uint16_t qrtl_radius = abs(plane.g.rtl_radius);
+            if (qrtl_radius == 0) {
+                qrtl_radius = abs(plane.aparm.loiter_radius);
+            }
+            int32_t alt_cm;
+            if ((plane.current_loc.get_distance(plane.next_WP_loc) < qrtl_radius) &&
+                plane.current_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_cm) && (alt_cm < plane.quadplane.qrtl_alt*100)) {
+                plane.set_mode(plane.mode_qrtl, ModeReason::QRTL_INSTEAD_OF_RTL);
+                return true;
+            }
+        }
+    }
 #endif
 
     return true;
@@ -60,10 +81,10 @@ void ModeRTL::update()
 void ModeRTL::navigate()
 {
 #if HAL_QUADPLANE_ENABLED
-    if (plane.control_mode->mode_number() != QRTL) {
+    if ((plane.control_mode->mode_number() != QRTL) && plane.quadplane.available()) {
         // QRTL shares this navigate function with RTL
 
-        if (plane.quadplane.available() && (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::VTOL_APPROACH_QRTL)) {
+        if (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::VTOL_APPROACH_QRTL) {
             // VTOL approach landing
             AP_Mission::Mission_Command cmd;
             cmd.content.location = plane.next_WP_loc;
@@ -118,24 +139,18 @@ void ModeRTL::navigate()
 
 #if HAL_QUADPLANE_ENABLED
 // Switch to QRTL if enabled and within radius
-bool ModeRTL::switch_QRTL(bool check_loiter_target)
+bool ModeRTL::switch_QRTL()
 {
-    if (!plane.quadplane.available() || ((plane.quadplane.rtl_mode != QuadPlane::RTL_MODE::SWITCH_QRTL) && (plane.quadplane.rtl_mode != QuadPlane::RTL_MODE::QRTL_ALWAYS))) {  
+    if (plane.quadplane.rtl_mode != QuadPlane::RTL_MODE::SWITCH_QRTL) {
         return false;
     }
 
-   // if Q_RTL_MODE is QRTL always, then immediately switch to QRTL mode
-   if (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::QRTL_ALWAYS) {
-       plane.set_mode(plane.mode_qrtl, ModeReason::QRTL_INSTEAD_OF_RTL);
-       return true;
-   }
-
     uint16_t qrtl_radius = abs(plane.g.rtl_radius);
     if (qrtl_radius == 0) {
         qrtl_radius = abs(plane.aparm.loiter_radius);
     }
 
-    if ( (check_loiter_target && plane.nav_controller->reached_loiter_target()) ||
+    if (plane.nav_controller->reached_loiter_target() ||
          plane.current_loc.past_interval_finish_line(plane.prev_WP_loc, plane.next_WP_loc) ||
          plane.auto_state.wp_distance < MAX(qrtl_radius, plane.quadplane.stopping_distance())) {
         /*