From aa430a6e2c238ed80f1be62c5fe73f2fcdc936ef Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 30 Oct 2017 16:24:32 +1100
Subject: [PATCH] Plane: use RTL_RADIUS for QRTL threshold

when Q_RTL_MODE=1 in a quadplane use RTL_RADIUS as the distance to
switch to QRTL mode. We navigate to the destination using direct
waypoint navigation instead of using a loiter circle.
---
 ArduPlane/ArduPlane.cpp  |  4 +++-
 ArduPlane/navigation.cpp | 19 +++++++++++++------
 2 files changed, 16 insertions(+), 7 deletions(-)

diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index 7443b237b3..353aa5fe33 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -807,7 +807,9 @@ void Plane::update_navigation()
             
     case RTL:
         if (quadplane.available() && quadplane.rtl_mode == 1 &&
-            nav_controller->reached_loiter_target() &&
+            (nav_controller->reached_loiter_target() ||
+             location_passed_point(current_loc, prev_WP_loc, next_WP_loc) ||
+             auto_state.wp_distance < (uint16_t)g.rtl_radius) &&
             AP_HAL::millis() - last_mode_change_ms > 1000) {
             set_mode(QRTL, MODE_REASON_UNKNOWN);
             break;
diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp
index 669f5423e8..e53d03a798 100644
--- a/ArduPlane/navigation.cpp
+++ b/ArduPlane/navigation.cpp
@@ -185,12 +185,19 @@ void Plane::update_loiter(uint16_t radius)
             loiter.start_time_ms = 0;
             quadplane.guided_start();
         }
-    } else if (loiter.start_time_ms == 0 &&
-        control_mode == AUTO &&
-        !auto_state.no_crosstrack &&
-        get_distance(current_loc, next_WP_loc) > radius*3) {
-        // if never reached loiter point and using crosstrack and somewhat far away from loiter point
-        // navigate to it like in auto-mode for normal crosstrack behavior
+    } else if ((loiter.start_time_ms == 0 &&
+                control_mode == AUTO &&
+                !auto_state.no_crosstrack &&
+                get_distance(current_loc, next_WP_loc) > radius*3) ||
+               (quadplane.available() && quadplane.rtl_mode == 1)) {
+        /*
+          if never reached loiter point and using crosstrack and somewhat far away from loiter point
+          navigate to it like in auto-mode for normal crosstrack behavior
+
+          we also use direct waypoint navigation if we are a quadplane
+          that is going to be switching to QRTL when it gets within
+          RTL_RADIUS
+        */
         nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
     } else {
         nav_controller->update_loiter(next_WP_loc, radius, loiter.direction);