From 36e6aef981f6cd8f206ce0556b4bcd32bcdb0795 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Fri, 8 Dec 2023 16:04:38 +0000
Subject: [PATCH] Copter: allow copter to yaw to velocity target

add set_target_angle_and_rate_and_throttle() for precise vehicle control
don't allow set_target_angle_and_rate_and_throttle() to be called unless in guided mode.
---
 ArduCopter/Copter.cpp | 32 ++++++++++++++++++++++++++++++--
 ArduCopter/Copter.h   |  3 ++-
 2 files changed, 32 insertions(+), 3 deletions(-)

diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp
index 39988f300fe..96796f23096 100644
--- a/ArduCopter/Copter.cpp
+++ b/ArduCopter/Copter.cpp
@@ -341,14 +341,23 @@ bool Copter::set_target_posvelaccel_NED(const Vector3f& target_pos_ned_m, const
     return mode_guided.set_pos_vel_accel_NED_m(target_pos_ned_m.topostype(), target_vel_ned_ms, target_accel_ned_mss, use_yaw, radians(yaw_deg), use_yaw_rate, radians(yaw_rate_degs), yaw_relative);
 }
 
-bool Copter::set_target_velocity_NED(const Vector3f& target_vel_ned_ms)
+bool Copter::set_target_velocity_NED(const Vector3f& target_vel_ned_ms, bool align_yaw_to_target)
 {
     // exit if vehicle is not in Guided mode or Auto-Guided mode
     if (!flightmode->in_guided_mode()) {
         return false;
     }
 
-    mode_guided.set_vel_NED_ms(target_vel_ned_ms);
+    // optionally line up the copter with the velocity vector
+    float yaw_rads = 0.0f;
+    if (align_yaw_to_target) {
+        const float speed_sq = target_vel_ned_ms.xy().length_squared();
+        if (copter.position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED_MS * YAW_LOOK_AHEAD_MIN_SPEED_MS))) {
+            yaw_rads = atan2f(target_vel_ned_ms.y, target_vel_ned_ms.x);
+        }
+    }
+
+    mode_guided.set_vel_accel_NED_m(target_vel_ned_ms, Vector3f(), align_yaw_to_target, yaw_rads);
     return true;
 }
 
@@ -400,6 +409,25 @@ bool Copter::set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_
     return true;
 }
 
+// set target roll pitch and yaw angles and roll pitch and yaw rates with throttle (for use by scripting)
+bool Copter::set_target_angle_and_rate_and_throttle(float roll_deg, float pitch_deg, float yaw_deg, float roll_rate_degs, float pitch_rate_degs, float yaw_rate_degs, float throttle)
+{
+    // exit if vehicle is not in Guided mode or Auto-Guided mode
+    if (!flightmode->in_guided_mode()) {
+        return false;
+    }
+
+    Quaternion q;
+    q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg));
+
+    // Convert from degrees per second to radians per second
+    Vector3f ang_vel_body_degs { roll_rate_degs, pitch_rate_degs, yaw_rate_degs };
+    ang_vel_body_degs *= DEG_TO_RAD;
+
+    mode_guided.set_angle(q, ang_vel_body_degs, throttle, true);
+    return true;
+}
+
 // Register a custom mode with given number and names
 AP_Vehicle::custom_mode_state* Copter::register_custom_mode(const uint8_t num, const char* full_name, const char* short_name)
 {
diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index e84ca2841d5..ee32e7549f4 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -684,10 +684,11 @@ private:
     bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool is_terrain_alt) override;
     bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) override;
     bool set_target_posvelaccel_NED(const Vector3f& target_pos, const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) override;
-    bool set_target_velocity_NED(const Vector3f& vel_ned) override;
+    bool set_target_velocity_NED(const Vector3f& vel_ned, bool align_yaw_to_target) 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;
+    bool set_target_angle_and_rate_and_throttle(float roll_deg, float pitch_deg, float yaw_deg, float roll_rate_degs, float pitch_rate_degs, float yaw_rate_degs, float throttle) override;
 
     // Register a custom mode with given number and names
     AP_Vehicle::custom_mode_state* register_custom_mode(const uint8_t number, const char* full_name, const char* short_name) override;