Rover: separate nudge from calc_throttle

This is required because AR_WPNav produces an acceleration adjusted desired speed meaning in rare cases where the vehicle is moving in reverse at the time auto is engaged, the desired speed may be temporarily negative as the vehicle slows.  In these situations we do not want to allow the vehicle's speed to be nudged to a higher reverse speed if the pilot's throttle stick is all the way down

APMrover2/mode.cpp

@@ -248,13 +248,8 @@ void Mode::handle_tack_request()
     }
 }
 
-void Mode::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled)
+void Mode::calc_throttle(float target_speed, bool avoidance_enabled)
 {
-    // add in speed nudging
-    if (nudge_allowed) {
-        target_speed = calc_speed_nudge(target_speed, g.speed_cruise, g.throttle_cruise * 0.01f);
-    }
-
     // get acceleration limited target speed
     target_speed = attitude_control.get_desired_speed_accel_limited(target_speed, rover.G_Dt);
 
@@ -334,9 +329,8 @@ float Mode::calc_speed_max(float cruise_speed, float cruise_throttle) const
 
 // calculate pilot input to nudge speed up or down
 //  target_speed should be in meters/sec
-//  cruise_speed is vehicle's cruising speed, cruise_throttle is the throttle (from -1 to +1) that achieves the cruising speed
-//  return value is a new speed (in m/s) which up to the projected maximum speed based on the cruise speed and cruise throttle
-float Mode::calc_speed_nudge(float target_speed, float cruise_speed, float cruise_throttle)
+//  reversed should be true if the vehicle is intentionally backing up which allows the pilot to increase the backing up speed by pulling the throttle stick down
+float Mode::calc_speed_nudge(float target_speed, bool reversed)
 {
     // return immediately during RC/GCS failsafe
     if (rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
@@ -346,18 +340,18 @@ float Mode::calc_speed_nudge(float target_speed, float cruise_speed, float cruis
     // return immediately if pilot is not attempting to nudge speed
     // pilot can nudge up speed if throttle (in range -100 to +100) is above 50% of center in direction of travel
     const int16_t pilot_throttle = constrain_int16(rover.channel_throttle->get_control_in(), -100, 100);
-    if (((pilot_throttle <= 50) && (target_speed >= 0.0f)) ||
-        ((pilot_throttle >= -50) && (target_speed <= 0.0f))) {
+    if (((pilot_throttle <= 50) && !reversed) ||
+        ((pilot_throttle >= -50) && reversed)) {
         return target_speed;
     }
 
     // sanity checks
-    if (cruise_throttle > 1.0f || cruise_throttle < 0.05f) {
+    if (g.throttle_cruise > 100 || g.throttle_cruise < 5) {
         return target_speed;
     }
 
     // project vehicle's maximum speed
-    const float vehicle_speed_max = calc_speed_max(cruise_speed, cruise_throttle);
+    const float vehicle_speed_max = calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
 
     // return unadjusted target if already over vehicle's projected maximum speed
     if (fabsf(target_speed) >= vehicle_speed_max) {
@@ -382,9 +376,10 @@ void Mode::navigate_to_waypoint()
     g2.wp_nav.update(rover.G_Dt);
     _distance_to_destination = g2.wp_nav.get_distance_to_destination();
 
-    // pass speed to throttle controller
-    const float desired_speed = g2.wp_nav.get_speed();
-    calc_throttle(desired_speed, true, true);
+    // pass speed to throttle controller after applying nudge from pilot
+    float desired_speed = g2.wp_nav.get_speed();
+    desired_speed = calc_speed_nudge(desired_speed, g2.wp_nav.get_reversed());
+    calc_throttle(desired_speed, true);
 
     float desired_heading_cd = g2.wp_nav.wp_bearing_cd();
     _yaw_error_cd = wrap_180_cd(desired_heading_cd - ahrs.yaw_sensor);

APMrover2/mode.h

@@ -168,7 +168,7 @@ protected:
 
     // calculates the amount of throttle that should be output based
     // on things like proximity to corners and current speed
-    virtual void calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled);
+    virtual void calc_throttle(float target_speed, bool avoidance_enabled);
 
     // performs a controlled stop. returns true once vehicle has stopped
     bool stop_vehicle();
@@ -179,9 +179,8 @@ protected:
 
     // calculate pilot input to nudge speed up or down
     //  target_speed should be in meters/sec
-    //  cruise_speed is vehicle's cruising speed, cruise_throttle is the throttle (from -1 to +1) that achieves the cruising speed
-    //  return value is a new speed (in m/s) which up to the projected maximum speed based on the cruise speed and cruise throttle
-    float calc_speed_nudge(float target_speed, float cruise_speed, float cruise_throttle);
+    //  reversed should be true if the vehicle is intentionally backing up which allows the pilot to increase the backing up speed by pulling the throttle stick down
+    float calc_speed_nudge(float target_speed, bool reversed);
 
     // calculate vehicle stopping location using current location, velocity and maximum acceleration
     void calc_stopping_location(Location& stopping_loc);
@@ -244,7 +243,7 @@ public:
 
     // methods that affect movement of the vehicle in this mode
     void update() override;
-    void calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled) override;
+    void calc_throttle(float target_speed, bool avoidance_enabled) override;
 
     // attributes of the mode
     bool is_autopilot_mode() const override { return true; }

APMrover2/mode_acro.cpp

@@ -15,7 +15,7 @@ void ModeAcro::update()
         float desired_speed;
         // convert pilot stick input into desired steering and speed
         get_pilot_desired_steering_and_speed(desired_steering, desired_speed);
-        calc_throttle(desired_speed, false, true);
+        calc_throttle(desired_speed, true);
     }
 
     float steering_out;

APMrover2/mode_auto.cpp

@@ -64,7 +64,7 @@ void ModeAuto::update()
             if (!_reached_heading) {
                 // run steering and throttle controllers
                 calc_steering_to_heading(_desired_yaw_cd);
-                calc_throttle(_desired_speed, true, true);
+                calc_throttle(calc_speed_nudge(_desired_speed, is_negative(_desired_speed)), true);
                 // check if we have reached within 5 degrees of target
                 _reached_heading = (fabsf(_desired_yaw_cd - ahrs.yaw_sensor) < 500);
             } else {
@@ -98,14 +98,14 @@ void ModeAuto::update()
     }
 }
 
-void ModeAuto::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled)
+void ModeAuto::calc_throttle(float target_speed, bool avoidance_enabled)
 {
     // If not autostarting set the throttle to minimum
     if (!check_trigger()) {
         stop_vehicle();
         return;
     }
-    Mode::calc_throttle(target_speed, nudge_allowed, avoidance_enabled);
+    Mode::calc_throttle(target_speed, avoidance_enabled);
 }
 
 // return distance (in meters) to destination

APMrover2/mode_follow.cpp

@@ -69,7 +69,7 @@ void ModeFollow::update()
 
     // run steering and throttle controllers
     calc_steering_to_heading(_desired_yaw_cd);
-    calc_throttle(desired_speed, false, true);
+    calc_throttle(desired_speed, true);
 }
 
 // return desired heading (in degrees) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)

APMrover2/mode_guided.cpp

@@ -57,7 +57,7 @@ void ModeGuided::update()
             if (have_attitude_target) {
                 // run steering and throttle controllers
                 calc_steering_to_heading(_desired_yaw_cd);
-                calc_throttle(_desired_speed, true, true);
+                calc_throttle(calc_speed_nudge(_desired_speed, is_negative(_desired_speed)), true);
             } else {
                 // we have reached the destination so stay here
                 if (rover.is_boat()) {
@@ -85,7 +85,7 @@ void ModeGuided::update()
                                                                             g2.motors.limit.steer_right,
                                                                             rover.G_Dt);
                 set_steering(steering_out * 4500.0f);
-                calc_throttle(_desired_speed, true, true);
+                calc_throttle(calc_speed_nudge(_desired_speed, is_negative(_desired_speed)), true);
             } else {
                 // we have reached the destination so stay here
                 if (rover.is_boat()) {

APMrover2/mode_loiter.cpp

@@ -52,5 +52,5 @@ void ModeLoiter::update()
 
     // run steering and throttle controllers
     calc_steering_to_heading(_desired_yaw_cd);
-    calc_throttle(_desired_speed, false, true);
+    calc_throttle(_desired_speed, true);
 }

APMrover2/mode_simple.cpp

@@ -29,5 +29,5 @@ void ModeSimple::update()
 
     // run throttle and steering controllers
     calc_steering_to_heading(desired_heading_cd);
-    calc_throttle(desired_speed, false, true);
+    calc_throttle(desired_speed, true);
 }

APMrover2/mode_steering.cpp

@@ -49,5 +49,5 @@ void ModeSteering::update()
     }
 
     // run speed to throttle controller
-    calc_throttle(desired_speed, false, true);
+    calc_throttle(desired_speed, true);
 }