From a5582bf32231a54e43113ed441ec0521926baefb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Jul 2022 10:01:36 +1000 Subject: [PATCH] Plane: adjust target vector for wind in overshoot at low speeds we want to keep the nose pointed into the wind on landings --- ArduPlane/quadplane.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 0bce362152..a57c270961 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2520,6 +2520,12 @@ void QuadPlane::vtol_position_controller(void) target_accel_cms.zero(); target_speed_xy_cms = diff_wp_norm * position2_target_speed * 100; have_target_yaw = true; + + // adjust target yaw angle for wind + const Vector2f wind = plane.ahrs.wind_estimate().xy(); + const float gnd_speed = plane.ahrs.groundspeed(); + Vector2f target_speed_xy = diff_wp_norm * gnd_speed - wind; + target_yaw_deg = degrees(target_speed_xy.angle()); } } const float target_speed_ms = target_speed_xy_cms.length() * 0.01;