diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index b2b022492f..8d873aff5e 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -520,6 +520,7 @@ private:
     void update_sensor_status_flags(void);
 
     // Steering.cpp
+    bool use_pivot_steering_at_next_WP(float yaw_error_cd);
     bool use_pivot_steering(float yaw_error_cd);
     void set_servos(void);
 
diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp
index 58af6cae27..8ba56b7a50 100644
--- a/APMrover2/Steering.cpp
+++ b/APMrover2/Steering.cpp
@@ -1,5 +1,23 @@
 #include "Rover.h"
 
+/*
+    work out if we are going to use pivot steering at next waypoint
+*/
+bool Rover::use_pivot_steering_at_next_WP(float yaw_error_cd)
+{
+    // check cases where we clearly cannot use pivot steering
+    if (!g2.motors.have_skid_steering() || g.pivot_turn_angle <= 0) {
+        return false;
+    }
+
+    // if error is larger than pivot_turn_angle then use pivot steering at next WP
+    if (fabsf(yaw_error_cd) * 0.01f > g.pivot_turn_angle) {
+        return true;
+    }
+
+    return false;
+}
+
 /*
     work out if we are going to use pivot steering
 */
diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp
index 2c4549e90f..fbd707aef9 100644
--- a/APMrover2/mode.cpp
+++ b/APMrover2/mode.cpp
@@ -162,7 +162,7 @@ void Mode::set_desired_location(const struct Location& destination, float next_l
         if (is_zero(turn_angle_cd)) {
             // if not turning can continue at full speed
             _desired_speed_final = _desired_speed;
-        } else if (rover.use_pivot_steering(turn_angle_cd)) {
+        } else if (rover.use_pivot_steering_at_next_WP(turn_angle_cd)) {
             // pivoting so we will stop
             _desired_speed_final = 0.0f;
         } else {