From 18200c88d03eb4ebc8600a537e2386820d4732d0 Mon Sep 17 00:00:00 2001 From: xianglunkai <1322099375@qq.com> Date: Fri, 21 Oct 2022 14:11:12 +0800 Subject: [PATCH] AR_WPNav:allow other PSC limit-parameters can be updated --- libraries/AR_WPNav/AR_WPNav.cpp | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 8372180efd..ea4ddb7f4f 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -605,8 +605,20 @@ void AR_WPNav::update_speed_max() { const float speed_max = MAX(_base_speed_max, _nudge_speed_max); - // ignore calls that do not change the speed - if (is_equal(speed_max, _pos_control.get_speed_max())) { + float atc_accel_max = MIN(_atc.get_accel_max(), _atc.get_decel_max()); + if (!is_positive(atc_accel_max)) { + // accel_max of zero means no limit so use maximum acceleration + atc_accel_max = AR_WPNAV_ACCEL_MAX; + } + const float accel_max = is_positive(_accel_max) ? MIN(_accel_max, atc_accel_max) : atc_accel_max; + const float jerk_max = is_positive(_jerk_max) ? _jerk_max : accel_max; + const float lat_accel_max = _atc.get_turn_lat_accel_max(); + + // ignore calls that do not change the position controller parameters + if (is_equal(speed_max, _pos_control.get_speed_max()) && + is_equal(accel_max, _pos_control.get_accel_max()) && + is_equal(jerk_max, _pos_control.get_jerk_max()) && + is_equal(lat_accel_max, _pos_control.get_lat_accel_max())) { return; } @@ -618,7 +630,7 @@ void AR_WPNav::update_speed_max() _last_speed_update_ms = now_ms; // update position controller max speed - _pos_control.set_limits(speed_max, _pos_control.get_accel_max(), _pos_control.get_lat_accel_max(), _pos_control.get_jerk_max()); + _pos_control.set_limits(speed_max, accel_max, lat_accel_max, jerk_max); // change track speed _scurve_this_leg.set_speed_max(_pos_control.get_speed_max(), _pos_control.get_speed_max(), _pos_control.get_speed_max());