From 94944427dae8522baa9acb415dfc6993baa9e1f5 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sun, 15 Sep 2024 01:02:26 +0100
Subject: [PATCH] Copter: Scripting: add support for
 `set_target_rate_and_throttle`

---
 ArduCopter/Copter.cpp | 22 ++++++++++++++++++++++
 ArduCopter/Copter.h   |  2 ++
 2 files changed, 24 insertions(+)

diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp
index e635c808a5..47e4596902 100644
--- a/ArduCopter/Copter.cpp
+++ b/ArduCopter/Copter.cpp
@@ -376,6 +376,7 @@ bool Copter::set_target_velaccel_NED(const Vector3f& target_vel, const Vector3f&
     return true;
 }
 
+// set target roll pitch and yaw angles with throttle (for use by scripting)
 bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs)
 {
     // exit if vehicle is not in Guided mode or Auto-Guided mode
@@ -389,6 +390,27 @@ bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, flo
     mode_guided.set_angle(q, Vector3f{}, climb_rate_ms*100, false);
     return true;
 }
+
+// set target roll pitch and yaw rates with throttle (for use by scripting)
+bool Copter::set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle)
+{
+    // exit if vehicle is not in Guided mode or Auto-Guided mode
+    if (!flightmode->in_guided_mode()) {
+        return false;
+    }
+
+    // Zero quaternion indicates rate control
+    Quaternion q;
+    q.zero();
+
+    // Convert from degrees per second to radians per second
+    Vector3f ang_vel_body { roll_rate_dps, pitch_rate_dps, yaw_rate_dps };
+    ang_vel_body *= DEG_TO_RAD;
+
+    // Pass to guided mode
+    mode_guided.set_angle(q, ang_vel_body, throttle, true);
+    return true;
+}
 #endif
 
 #if MODE_CIRCLE_ENABLED
diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 8a9822d165..3bdba074d5 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -681,6 +681,8 @@ private:
     bool set_target_velocity_NED(const Vector3f& vel_ned) override;
     bool set_target_velaccel_NED(const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool relative_yaw) override;
     bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) override;
+    bool set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle) override;
+
 #endif
 #if MODE_CIRCLE_ENABLED
     bool get_circle_radius(float &radius_m) override;