Plane: Convert angular rate to radius

Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
ArduPilot · Dec 30, 2024 · 727c640 · 727c640
1 parent 4bf0bc7
commit 727c640
Showing 1 changed file with 7 additions and 2 deletions.
diff --git 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