From a2fe846a5f0b8c3ec237e424aa17946e1c40f72c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 7 Dec 2020 13:47:59 +0900 Subject: [PATCH] AR_WPNav: apply_speed_min used for overshoot limits speed_min is applied to overshoot_speed_max only instead of des_speed_lim so that the target doesn't immediately jump to the minimum. also constify apply_speed_min --- libraries/AR_WPNav/AR_WPNav.cpp | 18 +++++++----------- libraries/AR_WPNav/AR_WPNav.h | 3 ++- 2 files changed, 9 insertions(+), 12 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 896e9a26fd..fbc5ce0fa2 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -421,12 +421,11 @@ void AR_WPNav::update_desired_speed(float dt) } // calculate and limit speed to allow vehicle to stay on circle - const float overshoot_speed_max = safe_sqrt(_turn_max_mss * MAX(_turn_radius, radius_m)); + // ensure limit does not force speed below minimum + float overshoot_speed_max = safe_sqrt(_turn_max_mss * MAX(_turn_radius, radius_m)); + apply_speed_min(overshoot_speed_max); des_speed_lim = constrain_float(des_speed_lim, -overshoot_speed_max, overshoot_speed_max); - // ensure speed does not fall below minimum - apply_speed_min(des_speed_lim); - // limit speed based on distance to waypoint and max acceleration/deceleration if (is_positive(_oa_distance_to_destination) && is_positive(_atc.get_decel_max())) { const float dist_speed_max = safe_sqrt(2.0f * _oa_distance_to_destination * _atc.get_decel_max() + sq(_desired_speed_final)); @@ -445,18 +444,15 @@ void AR_WPNav::set_turn_params(float turn_max_g, float turn_radius, bool pivot_p } // adjust speed to ensure it does not fall below value held in SPEED_MIN -void AR_WPNav::apply_speed_min(float &desired_speed) +// desired_speed should always be positive (or zero) +void AR_WPNav::apply_speed_min(float &desired_speed) const { - if (!is_positive(_speed_min)) { + if (!is_positive(_speed_min) || (_speed_min > _speed_max)) { return; } - float speed_min = MIN(_speed_min, _speed_max); - // ensure speed does not fall below minimum - if (fabsf(desired_speed) < speed_min) { - desired_speed = is_negative(desired_speed) ? -speed_min : speed_min; - } + desired_speed = MAX(desired_speed, _speed_min); } // calculate the crosstrack error (does not rely on L1 controller) diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index c3e652b408..61130de687 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -110,7 +110,8 @@ private: void update_pivot_active_flag(); // adjust speed to ensure it does not fall below value held in SPEED_MIN - void apply_speed_min(float &desired_speed); + // desired_speed should always be positive (or zero) + void apply_speed_min(float &desired_speed) const; // calculate the crosstrack error (does not rely on L1 controller) float calc_crosstrack_error(const Location& current_loc) const;