From 715bb6e7056a91d723e8e0dc2c8af61cddf39754 Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Sat, 4 Mar 2023 21:32:41 +1030
Subject: [PATCH] Copter: Guided Yaw Fix

---
 ArduCopter/autoyaw.cpp | 63 +++++++++++++++++++++++++-----------------
 ArduCopter/mode.h      |  7 ++---
 2 files changed, 40 insertions(+), 30 deletions(-)

diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp
index 6853789ee3..3254dfde78 100644
--- a/ArduCopter/autoyaw.cpp
+++ b/ArduCopter/autoyaw.cpp
@@ -92,7 +92,7 @@ void Mode::AutoYaw::set_mode(Mode yaw_mode)
 
     case Mode::RATE:
         // initialise target yaw rate to zero
-        _yaw_rate_cds = 0.0f;
+        _yaw_rate_cds = 0.0;
         break;
 
     case Mode::CIRCLE:
@@ -108,9 +108,6 @@ void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t di
 {
     _last_update_ms = millis();
 
-    _yaw_angle_cd = copter.attitude_control->get_att_target_euler_cd().z;
-    _yaw_rate_cds = 0.0;
-
     // calculate final angle as relative to vehicle heading or absolute
     if (relative_angle) {
         _fixed_yaw_offset_cd = angle_deg * 100.0 * (direction >= 0 ? 1.0 : -1.0);
@@ -211,14 +208,15 @@ bool Mode::AutoYaw::reached_fixed_yaw_target()
     return (fabsf(wrap_180_cd(copter.ahrs.yaw_sensor-_yaw_angle_cd)) <= 200);
 }
 
-// yaw - returns target heading depending upon auto_yaw.mode()
-float Mode::AutoYaw::yaw()
+// yaw_cd - returns target heading depending upon auto_yaw.mode()
+float Mode::AutoYaw::yaw_cd()
 {
     switch (_mode) {
 
     case Mode::ROI:
         // point towards a location held in roi
-        return roi_yaw();
+        _yaw_angle_cd = roi_yaw();
+        break;
 
     case Mode::FIXED: {
         // keep heading pointing in the direction held in fixed_yaw
@@ -229,45 +227,55 @@ float Mode::AutoYaw::yaw()
         float yaw_angle_step = constrain_float(_fixed_yaw_offset_cd, - dt * _fixed_yaw_slewrate_cds, dt * _fixed_yaw_slewrate_cds);
         _fixed_yaw_offset_cd -= yaw_angle_step;
         _yaw_angle_cd += yaw_angle_step;
-        return _yaw_angle_cd;
+        break;
     }
 
     case Mode::LOOK_AHEAD:
         // Commanded Yaw to automatically look ahead.
-        return look_ahead_yaw();
+        _yaw_angle_cd = look_ahead_yaw();
+        break;
 
     case Mode::RESETTOARMEDYAW:
         // changes yaw to be same as when quad was armed
-        return copter.initial_armed_bearing;
+        _yaw_angle_cd = copter.initial_armed_bearing;
+        break;
 
     case Mode::CIRCLE:
 #if MODE_CIRCLE_ENABLED
         if (copter.circle_nav->is_active()) {
-            return copter.circle_nav->get_yaw();
+            _yaw_angle_cd = copter.circle_nav->get_yaw();
         }
 #endif
-        // return the current attitude target
-        return wrap_360_cd(copter.attitude_control->get_att_target_euler_cd().z);
+        break;
 
     case Mode::ANGLE_RATE:{
         const uint32_t now_ms = millis();
         float dt = (now_ms - _last_update_ms) * 0.001;
         _last_update_ms = now_ms;
         _yaw_angle_cd += _yaw_rate_cds * dt;
-        return _yaw_angle_cd;
+        break;
     }
 
+    case Mode::RATE:
+    case Mode::WEATHERVANE:
+    case Mode::PILOT_RATE:
+        _yaw_angle_cd = copter.attitude_control->get_att_target_euler_cd().z;
+        break;
+
     case Mode::LOOK_AT_NEXT_WP:
     default:
         // point towards next waypoint.
         // we don't use wp_bearing because we don't want the copter to turn too much during flight
-        return copter.pos_control->get_yaw_cd();
+        _yaw_angle_cd = copter.pos_control->get_yaw_cd();
+    break;
     }
+    
+    return _yaw_angle_cd;
 }
 
 // returns yaw rate normally set by SET_POSITION_TARGET mavlink
 // messages (positive is clockwise, negative is counter clockwise)
-float Mode::AutoYaw::rate_cds() const
+float Mode::AutoYaw::rate_cds()
 {
     switch (_mode) {
 
@@ -277,22 +285,25 @@ float Mode::AutoYaw::rate_cds() const
     case Mode::LOOK_AHEAD:
     case Mode::RESETTOARMEDYAW:
     case Mode::CIRCLE:
-        return 0.0f;
+        _yaw_rate_cds = 0.0f;
+        break;
+
+    case Mode::LOOK_AT_NEXT_WP:
+        _yaw_rate_cds = copter.pos_control->get_yaw_rate_cds();
+        break;
+
+    case Mode::PILOT_RATE:
+        _yaw_rate_cds = _pilot_yaw_rate_cds;
+        break;
 
     case Mode::ANGLE_RATE:
     case Mode::RATE:
     case Mode::WEATHERVANE:
-        return _yaw_rate_cds;
-
-    case Mode::LOOK_AT_NEXT_WP:
-        return copter.pos_control->get_yaw_rate_cds();
-
-    case Mode::PILOT_RATE:
-        return _pilot_yaw_rate_cds;
+        break;
     }
 
     // return zero turn rate (this should never happen)
-    return 0.0f;
+    return _yaw_rate_cds;
 }
 
 AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading()
@@ -315,7 +326,7 @@ AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading()
 #endif
 
     AC_AttitudeControl::HeadingCommand heading;
-    heading.yaw_angle_cd = yaw();
+    heading.yaw_angle_cd = auto_yaw.yaw_cd();
     heading.yaw_rate_cds = auto_yaw.rate_cds();
 
     switch (auto_yaw.mode()) {
diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index 6a28a92195..52ed637980 100644
--- a/ArduCopter/mode.h
+++ b/ArduCopter/mode.h
@@ -257,7 +257,6 @@ public:
         void set_mode(Mode new_mode);
         Mode default_mode(bool rtl) const;
 
-
         void set_rate(float new_rate_cds);
 
         // set_roi(...): set a "look at" location:
@@ -280,11 +279,11 @@ public:
 
     private:
 
-        // yaw(): main product of AutoYaw; the heading:
-        float yaw();
+        // yaw_cd(): main product of AutoYaw; the heading:
+        float yaw_cd();
 
         // rate_cds(): desired yaw rate in centidegrees/second:
-        float rate_cds() const;
+        float rate_cds();
 
         float look_ahead_yaw();
         float roi_yaw() const;