From eb73a10cb8f98fc52aaf58ffac09ae5078a45034 Mon Sep 17 00:00:00 2001
From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
Date: Thu, 24 Oct 2024 17:42:19 +0900
Subject: [PATCH] Plane: Implement yaw rate control for DDS

* And add const to external control
* This method bypasses the loiter controller
---
 ArduPlane/AP_ExternalControl_Plane.cpp | 10 ++++++++++
 ArduPlane/AP_ExternalControl_Plane.h   |  6 ++++++
 ArduPlane/Plane.cpp                    | 18 ++++++++++++++++++
 ArduPlane/Plane.h                      |  1 +
 4 files changed, 35 insertions(+)

diff --git a/ArduPlane/AP_ExternalControl_Plane.cpp b/ArduPlane/AP_ExternalControl_Plane.cpp
index c5afabf584..361f6d6646 100644
--- a/ArduPlane/AP_ExternalControl_Plane.cpp
+++ b/ArduPlane/AP_ExternalControl_Plane.cpp
@@ -19,4 +19,14 @@ bool AP_ExternalControl_Plane::set_global_position(const Location& loc)
     return plane.set_target_location(loc);
 }
 
+/*
+    Sets only the target yaw rate
+    Yaw is in earth frame, NED [rad/s]
+*/
+bool AP_ExternalControl_Plane::set_yaw_rate(const float yaw_rate_rads)
+{
+    // TODO validate yaw rate is feasible for current dynamics
+    return plane.set_target_yaw_rate(yaw_rate_rads);
+}
+
 #endif // AP_EXTERNAL_CONTROL_ENABLED
diff --git a/ArduPlane/AP_ExternalControl_Plane.h b/ArduPlane/AP_ExternalControl_Plane.h
index 71308780d5..fa598fae2a 100644
--- a/ArduPlane/AP_ExternalControl_Plane.h
+++ b/ArduPlane/AP_ExternalControl_Plane.h
@@ -16,6 +16,12 @@ public:
         Sets the target global position for a loiter point.
     */
     bool set_global_position(const Location& loc) override WARN_IF_UNUSED;
+
+    /*
+        Sets only the target yaw rate
+        Yaw is in earth frame, NED [rad/s]
+    */
+    bool set_yaw_rate(const float yaw_rate_rads) override WARN_IF_UNUSED;
 };
 
 #endif // AP_EXTERNAL_CONTROL_ENABLED
diff --git a/ArduPlane/Plane.cpp b/ArduPlane/Plane.cpp
index 1990ca0ed2..a8f96e0492 100644
--- a/ArduPlane/Plane.cpp
+++ b/ArduPlane/Plane.cpp
@@ -884,6 +884,24 @@ bool Plane::set_target_location(const Location &target_loc)
     plane.set_guided_WP(loc);
     return true;
 }
+
+bool Plane::set_target_yaw_rate(const float yaw_rate)
+{
+    if (plane.control_mode != &plane.mode_guided) {
+        // only accept yaw rate updates when in GUIDED mode
+        return false;
+    }
+
+    const auto direction_is_ccw = yaw_rate < 0;
+    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
 
 #if AP_SCRIPTING_ENABLED
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 88ade0d534..9efd5893b2 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -1306,6 +1306,7 @@ public:
     bool is_taking_off() const override;
 #if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
     bool set_target_location(const Location& target_loc) override;
+    bool set_target_yaw_rate(const float yaw_rate_rads) override;
 #endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
 #if AP_SCRIPTING_ENABLED
     bool get_target_location(Location& target_loc) override;