Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Randy Mackay 2018-05-06 10:47:59 +09:00
parent ef2223a712
commit 957458ca56
2 changed files with 47 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
APMrover2/mode.cpp
View File

@ -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)
{

12
APMrover2/mode.h
View File

@ -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;