From a0ef7a1476e8632cbf8443d7ec564c4f217a5fb5 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 2 Dec 2021 20:22:29 +0900
Subject: [PATCH] Rover: stop_vehicle controls turn rate until stopped

---
 Rover/mode.cpp | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/Rover/mode.cpp b/Rover/mode.cpp
index 643fe1ac7e..12ef9570c6 100644
--- a/Rover/mode.cpp
+++ b/Rover/mode.cpp
@@ -331,7 +331,7 @@ void Mode::calc_throttle(float target_speed, bool avoidance_enabled)
     g2.motors.set_throttle(throttle_out);
 }
 
-// performs a controlled stop with steering centered
+// performs a controlled stop without turning
 bool Mode::stop_vehicle()
 {
     // call throttle controller and convert output to -100 to +100 range
@@ -353,8 +353,12 @@ bool Mode::stop_vehicle()
     // send to motor
     g2.motors.set_throttle(throttle_out);
 
-    // do not attempt to steer
-    g2.motors.set_steering(0.0f);
+    // do not turn while slowing down
+    float steering_out = 0.0;
+    if (!stopped) {
+        steering_out = attitude_control.get_steering_out_rate(0.0, g2.motors.limit.steer_left, g2.motors.limit.steer_right, rover.G_Dt);
+    }
+    g2.motors.set_steering(steering_out * 4500.0);
 
     // return true once stopped
     return stopped;