From 727c640c3cd4456bbc536707a3d1d1acd9a72302 Mon Sep 17 00:00:00 2001 From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Date: Thu, 24 Oct 2024 21:22:06 +0900 Subject: [PATCH] Plane: Convert angular rate to radius Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> --- ArduPlane/Plane.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/ArduPlane/Plane.cpp b/ArduPlane/Plane.cpp index 022fea746ffae..a8f96e04920fd 100644 --- a/ArduPlane/Plane.cpp +++ b/ArduPlane/Plane.cpp @@ -893,8 +893,13 @@ bool Plane::set_target_yaw_rate(const float yaw_rate) } const auto direction_is_ccw = yaw_rate < 0; - plane.mode_guided.set_radius_and_direction(abs(yaw_rate), direction_is_ccw); - + if (!is_zero(yaw_rate)) { + auto const speed = plane.ahrs.groundspeed(); + auto const radius = speed / yaw_rate; + plane.mode_guided.set_radius_and_direction(abs(radius), direction_is_ccw); + } else { + plane.mode_guided.set_radius_and_direction(0.0, true); + } return true; } #endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED