diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
index f9460ad468..5b81f94c20 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
@@ -353,8 +353,19 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_m(float
     float euler_pitch = radians(constrain_float(euler_pitch_cd * 0.01f, -90.0f, 90.0f));
     float body_roll   = radians(constrain_float(body_roll_cd   * 0.01f, -90.0f, 90.0f));
 
-    // new heading
-    _attitude_target_euler_angle.z = wrap_PI(_attitude_target_euler_angle.z + euler_yaw_rate * _dt);
+    // Compute attitude error
+    Quaternion attitude_vehicle_quat;
+    Quaternion error_quat;
+    attitude_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
+    error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
+    Vector3f att_error;
+    error_quat.to_axis_angle(att_error);
+
+    // limit yaw error
+    if (fabsf(att_error.z) < AC_ATTITUDE_THRUST_ERROR_ANGLE) {
+        // update heading
+        _attitude_target_euler_angle.z = wrap_PI(_attitude_target_euler_angle.z + euler_yaw_rate * _dt);
+    }
 
     // init attitude target to desired euler yaw and pitch with zero roll
     _attitude_target_quat.from_euler(0, euler_pitch, _attitude_target_euler_angle.z);
@@ -384,12 +395,7 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_m(float
     _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
 
     // Compute attitude error
-    Quaternion attitude_vehicle_quat;
-    _ahrs.get_quat_body_to_ned(attitude_vehicle_quat);
-
-    Quaternion error_quat;
     error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
-    Vector3f att_error;
     error_quat.to_axis_angle(att_error);
 
     // Compute the angular velocity target from the attitude error
@@ -408,9 +414,20 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(float
     const float cpitch = cosf(euler_pitch);
     const float spitch = fabsf(sinf(euler_pitch));
 
-    // new heading
-    float yaw_rate = euler_yaw_rate * spitch + body_roll * cpitch;
-    _attitude_target_euler_angle.z = wrap_PI(_attitude_target_euler_angle.z + yaw_rate * _dt);
+    // Compute attitude error
+    Quaternion attitude_vehicle_quat;
+    Quaternion error_quat;
+    attitude_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
+    error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
+    Vector3f att_error;
+    error_quat.to_axis_angle(att_error);
+
+    // limit yaw error
+    if (fabsf(att_error.z) < AC_ATTITUDE_THRUST_ERROR_ANGLE) {
+        // update heading
+        float yaw_rate = euler_yaw_rate * spitch + body_roll * cpitch;
+        _attitude_target_euler_angle.z = wrap_PI(_attitude_target_euler_angle.z + yaw_rate * _dt);
+    }
 
     // init attitude target to desired euler yaw and pitch with zero roll
     _attitude_target_quat.from_euler(0, euler_pitch, _attitude_target_euler_angle.z);
@@ -437,12 +454,7 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(float
     _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
 
     // Compute attitude error
-    Quaternion attitude_vehicle_quat;
-    attitude_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
-
-    Quaternion error_quat;
     error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
-    Vector3f att_error;
     error_quat.to_axis_angle(att_error);
 
     // Compute the angular velocity target from the attitude error
@@ -897,7 +909,7 @@ Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(const Vector3f
     }
     // todo: Add Angular Velocity Limit
 
-    // Compute the pitch angular velocity demand from the roll angle error
+    // Compute the pitch angular velocity demand from the pitch angle error
     if (_use_sqrt_controller) {
         rate_target_ang_vel.y = sqrt_controller(attitude_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt);
     } else {
@@ -905,7 +917,7 @@ Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(const Vector3f
     }
     // todo: Add Angular Velocity Limit
 
-    // Compute the yaw angular velocity demand from the roll angle error
+    // Compute the yaw angular velocity demand from the yaw angle error
     if (_use_sqrt_controller) {
         rate_target_ang_vel.z = sqrt_controller(attitude_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS), _dt);
     } else {