From 645edf1f57b3af794a30621460831fb143e79aff Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 3 Jun 2018 11:44:37 +0900 Subject: [PATCH] Rover: wp_overshoot used to limit speed in Auto Limits desired speed to max possible given max lateral acceleration and distance to line between waypoints Replaces the use of SPEED_TURN_GAIN which scaled down speed Also used in Guided, RTL, SmartRTL --- APMrover2/mode.cpp | 59 ++++++++++++++++++++++++---------------------- 1 file changed, 31 insertions(+), 28 deletions(-) diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index 2162b01287..0bdade9f2d 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -319,43 +319,46 @@ float Mode::calc_speed_nudge(float target_speed, float cruise_speed, float cruis // calculated a reduced speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint // should be called after calc_lateral_acceleration and before calc_throttle -// relies on these internal members being updated: lateral_acceleration, _yaw_error_cd, _distance_to_destination +// relies on these internal members being updated: _yaw_error_cd, _distance_to_destination float Mode::calc_reduced_speed_for_turn_or_distance(float desired_speed) { - // this method makes use the following internal variables - const float yaw_error_cd = _yaw_error_cd; - const float target_lateral_accel_G = attitude_control.get_desired_lat_accel(); - const float distance_to_waypoint = _distance_to_destination; - - // calculate the yaw_error_ratio which is the error (capped at 90degrees) expressed as a ratio (from 0 ~ 1) - float yaw_error_ratio = constrain_float(fabsf(yaw_error_cd / 9000.0f), 0.0f, 1.0f); - - // apply speed_turn_gain parameter (expressed as a percentage) to yaw_error_ratio - yaw_error_ratio *= (100 - g.speed_turn_gain) * 0.01f; - - // calculate absolute lateral acceleration expressed as a ratio (from 0 ~ 1) of the vehicle's maximum lateral acceleration - const float lateral_accel_ratio = constrain_float(fabsf(target_lateral_accel_G / (g.turn_max_g * GRAVITY_MSS)), 0.0f, 1.0f); - - // calculate a lateral acceleration based speed scaling - const float lateral_accel_speed_scaling = 1.0f - lateral_accel_ratio * yaw_error_ratio; - - // calculate a pivot steering based speed scaling (default to no reduction) - float pivot_speed_scaling = 1.0f; - if (rover.use_pivot_steering(yaw_error_cd)) { - pivot_speed_scaling = 0.0f; + // reduce speed to zero during pivot turns + if (rover.use_pivot_steering(_yaw_error_cd)) { + return 0.0f; } - // scaled speed - float speed_scaled = desired_speed * MIN(lateral_accel_speed_scaling, pivot_speed_scaling); + // reduce speed to limit overshoot from line between origin and destination + // calculate number of degrees vehicle must turn to face waypoint + const float heading_cd = is_negative(desired_speed) ? wrap_180_cd(ahrs.yaw_sensor + 18000) : ahrs.yaw_sensor; + const float wp_yaw_diff = wrap_180_cd(rover.nav_controller->target_bearing_cd() - heading_cd); + const float turn_angle_rad = fabsf(radians(wp_yaw_diff * 0.01f)); + + // calculate distance from vehicle to line + wp_overshoot + const float line_yaw_diff = wrap_180_cd(get_bearing_cd(_origin, _destination) - heading_cd); + const float crosstrack_error = rover.nav_controller->crosstrack_error(); + const float dist_from_line = fabsf(crosstrack_error); + const bool heading_away = is_positive(line_yaw_diff) == is_positive(crosstrack_error); + const float wp_overshoot_adj = heading_away ? -dist_from_line : dist_from_line; + + // calculate radius of circle that touches vehicle's current position and heading and target position and heading + float radius_m = 999.0f; + float radius_calc_denom = fabsf(1.0f - cosf(turn_angle_rad)); + if (!is_zero(radius_calc_denom)) { + radius_m = MAX(0.0f, rover.g.waypoint_overshoot + wp_overshoot_adj) / radius_calc_denom; + } + + // calculate and limit speed to allow vehicle to stay on circle + float overshoot_speed_max = safe_sqrt(g.turn_max_g * GRAVITY_MSS * MAX(g2.turn_radius, radius_m)); + float speed_max = constrain_float(desired_speed, -overshoot_speed_max, overshoot_speed_max); // limit speed based on distance to waypoint and max acceleration/deceleration - if (is_positive(distance_to_waypoint) && is_positive(attitude_control.get_decel_max())) { - const float speed_max = safe_sqrt(2.0f * distance_to_waypoint * attitude_control.get_decel_max() + sq(_desired_speed_final)); - speed_scaled = constrain_float(speed_scaled, -speed_max, speed_max); + if (is_positive(_distance_to_destination) && is_positive(attitude_control.get_decel_max())) { + const float dist_speed_max = safe_sqrt(2.0f * _distance_to_destination * attitude_control.get_decel_max() + sq(_desired_speed_final)); + speed_max = constrain_float(speed_max, -dist_speed_max, dist_speed_max); } // return minimum speed - return speed_scaled; + return speed_max; } // calculate the lateral acceleration target to cause the vehicle to drive along the path from origin to destination