From 200a644cfea232027c9da95cc3c724d3907f4f71 Mon Sep 17 00:00:00 2001
From: jeff567 <jeffreylaut@gmail.com>
Date: Tue, 10 Jul 2018 17:40:51 -0400
Subject: [PATCH] Rover: fixes bug in rovers with pivot turning

---
 APMrover2/Rover.h      |  1 +
 APMrover2/Steering.cpp | 18 ++++++++++++++++++
 APMrover2/mode.cpp     |  2 +-
 3 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index 0abf94f8de..0ee03672d8 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -512,6 +512,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 d6fd83393a..0845ae314b 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 {