Rover: add get_pilot_desired_steering_and_speed

Also fix reversed steering response where throttle or acceleration limiting leads to throttle output being in the opposite direction from pilot's throttle input

APMrover2/mode.cpp

@@ -44,10 +44,10 @@ bool Mode::enter()
     return _enter();
 }
 
-// decode pilot steering held in channel_steer, channel_throttle and return in steer_out and throttle_out arguments
+// decode pilot steering and throttle inputs and return in steer_out and throttle_out arguments
 // steering_out is in the range -4500 ~ +4500 with positive numbers meaning rotate clockwise
 // throttle_out is in the range -100 ~ +100
-void Mode::get_pilot_desired_steering_and_throttle(float &steering_out, float &throttle_out)
+void Mode::get_pilot_input(float &steering_out, float &throttle_out)
 {
     // no RC input means no throttle and centered steering
     if (rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
@@ -90,6 +90,40 @@ void Mode::get_pilot_desired_steering_and_throttle(float &steering_out, float &t
     }
 }
 
+// decode pilot steering and throttle inputs and return in steer_out and throttle_out arguments
+// steering_out is in the range -4500 ~ +4500 with positive numbers meaning rotate clockwise
+// throttle_out is in the range -100 ~ +100
+void Mode::get_pilot_desired_steering_and_throttle(float &steering_out, float &throttle_out)
+{
+    // do basic conversion
+    get_pilot_input(steering_out, throttle_out);
+
+    // check for special case of input and output throttle being in opposite directions
+    float throttle_out_limited = g2.motors.get_slew_limited_throttle(throttle_out, rover.G_Dt);
+    if ((is_negative(throttle_out) != is_negative(throttle_out_limited)) &&
+        ((g.pilot_steer_type == PILOT_STEER_TYPE_DEFAULT) ||
+         (g.pilot_steer_type == PILOT_STEER_TYPE_DIR_REVERSED_WHEN_REVERSING))) {
+        steering_out *= -1;
+    }
+    throttle_out = throttle_out_limited;
+}
+
+// decode pilot steering and return steering_out and speed_out (in m/s)
+void Mode::get_pilot_desired_steering_and_speed(float &steering_out, float &speed_out)
+{
+    float desired_throttle;
+    get_pilot_input(steering_out, desired_throttle);
+    speed_out = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
+    // check for special case of input and output throttle being in opposite directions
+    float speed_out_limited = g2.attitude_control.get_desired_speed_accel_limited(speed_out);
+    if ((is_negative(speed_out) != is_negative(speed_out_limited)) &&
+        ((g.pilot_steer_type == PILOT_STEER_TYPE_DEFAULT) ||
+         (g.pilot_steer_type == PILOT_STEER_TYPE_DIR_REVERSED_WHEN_REVERSING))) {
+        steering_out *= -1;
+    }
+    speed_out = speed_out_limited;
+}
+
 // set desired location
 void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
 {

APMrover2/mode.h

@@ -101,11 +101,14 @@ protected:
     // subclasses override this to perform any required cleanup when exiting the mode
     virtual void _exit() { return; }
 
-    // decode pilot steering held in channel_steer, channel_throttle and return in steer_out and throttle_out arguments
+    // decode pilot steering and throttle inputs and return in steer_out and throttle_out arguments
     // steering_out is in the range -4500 ~ +4500 with positive numbers meaning rotate clockwise
     // throttle_out is in the range -100 ~ +100
     void get_pilot_desired_steering_and_throttle(float &steering_out, float &throttle_out);
 
+    // decode pilot input steering and return steering_out and speed_out (in m/s)
+    void get_pilot_desired_steering_and_speed(float &steering_out, float &speed_out);
+
     // calculate steering output to drive along line from origin to destination waypoint
     void calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed = false);
 
@@ -140,6 +143,13 @@ protected:
     // calculate vehicle stopping location using current location, velocity and maximum acceleration
     void calc_stopping_location(Location& stopping_loc);
 
+protected:
+
+    // decode pilot steering and throttle inputs and return in steer_out and throttle_out arguments
+    // steering_out is in the range -4500 ~ +4500 with positive numbers meaning rotate clockwise
+    // throttle_out is in the range -100 ~ +100
+    void get_pilot_input(float &steering_out, float &throttle_out);
+
     // references to avoid code churn:
     class AP_AHRS &ahrs;
     class Parameters &g;