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: replace floating point '==' with is_equal

Browse Source 
Also add div-by-zero check to fix calc_filt_alpha

Also get_i and get_shrink_i do not need to set values to zero in Vector2f
constructor because this is already done.
This commit is contained in:
dgrat 2015-04-28 14:33:39 +09:00 committed by Randy Mackay
parent 0392292489
commit 303cfd683a
2 changed files with 13 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AC_PID/AC_PID.cpp
View File

@ -132,7 +132,7 @@ float AC_PID::get_p() const
float AC_PID::get_i() float AC_PID::get_i()
{ {
if((_ki != 0) && (_dt != 0)) { if(!is_equal(_ki, 0.0f) && !is_equal(_dt, 0.0f)) {
_integrator += ((float)_input * _ki) * _dt; _integrator += ((float)_input * _ki) * _dt;
if (_integrator < -_imax) { if (_integrator < -_imax) {
_integrator = -_imax; _integrator = -_imax;
@ -199,11 +199,11 @@ void AC_PID::operator() (float p, float i, float d, float imaxval, float input_f
// calc_filt_alpha - recalculate the input filter alpha // calc_filt_alpha - recalculate the input filter alpha
float AC_PID::get_filt_alpha() const float AC_PID::get_filt_alpha() const
{ {
if (_filt_hz == 0.0f) { if (is_equal(_filt_hz, 0.0f)) {
return 1.0f; return 1.0f;
} }
// calculate alpha // calculate alpha
float rc = 1/(2*PI*_filt_hz); float rc = 1/(M_2PI_F*_filt_hz);
return _dt / (_dt + rc); return _dt / (_dt + rc);
} }

15
libraries/AC_PID/AC_PI_2D.cpp
View File

@ -95,7 +95,7 @@ Vector2f AC_PI_2D::get_p() const
Vector2f AC_PI_2D::get_i() Vector2f AC_PI_2D::get_i()
{ {
if((_ki != 0.0f) && (_dt != 0.0f)) { if(!is_equal(_ki, 0.0f) && !is_equal(_dt, 0.0f)) {
_integrator += (_input * _ki) * _dt; _integrator += (_input * _ki) * _dt;
float integrator_length = _integrator.length(); float integrator_length = _integrator.length();
if ((integrator_length > _imax) && (integrator_length > 0)) { if ((integrator_length > _imax) && (integrator_length > 0)) {
@ -103,13 +103,13 @@ Vector2f AC_PI_2D::get_i()
} }
return _integrator; return _integrator;
} }
return Vector2f(0,0); return Vector2f();
} }
// get_i_shrink - get_i but do not allow integrator to grow in length (it may shrink) // get_i_shrink - get_i but do not allow integrator to grow in length (it may shrink)
Vector2f AC_PI_2D::get_i_shrink() Vector2f AC_PI_2D::get_i_shrink()
{ {
if((_ki != 0.0f) && (_dt != 0.0f)) { if (!is_equal(_ki, 0.0f) && !is_equal(_dt, 0.0f)) {
float integrator_length_orig = min(_integrator.length(),_imax); float integrator_length_orig = min(_integrator.length(),_imax);
_integrator += (_input * _ki) * _dt; _integrator += (_input * _ki) * _dt;
float integrator_length_new = _integrator.length(); float integrator_length_new = _integrator.length();
@ -118,7 +118,7 @@ Vector2f AC_PI_2D::get_i_shrink()
} }
return _integrator; return _integrator;
} }
return Vector2f(0,0); return Vector2f();
} }
Vector2f AC_PI_2D::get_pi() Vector2f AC_PI_2D::get_pi()
@ -167,7 +167,12 @@ void AC_PI_2D::operator() (float p, float i, float imaxval, float input_filt_hz,
// calc_filt_alpha - recalculate the input filter alpha // calc_filt_alpha - recalculate the input filter alpha
void AC_PI_2D::calc_filt_alpha() void AC_PI_2D::calc_filt_alpha()
{ {
if (is_equal(_filt_hz, 0.0f)) {
_filt_alpha = 1.0f;
return;
}
// calculate alpha // calculate alpha
float rc = 1/(2*PI*_filt_hz); float rc = 1/(M_2PI_F*_filt_hz);
_filt_alpha = _dt / (_dt + rc); _filt_alpha = _dt / (_dt + rc);
} }