From 5aba7bef2984b389eb6ddea903360034c25ccaaf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 31 Jul 2022 15:51:47 +1000 Subject: [PATCH] Plane: base yaw in overshoot on target speed not scaled speed this prevents an oscillating due to the speed changing as we yaw, resulting in a different yaw target --- ArduPlane/quadplane.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index a1178c3bcb..332d16762e 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2522,7 +2522,7 @@ void QuadPlane::vtol_position_controller(void) // allow up to the WP speed when we are further away, slowing to the pos2 target speed // when we are close - target_speed = linear_interpolate(position2_target_speed, scaled_wp_speed, + target_speed = linear_interpolate(position2_target_speed, wp_speed, distance, position2_dist_threshold*1.5, 2*position2_dist_threshold + stopping_distance(rel_groundspeed_sq)); @@ -2530,11 +2530,15 @@ void QuadPlane::vtol_position_controller(void) target_speed_xy_cms = diff_wp_norm * target_speed * 100; have_target_yaw = true; - // adjust target yaw angle for wind + // adjust target yaw angle for wind. We calculate yaw based on the target speed + // we want assuming no speed scaling due to direction const Vector2f wind = plane.ahrs.wind_estimate().xy(); const float gnd_speed = plane.ahrs.groundspeed(); Vector2f target_speed_xy = landing_velocity + diff_wp_norm * gnd_speed - wind; target_yaw_deg = degrees(target_speed_xy.angle()); + + // now limit speed to scaled wp speed, so we don't fly rapidly backwards + target_speed_xy.limit_length(scaled_wp_speed*100); } } const float target_speed_ms = target_speed_xy_cms.length() * 0.01;