Browse Source

AC_PID: revert AP_Math class change

master
Andrew Tridgell 10 years ago
parent
commit
33555b7f12
  1. 2
      libraries/AC_PID/AC_HELI_PID.cpp
  2. 4
      libraries/AC_PID/AC_PID.cpp
  3. 6
      libraries/AC_PID/AC_PI_2D.cpp

2
libraries/AC_PID/AC_HELI_PID.cpp

@ -56,7 +56,7 @@ float AC_HELI_PID::get_ff(float requested_rate) const
float AC_HELI_PID::get_leaky_i(float leak_rate) float AC_HELI_PID::get_leaky_i(float leak_rate)
{ {
if(!AP_Math::is_zero(_ki) && !AP_Math::is_zero(_dt)){ if(!is_zero(_ki) && !is_zero(_dt)){
_integrator -= (float)_integrator * leak_rate; _integrator -= (float)_integrator * leak_rate;
_integrator += ((float)_input * _ki) * _dt; _integrator += ((float)_input * _ki) * _dt;
if (_integrator < -_imax) { if (_integrator < -_imax) {

4
libraries/AC_PID/AC_PID.cpp

@ -132,7 +132,7 @@ float AC_PID::get_p() const
float AC_PID::get_i() float AC_PID::get_i()
{ {
if(!AP_Math::is_zero(_ki) && !AP_Math::is_zero(_dt)) { if(!is_zero(_ki) && !is_zero(_dt)) {
_integrator += ((float)_input * _ki) * _dt; _integrator += ((float)_input * _ki) * _dt;
if (_integrator < -_imax) { if (_integrator < -_imax) {
_integrator = -_imax; _integrator = -_imax;
@ -199,7 +199,7 @@ 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 (AP_Math::is_zero(_filt_hz)) { if (is_zero(_filt_hz)) {
return 1.0f; return 1.0f;
} }

6
libraries/AC_PID/AC_PI_2D.cpp

@ -95,7 +95,7 @@ Vector2f AC_PI_2D::get_p() const
Vector2f AC_PI_2D::get_i() Vector2f AC_PI_2D::get_i()
{ {
if(!AP_Math::is_zero(_ki) && !AP_Math::is_zero(_dt)) { if(!is_zero(_ki) && !is_zero(_dt)) {
_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)) {
@ -109,7 +109,7 @@ Vector2f AC_PI_2D::get_i()
// 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 (!AP_Math::is_zero(_ki) && !AP_Math::is_zero(_dt)) { if (!is_zero(_ki) && !is_zero(_dt)) {
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();
@ -167,7 +167,7 @@ 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 (AP_Math::is_zero(_filt_hz)) { if (is_zero(_filt_hz)) {
_filt_alpha = 1.0f; _filt_alpha = 1.0f;
return; return;
} }

Loading…
Cancel
Save