AC_PosControl: always update yaw targets

mirkix · Mar 25, 2024 · 1eb2a30 · 1eb2a30
1 parent 6090138
commit 1eb2a30
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 4 deletions.
diff --git 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
@@ -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();