From 5f5f70d20a23384a6e78c82ec0f2f27fea4c60f5 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 29 Jun 2022 10:10:51 +1000
Subject: [PATCH] Plane: fixed check for fixed wing approach QRTL start

ensure we are lined up, or we are at less than 0.5 of radius before we
switch to QRTL. This fixes the case where the stopping distance is
greater than the radius, prevening us from switching to QRTL while not
lined up with the landing point
---
 ArduPlane/commands_logic.cpp | 38 ++++++++++++++++++------------------
 1 file changed, 19 insertions(+), 19 deletions(-)

diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
index 76deb01bd3..e3f62509e6 100644
--- a/ArduPlane/commands_logic.cpp
+++ b/ArduPlane/commands_logic.cpp
@@ -1018,6 +1018,10 @@ void Plane::exit_mission_callback()
 #if HAL_QUADPLANE_ENABLED
 bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
 {
+    const float radius = is_zero(quadplane.fw_land_approach_radius)? aparm.loiter_radius : quadplane.fw_land_approach_radius;
+    const int8_t direction = is_negative(radius) ? -1 : 1;
+    const float abs_radius = fabsf(radius);
+
     switch (vtol_approach_s.approach_stage) {
         case RTL:
             {
@@ -1045,21 +1049,12 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
             }
         case ENSURE_RADIUS:
             {
-                float radius;
-                if (is_zero(quadplane.fw_land_approach_radius)) {
-                    radius = aparm.loiter_radius;
-                } else {
-                    radius = quadplane.fw_land_approach_radius;
-                }
-                const int8_t direction = is_negative(radius) ? -1 : 1;
-                radius = fabsf(radius);
-
                 // validate that the vehicle is at least the expected distance away from the loiter point
                 // require an angle total of at least 2 centidegrees, due to special casing of 1 centidegree
-                if (((fabsf(cmd.content.location.get_distance(current_loc) - radius) > 5.0f) &&
-                      (cmd.content.location.get_distance(current_loc) < radius)) ||
+                if (((fabsf(cmd.content.location.get_distance(current_loc) - abs_radius) > 5.0f) &&
+                      (cmd.content.location.get_distance(current_loc) < abs_radius)) ||
                     (loiter.sum_cd < 2)) {
-                    nav_controller->update_loiter(cmd.content.location, radius, direction);
+                    nav_controller->update_loiter(cmd.content.location, abs_radius, direction);
                     break;
                 }
                 vtol_approach_s.approach_stage = WAIT_FOR_BREAKOUT;
@@ -1067,12 +1062,6 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
             }
         case WAIT_FOR_BREAKOUT:
             {
-                float radius = quadplane.fw_land_approach_radius;
-                if (is_zero(radius)) {
-                    radius = aparm.loiter_radius;
-                }
-                const int8_t direction = is_negative(radius) ? -1 : 1;
-
                 nav_controller->update_loiter(cmd.content.location, radius, direction);
 
                 const float breakout_direction_rad = radians(wrap_180(vtol_approach_s.approach_direction_deg + (direction > 0 ? 270 : 90)));
@@ -1103,7 +1092,18 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
                 // check if we should move on to the next waypoint
                 Location breakout_loc = cmd.content.location;
                 breakout_loc.offset_bearing(vtol_approach_s.approach_direction_deg + 180, quadplane.stopping_distance());
-                if(current_loc.past_interval_finish_line(start, breakout_loc)) {
+
+                const bool past_finish_line = current_loc.past_interval_finish_line(start, breakout_loc);
+                const bool half_radius = current_loc.get_distance(cmd.content.location) < 0.5 * abs_radius;
+                bool lined_up = true;
+                Vector3f vel_NED;
+                if (ahrs.get_velocity_NED(vel_NED)) {
+                    const Vector2f target_vec = current_loc.get_distance_NE(cmd.content.location);
+                    const float angle_err = fabsf(wrap_180(degrees(vel_NED.xy().angle(target_vec))));
+                    lined_up = (angle_err < 30);
+                }
+
+                if (past_finish_line && (lined_up || half_radius)) {
                     vtol_approach_s.approach_stage = VTOL_LANDING;
                     quadplane.do_vtol_land(cmd);
                     // fallthrough