From 957458ca56517c38886e6337126f067efd9cf191 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 6 May 2018 10:47:59 +0900 Subject: [PATCH] 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 | 38 ++++++++++++++++++++++++++++++++++++-- APMrover2/mode.h | 12 +++++++++++- 2 files changed, 47 insertions(+), 3 deletions(-) diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index 58d412335f..b5c35daebe 100644 --- a/APMrover2/mode.cpp +++ b/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) { diff --git a/APMrover2/mode.h b/APMrover2/mode.h index a72bbff043..b71b9e049b 100644 --- a/APMrover2/mode.h +++ b/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;