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

AC_PID: add const and use is_positive in AC_PI_2D

This commit is contained in:
Randy Mackay 2017-11-20 14:35:34 +09:00
parent fd964a21d6
commit c1708db208
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_PI_2D.cpp
View File

@ -93,10 +93,10 @@ Vector2f AC_PI_2D::get_p() const
Vector2f AC_PI_2D::get_i()
{
if(!is_zero(_ki) && !is_zero(_dt)) {
if (!is_zero(_ki) && !is_zero(_dt)) {
_integrator += (_input * _ki) * _dt;
float integrator_length = _integrator.length();
if ((integrator_length > _imax) && (integrator_length > 0)) {
const float integrator_length = _integrator.length();
if ((integrator_length > _imax) && (is_positive(integrator_length))) {
_integrator *= (_imax / integrator_length);
}
return _integrator;
@ -108,10 +108,10 @@ Vector2f AC_PI_2D::get_i()
Vector2f AC_PI_2D::get_i_shrink()
{
if (!is_zero(_ki) && !is_zero(_dt)) {
float integrator_length_orig = MIN(_integrator.length(),_imax);
const float integrator_length_orig = MIN(_integrator.length(),_imax);
_integrator += (_input * _ki) * _dt;
float integrator_length_new = _integrator.length();
if ((integrator_length_new > integrator_length_orig) && (integrator_length_new > 0)) {
const float integrator_length_new = _integrator.length();
if ((integrator_length_new > integrator_length_orig) && is_positive(integrator_length_new)) {
_integrator *= (integrator_length_orig / integrator_length_new);
}
return _integrator;
@ -171,6 +171,6 @@ void AC_PI_2D::calc_filt_alpha()
}
// calculate alpha
float rc = 1/(M_2PI*_filt_hz);
const float rc = 1/(M_2PI*_filt_hz);
_filt_alpha = _dt / (_dt + rc);
}