From 75ba96b7a24d5b6797189e921a682d05f9f0cede Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 4 May 2019 12:09:24 +0900 Subject: [PATCH] Rover: separate nudge from calc_throttle 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 --- APMrover2/mode.cpp | 27 +++++++++++---------------- APMrover2/mode.h | 9 ++++----- APMrover2/mode_acro.cpp | 2 +- APMrover2/mode_auto.cpp | 6 +++--- APMrover2/mode_follow.cpp | 2 +- APMrover2/mode_guided.cpp | 4 ++-- APMrover2/mode_loiter.cpp | 2 +- APMrover2/mode_simple.cpp | 2 +- APMrover2/mode_steering.cpp | 2 +- 9 files changed, 25 insertions(+), 31 deletions(-) diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index 8a53d1f3f8..6a92b97f68 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -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); diff --git a/APMrover2/mode.h b/APMrover2/mode.h index 290f69f8f3..d716b1900b 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -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; } diff --git a/APMrover2/mode_acro.cpp b/APMrover2/mode_acro.cpp index 268b7caf2a..30d15d9ae1 100644 --- a/APMrover2/mode_acro.cpp +++ b/APMrover2/mode_acro.cpp @@ -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; diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp index 30b0ab369c..ec2b7476ad 100644 --- a/APMrover2/mode_auto.cpp +++ b/APMrover2/mode_auto.cpp @@ -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 diff --git a/APMrover2/mode_follow.cpp b/APMrover2/mode_follow.cpp index b270c40602..018b440a19 100644 --- a/APMrover2/mode_follow.cpp +++ b/APMrover2/mode_follow.cpp @@ -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) diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp index 9f33364074..d23e1bb429 100644 --- a/APMrover2/mode_guided.cpp +++ b/APMrover2/mode_guided.cpp @@ -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()) { diff --git a/APMrover2/mode_loiter.cpp b/APMrover2/mode_loiter.cpp index b4eefe3748..3685d98080 100644 --- a/APMrover2/mode_loiter.cpp +++ b/APMrover2/mode_loiter.cpp @@ -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); } diff --git a/APMrover2/mode_simple.cpp b/APMrover2/mode_simple.cpp index 75156ad11c..c91313134b 100644 --- a/APMrover2/mode_simple.cpp +++ b/APMrover2/mode_simple.cpp @@ -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); } diff --git a/APMrover2/mode_steering.cpp b/APMrover2/mode_steering.cpp index f7fd78651f..a4d4dbf4bb 100644 --- a/APMrover2/mode_steering.cpp +++ b/APMrover2/mode_steering.cpp @@ -49,5 +49,5 @@ void ModeSteering::update() } // run speed to throttle controller - calc_throttle(desired_speed, false, true); + calc_throttle(desired_speed, true); }