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

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