From d013878c17a07b318be815be8b4a37839ac4b3a7 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 20 Apr 2016 18:07:04 +1000
Subject: [PATCH] Plane: improved speed limiting on landing approach in VTOL
 mode

---
 ArduPlane/quadplane.cpp | 28 ++++++++++++++++++----------
 ArduPlane/quadplane.h   |  1 +
 2 files changed, 19 insertions(+), 10 deletions(-)

diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index bd5fd727d0..b540c1101a 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -1080,14 +1080,14 @@ void QuadPlane::control_auto(const Location &loc)
             // velocity as we approach the waypoint, aiming for zero
             // speed at the waypoint
             Vector2f groundspeed = ahrs.groundspeed_vector();
-            // newdiff is the delta to our target if we keep going for one second
-            Vector2f newdiff = diff_wp - groundspeed;
-            // speed towards target is the change in distance to target over one second
-            float speed_towards_target = diff_wp.length() - newdiff.length();
+            float speed_towards_target = diff_wp.normalized() * groundspeed;
             // setup land_speed_scale so at current distance we maintain speed towards target, and slow down as
             // we approach
             float distance = diff_wp.length();
-            land.speed_scale = MAX(speed_towards_target, wp_nav->get_speed_xy() * 0.01) / MAX(distance, 1);
+
+            // max_speed will control how fast we will fly. It will always decrease
+            land.max_speed = MAX(speed_towards_target, wp_nav->get_speed_xy() * 0.01);
+            land.speed_scale = land.max_speed / MAX(distance, 1);
         }
 
         // run fixed wing navigation
@@ -1097,12 +1097,20 @@ void QuadPlane::control_auto(const Location &loc)
           calculate target velocity, not dropping it below 2m/s
          */
         const float final_speed = 2.0f;
-        Vector2f target_speed = diff_wp * land.speed_scale;
-        if (target_speed.length() < final_speed) {
-            target_speed = target_speed.normalized() * final_speed;
+        Vector2f target_speed_xy = diff_wp * land.speed_scale;
+        float target_speed = target_speed_xy.length();
+        if (target_speed < final_speed) {
+            // until we enter the loiter we always aim for at least 2m/s
+            target_speed_xy = target_speed_xy.normalized() * final_speed;
+            land.max_speed = final_speed;
+        } else if (target_speed > land.max_speed) {
+            // we never speed up during landing approaches
+            target_speed_xy = target_speed_xy.normalized() * land.max_speed;
+        } else {
+            land.max_speed = target_speed;
         }
-        pos_control->set_desired_velocity_xy(target_speed.x*100,
-                                             target_speed.y*100);
+        pos_control->set_desired_velocity_xy(target_speed_xy.x*100,
+                                             target_speed_xy.y*100);
         
         float ekfGndSpdLimit, ekfNavVelGainScaler;    
         ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index da13f7114d..90d91d8ef5 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -230,6 +230,7 @@ private:
         int32_t yaw_cd;
         float speed_scale;
         Vector2f target_velocity;
+        float max_speed;
     } land;
 
     enum frame_class {