From 3bac3618e1120f6b3a5e78f5723a840be22213be Mon Sep 17 00:00:00 2001
From: Nick Exton <NExton@sypaq.com.au>
Date: Tue, 1 Oct 2024 15:39:28 +1000
Subject: [PATCH] Copter: Add set_yaw_angle_offset() function to AutoYaw mode

---
 ArduCopter/autoyaw.cpp | 12 ++++++++++++
 ArduCopter/mode.h      |  2 ++
 2 files changed, 14 insertions(+)

diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp
index e63c08fe7b..9ce780a9a0 100644
--- a/ArduCopter/autoyaw.cpp
+++ b/ArduCopter/autoyaw.cpp
@@ -149,6 +149,18 @@ void Mode::AutoYaw::set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds)
     set_mode(Mode::ANGLE_RATE);
 }
 
+// set_yaw_angle_offset - sets the yaw look at heading for auto mode, as an offset from the current yaw angle
+void Mode::AutoYaw::set_yaw_angle_offset(const float yaw_angle_offset_d)
+{
+    _last_update_ms = millis();
+
+    _yaw_angle_cd = wrap_360_cd(_yaw_angle_cd + (yaw_angle_offset_d * 100.0));
+    _yaw_rate_cds = 0.0f;
+
+    // set yaw mode
+    set_mode(Mode::ANGLE_RATE);
+}
+
 // set_roi - sets the yaw to look at roi for auto mode
 void Mode::AutoYaw::set_roi(const Location &roi_location)
 {
diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index 009e74edd8..98f01449c5 100644
--- a/ArduCopter/mode.h
+++ b/ArduCopter/mode.h
@@ -335,6 +335,8 @@ public:
 
         void set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds);
 
+        void set_yaw_angle_offset(const float yaw_angle_offset_d);
+
         bool reached_fixed_yaw_target();
 
 #if WEATHERVANE_ENABLED