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