aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: separate nudge from calc_throttle

Browse Source 
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
This commit is contained in:
Randy Mackay 2019-05-04 12:09:24 +09:00
parent a94ebc5bc3
commit 75ba96b7a2
9 changed files with 25 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
APMrover2/mode.cpp
View File

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

9
APMrover2/mode.h
View File

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

2
APMrover2/mode_acro.cpp
View File

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

6
APMrover2/mode_auto.cpp
View File

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

2
APMrover2/mode_follow.cpp
View File

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

4
APMrover2/mode_guided.cpp
View File

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

2
APMrover2/mode_loiter.cpp
View File

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

2
APMrover2/mode_simple.cpp
View File

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

2
APMrover2/mode_steering.cpp
View File

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