Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AR_WPNav:allow other PSC limit-parameters can be updated

This commit is contained in:
xianglunkai 2022-10-21 14:11:12 +08:00
parent 725b7b30ae
commit 18200c88d0
1 changed files with 15 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AR_WPNav/AR_WPNav.cpp
View File

@ -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());