diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp
index 3e433e95f9..c9ba5f0c58 100644
--- a/APMrover2/mode.cpp
+++ b/APMrover2/mode.cpp
@@ -354,8 +354,7 @@ void Mode::calc_steering_from_lateral_acceleration(float lat_accel, bool reverse
 void Mode::calc_steering_to_heading(float desired_heading_cd, bool reversed)
 {
     // calculate yaw error (in radians) and pass to steering angle controller
-    const float yaw_error = wrap_PI(radians((desired_heading_cd - ahrs.yaw_sensor) * 0.01f));
-    const float steering_out = attitude_control.get_steering_out_angle_error(yaw_error, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed);
+    const float steering_out = attitude_control.get_steering_out_heading(radians(desired_heading_cd*0.01f), g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed);
     g2.motors.set_steering(steering_out * 4500.0f);
 }