Rover pivot turn fix

APMrover2/Rover.h

@@ -512,6 +512,7 @@ class Rover : public AP_HAL::HAL::Callbacks {
     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);
 

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
 */

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 {