diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp
index 14f3c0eb95..d5c3bc5aca 100644
--- a/APMrover2/mode.cpp
+++ b/APMrover2/mode.cpp
@@ -364,3 +364,12 @@ void Mode::calc_steering_from_lateral_acceleration(bool reversed)
     float steering_out = attitude_control.get_steering_out_lat_accel(_desired_lat_accel, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed);
     g2.motors.set_steering(steering_out * 4500.0f);
 }
+
+// calculate steering output to drive towards desired heading
+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);
+    g2.motors.set_steering(steering_out * 4500.0f);
+}
diff --git a/APMrover2/mode.h b/APMrover2/mode.h
index 0711d7718d..838a6279a2 100644
--- a/APMrover2/mode.h
+++ b/APMrover2/mode.h
@@ -113,6 +113,8 @@ protected:
     // calculate steering angle given a desired lateral acceleration
     void calc_steering_from_lateral_acceleration(bool reversed = false);
 
+    // calculate steering output to drive towards desired heading
+    void calc_steering_to_heading(float desired_heading_cd, bool reversed = false);
 
     // calculates the amount of throttle that should be output based
     // on things like proximity to corners and current speed
diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp
index 0fad21266f..3d7f12f735 100644
--- a/APMrover2/mode_auto.cpp
+++ b/APMrover2/mode_auto.cpp
@@ -69,12 +69,10 @@ void ModeAuto::update()
         {
             if (!_reached_heading) {
                 // run steering and throttle controllers
-                const float yaw_error = wrap_PI(radians((_desired_yaw_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, _desired_speed < 0);
-                g2.motors.set_steering(steering_out * 4500.0f);
+                calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
                 calc_throttle(_desired_speed, true);
-                // check if we have reached target
-                _reached_heading = (fabsf(yaw_error) < radians(5));
+                // check if we have reached within 5 degrees of target
+                _reached_heading = (fabsf(_desired_yaw_cd - ahrs.yaw_sensor) < 500);
             } else {
                 stop_vehicle();
             }
diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp
index 0ca2cb596d..ee071d31f7 100644
--- a/APMrover2/mode_guided.cpp
+++ b/APMrover2/mode_guided.cpp
@@ -46,9 +46,7 @@ void ModeGuided::update()
             }
             if (have_attitude_target) {
                 // run steering and throttle controllers
-                const float yaw_error = wrap_PI(radians((_desired_yaw_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, _desired_speed < 0);
-                g2.motors.set_steering(steering_out * 4500.0f);
+                calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
                 calc_throttle(_desired_speed, true);
             } else {
                 stop_vehicle();