Rover: modes use stop_vehicle to stop gently

modes slow to a stop instead of immediately setting motors to zero
vehicle centers steering when stopping

APMrover2/mode.cpp

@@ -82,6 +82,27 @@ void Mode::calc_throttle(float target_speed, bool nudge_allowed)
     }
 }
 
+// performs a controlled stop with steering centered
+bool Mode::stop_vehicle()
+{
+    // call throttle controller and convert output to -100 to +100 range
+    bool stopped = false;
+    float throttle_out = 100.0f * attitude_control.get_throttle_out_stop(g2.motors.have_skid_steering(), g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise / 100.0f, stopped);
+
+    // apply limits on throttle
+    if (is_negative(throttle_out)) {
+        g2.motors.set_throttle(constrain_float(throttle_out, -g.throttle_max, -g.throttle_min));
+    } else {
+        g2.motors.set_throttle(constrain_float(throttle_out, g.throttle_min, g.throttle_max));
+    }
+
+    // do not attempt to steer
+    g2.motors.set_steering(0.0f);
+
+    // return true once stopped
+    return stopped;
+}
+
 // estimate maximum vehicle speed (in m/s)
 float Mode::calc_speed_max(float cruise_speed, float cruise_throttle)
 {

APMrover2/mode.h

@@ -98,6 +98,9 @@ protected:
     // on things like proximity to corners and current speed
     virtual void calc_throttle(float target_speed, bool nudge_allowed = true);
 
+    // performs a controlled stop. returns true once vehicle has stopped
+    bool stop_vehicle();
+
     // estimate maximum vehicle speed (in m/s)
     float calc_speed_max(float cruise_speed, float cruise_throttle);
 

APMrover2/mode_auto.cpp

@@ -54,8 +54,7 @@ void ModeAuto::update()
                 calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true);
             } else {
                 // we have reached the destination so stop
-                g2.motors.set_throttle(g.throttle_min.get());
+                stop_vehicle();
-                g2.motors.set_steering(0.0f);
                 lateral_acceleration = 0.0f;
             }
             break;
@@ -72,8 +71,7 @@ void ModeAuto::update()
                 // check if we have reached target
                 _reached_heading = (fabsf(yaw_error) < radians(5));
             } else {
-                g2.motors.set_throttle(g.throttle_min.get());
+                stop_vehicle();
-                g2.motors.set_steering(0.0f);
             }
             break;
         }
@@ -178,11 +176,7 @@ void ModeAuto::calc_throttle(float target_speed, bool nudge_allowed)
 {
     // If not autostarting set the throttle to minimum
     if (!check_trigger()) {
-        g2.motors.set_throttle(g.throttle_min.get());
+        stop_vehicle();
-        // Stop rotation in case of loitering and skid steering
-        if (g2.motors.have_skid_steering()) {
-            g2.motors.set_steering(0.0f);
-        }
         return;
     }
     Mode::calc_throttle(target_speed, nudge_allowed);

APMrover2/mode_guided.cpp

@@ -30,15 +30,12 @@ void ModeGuided::update()
                     _reached_destination = true;
                     rover.gcs().send_mission_item_reached_message(0);
                 }
-                // continue driving towards destination
+                // drive towards destination
                 calc_lateral_acceleration(_origin, _destination);
                 calc_nav_steer();
                 calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
             } else {
-                // we've reached destination so stop
+                stop_vehicle();
-                g2.motors.set_throttle(g.throttle_min.get());
-                g2.motors.set_steering(0.0f);
-                lateral_acceleration = 0.0f;
             }
             break;
         }
@@ -57,7 +54,7 @@ void ModeGuided::update()
                 g2.motors.set_steering(steering_out * 4500.0f);
                 calc_throttle(_desired_speed, true);
             } else {
-                g2.motors.set_throttle(g.throttle_min.get());
+                stop_vehicle();
                 g2.motors.set_steering(0.0f);
             }
             break;
@@ -76,7 +73,7 @@ void ModeGuided::update()
                 g2.motors.set_steering(steering_out * 4500.0f);
                 calc_throttle(_desired_speed, true);
             } else {
-                g2.motors.set_throttle(g.throttle_min.get());
+                stop_vehicle();
                 g2.motors.set_steering(0.0f);
             }
             break;

APMrover2/mode_rtl.cpp

@@ -35,8 +35,7 @@ void ModeRTL::update()
         calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
     } else {
         // we've reached destination so stop
-        g2.motors.set_throttle(g.throttle_min.get());
+        stop_vehicle();
-        g2.motors.set_steering(0.0f);
         lateral_acceleration = 0.0f;
     }
 }

APMrover2/mode_steering.cpp

@@ -9,7 +9,7 @@ void ModeSteering::update()
     // get speed forward
     float speed;
     if (!attitude_control.get_forward_speed(speed)) {
-        // no valid speed so so stop
+        // no valid speed so stop
         g2.motors.set_throttle(g.throttle_min.get());
         g2.motors.set_steering(0.0f);
         lateral_acceleration = 0.0f;
@@ -37,9 +37,12 @@ void ModeSteering::update()
     // mark us as in_reverse when using a negative throttle
     rover.set_reverse(reversed);
 
-    // run steering controller
-    calc_nav_steer(reversed);
-
     // run speed to throttle output controller
-    calc_throttle(target_speed, false);
+    if (is_zero(target_speed)) {
+        stop_vehicle();
+    } else {
+        // run steering controller
+        calc_nav_steer(reversed);
+        calc_throttle(target_speed, false);
+    }
 }