diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
index 5ef859da55..856f48b9df 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
@@ -875,13 +875,20 @@ Vector3f AC_AttitudeControl::euler_accel_limit(const Vector3f &euler_rad, const
     return rot_accel;
 }
 
-// Shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centidegrees and is added to the current target heading
-void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd)
+// Sets yaw target to vehicle heading and sets yaw rate to zero
+void AC_AttitudeControl::set_yaw_target_to_current_heading()
 {
-    float yaw_shift = radians(yaw_shift_cd * 0.01f);
+    // move attitude target to current heading
+    float yaw_shift = _ahrs.yaw - _euler_angle_target.z;
     Quaternion _attitude_target_update;
     _attitude_target_update.from_axis_angle(Vector3f{0.0f, 0.0f, yaw_shift});
     _attitude_target = _attitude_target_update * _attitude_target;
+
+    // set yaw rate to zero
+    _euler_rate_target.z = 0.0f;
+
+    // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
+    euler_rate_to_ang_vel(_euler_angle_target, _euler_rate_target, _ang_vel_target);
 }
 
 // Shifts the target attitude to maintain the current error in the event of an EKF reset
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h
index c06f9d0924..bfbeb50d2c 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h
@@ -132,11 +132,8 @@ public:
     // Sets attitude target to vehicle attitude
     void set_attitude_target_to_current_attitude() { _ahrs.get_quat_body_to_ned(_attitude_target); }
 
-    // Sets yaw target to vehicle heading
-    void set_yaw_target_to_current_heading() { shift_ef_yaw_target(degrees(_ahrs.yaw - _euler_angle_target.z) * 100.0f); }
-
-    // Shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centidegrees and is added to the current target heading
-    void shift_ef_yaw_target(float yaw_shift_cd);
+    // Sets yaw target to vehicle heading and sets yaw rate to zero
+    void set_yaw_target_to_current_heading();
 
     // handle reset of attitude from EKF since the last iteration
     void inertial_frame_reset();