From 833f4945edd3b0a39eefee124e3289ae3d758bad Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 29 Oct 2021 07:36:09 +1100
Subject: [PATCH] Plane: avoid qpos approach when very close to destination

this prevents a fwd transition when doing something like LOITER_TIME
close to a VTOL_LAND wp. We use 1.5 times the stopping distance at
cruise airspeed for the threshold
---
 ArduPlane/quadplane.cpp | 38 ++++++++++++++++++++++++++++++++++----
 ArduPlane/quadplane.h   |  7 ++++++-
 2 files changed, 40 insertions(+), 5 deletions(-)

diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 85113beabb..7581093224 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -2581,9 +2581,21 @@ void QuadPlane::poscontrol_init_approach(void)
         // go straight to QPOS_POSITION1
         poscontrol.set_state(QPOS_POSITION1);
     } else if (poscontrol.get_state() != QPOS_APPROACH) {
+        // check if we are close to the destination. We don't want to
+        // do a full approach when very close
         const float dist = plane.current_loc.get_distance(plane.next_WP_loc);
-        gcs().send_text(MAV_SEVERITY_INFO,"VTOL approach d=%.1f", dist);
-        poscontrol.set_state(QPOS_APPROACH);
+        if (dist < transition_threshold()) {
+            if (is_tailsitter() || motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
+                gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 d=%.1f", dist);
+                poscontrol.set_state(QPOS_POSITION1);
+            } else {
+                gcs().send_text(MAV_SEVERITY_INFO,"VTOL short d=%.1f", dist);
+                poscontrol.set_state(QPOS_AIRBRAKE);
+            }
+        } else {
+            gcs().send_text(MAV_SEVERITY_INFO,"VTOL approach d=%.1f", dist);
+            poscontrol.set_state(QPOS_APPROACH);
+        }
         poscontrol.thrust_loss_start_ms = 0;
     }
 }
@@ -3944,12 +3956,30 @@ bool QuadPlane::in_transition(void) const
 /*
   calculate current stopping distance for a quadplane in fixed wing flight
  */
-float QuadPlane::stopping_distance(void)
+float QuadPlane::stopping_distance(float ground_speed_squared)
 {
     // use v^2/(2*accel). This is only quite approximate as the drag
     // varies with pitch, but it gives something for the user to
     // control the transition distance in a reasonable way
-    return plane.ahrs.groundspeed_vector().length_squared() / (2 * transition_decel);
+    return ground_speed_squared / (2 * transition_decel);
+}
+
+/*
+  calculate current stopping distance for a quadplane in fixed wing flight
+ */
+float QuadPlane::stopping_distance(void)
+{
+    return stopping_distance(plane.ahrs.groundspeed_vector().length_squared());
+}
+
+/*
+  distance below which we don't do approach, based on stopping
+  distance for cruise speed
+ */
+float QuadPlane::transition_threshold(void)
+{
+    // 1.5 times stopping distance for cruise speed
+    return 1.5 * stopping_distance(sq(plane.aparm.airspeed_cruise_cm*0.01));
 }
 
 #define LAND_CHECK_ANGLE_ERROR_DEG  30.0f       // maximum angle error to be considered landing
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index ef5bf6be4e..f916536b13 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -304,8 +304,13 @@ private:
     void setup_defaults(void);
 
     // calculate a stopping distance for fixed-wing to vtol transitions
+    float stopping_distance(float ground_speed_squared);
     float stopping_distance(void);
-    
+
+    // distance below which we don't do approach, based on stopping
+    // distance for cruise speed
+    float transition_threshold(void);
+
     AP_Int16 transition_time_ms;
 
     // transition deceleration, m/s/s