Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PID_2D: correct limit caculation

Browse Source
This commit is contained in:
Peter Hall 2021-11-19 21:14:01 +00:00 committed by Randy Mackay
parent 6a13613a0f
commit 82dd216195
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AC_PID/AC_PID_2D.cpp
View File

@ -129,13 +129,16 @@ 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())) { if (!is_zero(limit.length_squared())) {
// zero delta_vel if it will increase the velocity error // zero delta_vel if it will increase the velocity error
limit_direction.normalize();
if (is_positive(delta_integrator * limit)) { if (is_positive(delta_integrator * limit)) {
delta_integrator.zero(); delta_integrator.zero();
_pid_info_x.limit = true;
_pid_info_y.limit = true;
} }
} }
@ -202,9 +205,6 @@ void AC_PID_2D::set_integrator(const Vector2f& error, const Vector2f& i)
void AC_PID_2D::set_integrator(const Vector2f& i) void AC_PID_2D::set_integrator(const Vector2f& i)
{ {
_integrator = i; _integrator = i;
const float integrator_length = _integrator.length(); _integrator.limit_length(_kimax);
if (integrator_length > _kimax) {
_integrator *= (_kimax / integrator_length);
}
} }