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

@@ -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;

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);