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 2021-12-16 07:38:45 +10:30 committed by Randy Mackay
parent 90db81354b
commit b7801947af
1 changed files with 6 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_PID/AC_PID_2D.cpp
View File

@ -133,16 +133,14 @@ void AC_PID_2D::update_i(const Vector2f &limit)
_pid_info_y.limit = false; _pid_info_y.limit = false;
Vector2f delta_integrator = (_error * _ki) * _dt; Vector2f delta_integrator = (_error * _ki) * _dt;
if (!is_zero(limit.length_squared())) { float integrator_length = _integrator.length();
// zero delta_vel if it will increase the velocity error _integrator += delta_integrator;
if (is_positive(delta_integrator * limit)) { // do not let integrator increase in length if delta_integrator is in the direction of limit
delta_integrator.zero(); if (is_positive(delta_integrator * limit) && _integrator.limit_length(integrator_length)) {
_pid_info_x.limit = true; _pid_info_x.limit = true;
_pid_info_y.limit = true; _pid_info_y.limit = true;
} }
}
_integrator += delta_integrator;
_integrator.limit_length(_kimax); _integrator.limit_length(_kimax);
} }