Rover: use AR_AttitudeControl for throttle control

also direct throttle nudge replaced with speed nudge
calc_speed_max estimates vehicle's top speed based on cruise-speed and
cruise-throttle
steering mode now provides total target speed instead of using speed plug
throttle nudge
motor limits provided to attitude control to stop i-term buildup
uses negative desired speed instead of reversed flag
reporting to GCS uses new throttle controller
braking is simply enabled and allows a reverse motor output regardless of vehicle
speed

APMrover2/GCS_Mavlink.cpp

@@ -242,7 +242,7 @@ void Rover::send_pid_tuning(mavlink_channel_t chan)
         }
     }
     if (g.gcs_pid_mask & 2) {
-        pid_info = &g.pidSpeedThrottle.get_pid_info();
+        pid_info = &g2.attitude_control.get_throttle_speed_pid().get_pid_info();
         mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ,
                                     pid_info->desired,
                                     0,

APMrover2/Log.cpp

@@ -155,7 +155,7 @@ void Rover::Log_Write_Attitude()
 
     // log steering rate controller
     DataFlash.Log_Write_PID(LOG_PIDS_MSG, g2.attitude_control.get_steering_rate_pid().get_pid_info());
-    DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pidSpeedThrottle.get_pid_info());
+    DataFlash.Log_Write_PID(LOG_PIDA_MSG, g2.attitude_control.get_throttle_speed_pid().get_pid_info());
 }
 
 struct PACKED log_Rangefinder {

APMrover2/Parameters.cpp

@@ -123,24 +123,6 @@ const AP_Param::Info Rover::var_info[] = {
     // @User: Standard
     GSCALAR(speed_turn_gain,    "SPEED_TURN_GAIN",  50),
 
-    // @Param: BRAKING_PERCENT
-    // @DisplayName: Percentage braking to apply
-    // @Description: The maximum reverse throttle braking percentage to apply when cornering
-    // @Units: %
-    // @Range: 0 100
-    // @Increment: 1
-    // @User: Standard
-    GSCALAR(braking_percent,    "BRAKING_PERCENT",  0),
-
-    // @Param: BRAKING_SPEEDERR
-    // @DisplayName: Speed error at which to apply braking
-    // @Description: The amount of overspeed error at which to start applying braking
-    // @Units: m/s
-    // @Range: 0 100
-    // @Increment: 1
-    // @User: Standard
-    GSCALAR(braking_speederr,   "BRAKING_SPEEDERR",  3),
-
     // @Param: PIVOT_TURN_ANGLE
     // @DisplayName: Pivot turn angle
     // @Description: Navigation angle threshold in degrees to switch to pivot steering when SKID_STEER_OUT is 1. This allows you to setup a skid steering rover to turn on the spot in auto mode when the angle it needs to turn it greater than this angle. An angle of zero means to disable pivot turning. Note that you will probably also want to set a low value for WP_RADIUS to get neat turns.
@@ -341,10 +323,6 @@ const AP_Param::Info Rover::var_info[] = {
     // @User: Standard
     GSCALAR(turn_max_g,             "TURN_MAX_G",      2.0f),
 
-    // @Group: SPEED2THR_
-    // @Path: ../libraries/PID/PID.cpp
-    GGROUP(pidSpeedThrottle,        "SPEED2THR_", PID),
-
     // variables not in the g class which contain EEPROM saved variables
 
     // @Group: COMPASS_

APMrover2/Parameters.h

@@ -43,8 +43,8 @@ public:
         k_param_battery_curr_pin,
 
         // braking
-        k_param_braking_percent = 30,
-        k_param_braking_speederr,
+        k_param_braking_percent_old = 30,   // unused
+        k_param_braking_speederr_old,       // unused
 
         // misc2
         k_param_log_bitmask = 40,
@@ -180,7 +180,7 @@ public:
         // 240: PID Controllers
         k_param_pidNavSteer = 230,
         k_param_pidServoSteer,  // unused
-        k_param_pidSpeedThrottle,
+        k_param_pidSpeedThrottle_old,   // unused
 
         // high RC channels
         k_param_rc_9_old = 235,
@@ -215,10 +215,6 @@ public:
     AP_Int8     reset_switch_chan;
     AP_Int8     initial_mode;
 
-    // braking
-    AP_Int8     braking_percent;
-    AP_Float    braking_speederr;
-
     // Telemetry control
     //
     AP_Int16    sysid_this_mav;
@@ -276,15 +272,7 @@ public:
     //
     AP_Float    waypoint_radius;
 
-    // PID controllers
-    //
-    PID         pidSpeedThrottle;
-
-    Parameters() :
-        // PID controller    initial P        initial I        initial D        initial imax
-        //-----------------------------------------------------------------------------------
-        pidSpeedThrottle    (0.7f,            0.2f,            0.2f,            4000)
-        {}
+    Parameters() {}
 };
 
 /*

APMrover2/Rover.h

@@ -38,7 +38,6 @@
 #include <AP_Mission/AP_Mission.h>                  // Mission command library
 #include <AP_Rally/AP_Rally.h>
 #include <AP_Terrain/AP_Terrain.h>
-#include <PID/PID.h>                                // PID library
 #include <AC_PID/AC_P.h>
 #include <AC_PID/AC_PID.h>
 #include <RC_Channel/RC_Channel.h>                  // RC Channel Library

APMrover2/control_modes.cpp

@@ -76,9 +76,6 @@ void Rover::read_control_switch()
         }
 
         oldSwitchPosition = switchPosition;
-
-        // reset speed integrator
-        g.pidSpeedThrottle.reset_I();
     }
 
     switch_debouncer = false;

APMrover2/mode.cpp

@@ -17,7 +17,6 @@ void Mode::exit()
     _exit();
 
     lateral_acceleration = 0.0f;
-    rover.g.pidSpeedThrottle.reset_I();
 }
 
 bool Mode::enter()
@@ -50,56 +49,75 @@ void Mode::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed)
     _desired_speed = target_speed;
 }
 
-void Mode::calc_throttle(float target_speed, bool reversed)
+void Mode::calc_throttle(float target_speed, bool nudge_allowed)
 {
-    // get ground speed from vehicle
-    const float &groundspeed = rover.ground_speed;
+    // add in speed nudging
+    if (nudge_allowed) {
+        target_speed = calc_speed_nudge(target_speed, g.speed_cruise, g.throttle_cruise / 100.0f);
+    }
 
-    // calculate ground speed and ground speed error
-    _speed_error = fabsf(target_speed) - groundspeed;
+    // call throttle controller and convert output to -100 to +100 range
+    float throttle_out = 100.0f * attitude_control.get_throttle_out_speed(target_speed, g2.motors.have_skid_steering(), g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise / 100.0f);
 
-    const float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise;
-    const float throttle_target = throttle_base + calc_throttle_nudge();
+    // 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));
+    }
+}
 
-    float throttle = throttle_target + (g.pidSpeedThrottle.get_pid(_speed_error * 100.0f) / 100.0f);
+// estimate maximum vehicle speed (in m/s)
+float Mode::calc_speed_max(float cruise_speed, float cruise_throttle)
+{
+    float speed_max;
 
-    if (reversed) {
-        g2.motors.set_throttle(constrain_int16(-throttle, -g.throttle_max, -g.throttle_min));
+    // sanity checks
+    if (cruise_throttle > 1.0f || cruise_throttle < 0.05f) {
+        speed_max = cruise_speed;
     } else {
-        g2.motors.set_throttle(constrain_int16(throttle, g.throttle_min, g.throttle_max));
+        // project vehicle's maximum speed
+        speed_max = (1.0f / cruise_throttle) * cruise_speed;
     }
 
-    if (!reversed && g.braking_percent != 0 && _speed_error < -g.braking_speederr) {
-        // the user has asked to use reverse throttle to brake. Apply
-        // it in proportion to the ground speed error, but only when
-        // our ground speed error is more than BRAKING_SPEEDERR.
-        //
-        // We use a linear gain, with 0 gain at a ground speed error
-        // of braking_speederr, and 100% gain when groundspeed_error
-        // is 2*braking_speederr
-        const float brake_gain = constrain_float(((-_speed_error)-g.braking_speederr)/g.braking_speederr, 0.0f, 1.0f);
-        const int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
-        g2.motors.set_throttle(constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min));
-    }
+    // constrain to 30m/s (108km/h) and return
+    return constrain_float(speed_max, 0.0f, 30.0f);
 }
 
-
-// calculate pilot input to nudge throttle up or down
-int16_t Mode::calc_throttle_nudge()
+// 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)
 {
-    // get pilot throttle input (-100 to +100)
-    int16_t pilot_throttle = rover.channel_throttle->get_control_in();
-    int16_t throttle_nudge = 0;
-
-    // Check if the throttle value is above 50% and we need to nudge
-    // Make sure its above 50% in the direction we are travelling
-    if ((fabsf(pilot_throttle) > 50.0f) &&
-        (((pilot_throttle < 0) && rover.in_reverse) ||
-         ((pilot_throttle > 0) && !rover.in_reverse))) {
-        throttle_nudge = (rover.g.throttle_max - rover.g.throttle_cruise) * ((fabsf(rover.channel_throttle->norm_input()) - 0.5f) / 0.5f);
+    // 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 && is_positive(target_speed)) ||
+        (pilot_throttle >= -50 && is_negative(target_speed))) {
+        return target_speed;
+    }
+
+    // sanity checks
+    if (cruise_throttle > 1.0f || cruise_throttle < 0.05f) {
+        return target_speed;
+    }
+
+    // project vehicle's maximum speed
+    const float vehicle_speed_max = calc_speed_max(cruise_speed, cruise_throttle);
+
+    // return unadjusted target if already over vehicle's projected maximum speed
+    if (target_speed >= vehicle_speed_max) {
+        return target_speed;
+    }
+
+    const float speed_increase_max = vehicle_speed_max - fabsf(target_speed);
+    float speed_nudge = ((fabsf(pilot_throttle) - 50.0f) / 50.0f) * speed_increase_max;
+    if (pilot_throttle < 0) {
+        speed_nudge = -speed_nudge;
     }
 
-    return throttle_nudge;
+    return target_speed + speed_nudge;
 }
 
 // calculated a reduced speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint

APMrover2/mode.h

@@ -92,10 +92,16 @@ 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 reversed = false);
+    virtual void calc_throttle(float target_speed, bool nudge_allowed = true);
 
-    // calculate pilot input to nudge throttle up or down
-    int16_t calc_throttle_nudge();
+    // estimate maximum vehicle speed (in m/s)
+    float calc_speed_max(float cruise_speed, float cruise_throttle);
+
+    // 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);
 
     // calculated a reduced speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint
     // should be called after calc_lateral_acceleration and before calc_throttle
@@ -132,7 +138,7 @@ public:
 
     // methods that affect movement of the vehicle in this mode
     void update() override;
-    void calc_throttle(float target_speed, bool reversed = false);
+    void calc_throttle(float target_speed, bool nudge_allowed = true);
 
     // attributes of the mode
     bool is_autopilot_mode() const override { return true; }

APMrover2/mode_auto.cpp

@@ -9,8 +9,7 @@ bool ModeAuto::_enter()
         return false;
     }
 
-    // init controllers and location target
-    g.pidSpeedThrottle.reset_I();
+    // init location target
     set_desired_location(rover.current_loc, false);
 
     // other initialisation
@@ -52,7 +51,7 @@ void ModeAuto::update()
                 // continue driving towards destination
                 calc_lateral_acceleration(active_at_destination ? rover.current_loc : _origin, _destination, _reversed);
                 calc_nav_steer(_reversed);
-                calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), _reversed);
+                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());
@@ -69,7 +68,7 @@ void ModeAuto::update()
                 const float yaw_error = wrap_PI(radians((_desired_yaw_cd - ahrs.yaw_sensor) * 0.01f));
                 const float steering_out = attitude_control.get_steering_out_angle_error(yaw_error, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right);
                 g2.motors.set_steering(steering_out * 4500.0f);
-                calc_throttle(_desired_speed);
+                calc_throttle(_desired_speed, true);
                 // check if we have reached target
                 _reached_heading = (fabsf(yaw_error) < radians(5));
             } else {
@@ -175,7 +174,7 @@ bool ModeAuto::check_trigger(void)
     return false;
 }
 
-void ModeAuto::calc_throttle(float target_speed, bool reversed)
+void ModeAuto::calc_throttle(float target_speed, bool nudge_allowed)
 {
     // If not autostarting set the throttle to minimum
     if (!check_trigger()) {
@@ -186,5 +185,5 @@ void ModeAuto::calc_throttle(float target_speed, bool reversed)
         }
         return;
     }
-    Mode::calc_throttle(target_speed, reversed);
+    Mode::calc_throttle(target_speed, nudge_allowed);
 }

APMrover2/mode_guided.cpp

@@ -33,7 +33,7 @@ void ModeGuided::update()
                 // continue driving towards destination
                 calc_lateral_acceleration(_origin, _destination);
                 calc_nav_steer();
-                calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed));
+                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());
@@ -55,7 +55,7 @@ void ModeGuided::update()
                 const float yaw_error = wrap_PI(radians((_desired_yaw_cd - ahrs.yaw_sensor) * 0.01f));
                 const float steering_out = attitude_control.get_steering_out_angle_error(yaw_error, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right);
                 g2.motors.set_steering(steering_out * 4500.0f);
-                calc_throttle(_desired_speed);
+                calc_throttle(_desired_speed, true);
             } else {
                 g2.motors.set_throttle(g.throttle_min.get());
                 g2.motors.set_steering(0.0f);
@@ -74,7 +74,7 @@ void ModeGuided::update()
                 // run steering and throttle controllers
                 float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds / 100.0f), g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right);
                 g2.motors.set_steering(steering_out * 4500.0f);
-                calc_throttle(_desired_speed);
+                calc_throttle(_desired_speed, true);
             } else {
                 g2.motors.set_throttle(g.throttle_min.get());
                 g2.motors.set_steering(0.0f);

APMrover2/mode_rtl.cpp

@@ -32,7 +32,7 @@ void ModeRTL::update()
         // continue driving towards destination
         calc_lateral_acceleration(_origin, _destination);
         calc_nav_steer();
-        calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed));
+        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());

APMrover2/mode_steering.cpp

@@ -3,16 +3,23 @@
 
 void ModeSteering::update()
 {
-    // convert pilot throttle input to desired speed
-    // speed in proportion to cruise speed, up to 50% throttle, then uses nudging above that.
-    float target_speed = channel_throttle->get_control_in() * 0.01f * 2.0f * g.speed_cruise;
-    target_speed = constrain_float(target_speed, -g.speed_cruise, g.speed_cruise);
+    // convert pilot throttle input to desired speed (up to twice the cruise speed)
+    float target_speed = channel_throttle->get_control_in() * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise);
+
+    // get speed forward
+    float speed;
+    if (!attitude_control.get_forward_speed(speed)) {
+        // no valid speed so so stop
+        g2.motors.set_throttle(g.throttle_min.get());
+        g2.motors.set_steering(0.0f);
+        lateral_acceleration = 0.0f;
+        return;
+    }
 
     // in steering mode we control lateral acceleration directly. We first calculate the maximum lateral
     // acceleration at full steering lock for this speed. That is V^2/R where R is the radius of turn.
     // We get the radius of turn from half the STEER2SRV_P.
-    const float ground_speed = rover.ground_speed;
-    float max_g_force = ground_speed * ground_speed / MAX(g2.turn_radius, 0.1f);
+    float max_g_force = speed * speed / MAX(g2.turn_radius, 0.1f);
 
     // constrain to user set TURN_MAX_G
     max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);
@@ -25,7 +32,6 @@ void ModeSteering::update()
     if (is_negative(target_speed)) {
         reversed = true;
         lateral_acceleration = -lateral_acceleration;
-        target_speed = fabsf(target_speed);
     }
 
     // mark us as in_reverse when using a negative throttle
@@ -35,5 +41,5 @@ void ModeSteering::update()
     calc_nav_steer(reversed);
 
     // run speed to throttle output controller
-    calc_throttle(target_speed, reversed);
+    calc_throttle(target_speed, false);
 }