From 1c4cc3670a20b2d0bf1a7d83fa7af44817c17ec7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 2 Aug 2022 07:55:14 +1000 Subject: [PATCH] Plane: improve target airspeed in landing approach if the user hasn't set TECS_LAND_ARSPD then we can use an airspeed between ARSPD_FBW_MIN and TRIM_ARSPD_CM when on approach this also fixes landing_desired_closing_velocity() to never go above the landing target speed, so we don't try to speed up if we are starting the landing sequence too early --- ArduPlane/quadplane.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 332d16762e..c1bb94f240 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2344,7 +2344,11 @@ void QuadPlane::vtol_position_controller(void) // use nav controller roll plane.calc_nav_roll(); - const float stop_distance = stopping_distance(); + // work out the point to enter airbrake mode. We want enough + // distance to stop, plus some margin for the time it takes to + // change the accel (jerk limit) plus the min time in airbrake + // mode. For simplicity we assume 2 seconds margin + const float stop_distance = stopping_distance() + 2*closing_speed; if (!suppress_z_controller && poscontrol.get_state() == QPOS_AIRBRAKE) { hold_hover(0); @@ -3965,12 +3969,12 @@ float QuadPlane::get_land_airspeed(void) approach_speed = cruise_speed; } } - const float time_to_landing = plane.auto_state.wp_distance / MAX(approach_speed, 5); + const float time_to_pos1 = (plane.auto_state.wp_distance - stopping_distance(sq(approach_speed))) / MAX(approach_speed, 5); /* slow down to landing approach speed as we get closer to landing */ approach_speed = linear_interpolate(approach_speed, cruise_speed, - time_to_landing, + time_to_pos1, 20, 60); return approach_speed; }