From 1eb2a3009dc3bf2ed8b54ecdec09771490cfe1aa Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 23 Mar 2024 11:41:51 +0900
Subject: [PATCH] AC_PosControl: always update yaw targets

---
 libraries/AC_AttitudeControl/AC_PosControl.cpp | 9 ++++++---
 libraries/AC_AttitudeControl/AC_PosControl.h   | 2 +-
 2 files changed, 7 insertions(+), 4 deletions(-)

diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
index 0f98cd4a06e96..79e447e471bb0 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.cpp
+++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -1231,7 +1231,7 @@ void AC_PosControl::lean_angles_to_accel_xy(float& accel_x_cmss, float& accel_y_
 }
 
 // calculate_yaw_and_rate_yaw - update the calculated the vehicle yaw and rate of yaw.
-bool AC_PosControl::calculate_yaw_and_rate_yaw()
+void AC_PosControl::calculate_yaw_and_rate_yaw()
 {
     // Calculate the turn rate
     float turn_rate = 0.0f;
@@ -1250,9 +1250,12 @@ bool AC_PosControl::calculate_yaw_and_rate_yaw()
     if (vel_desired_xy_len > _vel_max_xy_cms * 0.05f) {
         _yaw_target = degrees(_vel_desired.xy().angle()) * 100.0f;
         _yaw_rate_target = turn_rate * degrees(100.0f);
-        return true;
+        return;
     }
-    return false;
+
+    // use the current attitude controller yaw target
+    _yaw_target = _attitude_control.get_att_target_euler_cd().z;
+    _yaw_rate_target = 0;
 }
 
 // calculate_overspeed_gain - calculated increased maximum acceleration and jerk if over speed condition is detected
diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h
index 34cad1bfdf8b6..3463da0306b8a 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.h
+++ b/libraries/AC_AttitudeControl/AC_PosControl.h
@@ -417,7 +417,7 @@ class AC_PosControl
     void lean_angles_to_accel_xy(float& accel_x_cmss, float& accel_y_cmss) const;
 
     // calculate_yaw_and_rate_yaw - calculate the vehicle yaw and rate of yaw.
-    bool calculate_yaw_and_rate_yaw();
+    void calculate_yaw_and_rate_yaw();
 
     // calculate_overspeed_gain - calculated increased maximum acceleration and jerk if over speed condition is detected
     float calculate_overspeed_gain();