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

AC_PID: AC_PID_2D: let I term change direction but not grow when limited

This commit is contained in:
Leonard Hall 2022-01-28 06:08:05 +10:30 committed by Randy Mackay
parent fcf4b8ad58
commit 545a20473e
1 changed files with 9 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AC_PID/AC_PID_2D.cpp
View File

@ -129,17 +129,18 @@ Vector2f AC_PID_2D::update_all(const Vector3f &target, const Vector3f &measureme
// If the limit is set the integral is only allowed to reduce in the direction of the limit // If the limit is set the integral is only allowed to reduce in the direction of the limit
void AC_PID_2D::update_i(const Vector2f &limit) void AC_PID_2D::update_i(const Vector2f &limit)
{ {
Vector2f limit_direction = limit; _pid_info_x.limit = false;
_pid_info_y.limit = false;
Vector2f delta_integrator = (_error * _ki) * _dt; Vector2f delta_integrator = (_error * _ki) * _dt;
if (!is_zero(limit_direction.length_squared())) { float integrator_length = _integrator.length();
// zero delta_vel if it will increase the velocity error _integrator += delta_integrator;
limit_direction.normalize(); // do not let integrator increase in length if delta_integrator is in the direction of limit
if (is_positive(delta_integrator * limit)) { if (is_positive(delta_integrator * limit) && _integrator.limit_length(integrator_length)) {
delta_integrator.zero(); _pid_info_x.limit = true;
} _pid_info_y.limit = true;
} }
_integrator += delta_integrator;
_integrator.limit_length(_kimax); _integrator.limit_length(_kimax);
} }