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

AC_PID: calculate filt_alpha every time the filter is run

This commit is contained in:
Jonathan Challinger 2015-03-27 17:19:24 -07:00 committed by Randy Mackay
parent 55d8f440d4
commit 6ea91d012e
2 changed files with 9 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AC_PID/AC_PID.cpp
View File

@ -64,7 +64,6 @@ void AC_PID::set_dt(float dt)
{
// set dt and calculate the input filter alpha
_dt = dt;
calc_filt_alpha();
}
// filt_hz - set input filter hz
@ -74,9 +73,6 @@ void AC_PID::filt_hz(float hz)
// sanity check _filt_hz
_filt_hz = max(_filt_hz, AC_PID_FILT_HZ_MIN);
// calculate the input filter alpha
calc_filt_alpha();
}
// set_input_filter_all - set input to PID controller
@ -97,7 +93,7 @@ void AC_PID::set_input_filter_all(float input)
}
// update filter and calculate derivative
float input_filt_change = _filt_alpha * (input - _input);
float input_filt_change = get_filt_alpha() * (input - _input);
_input = _input + input_filt_change;
if (_dt > 0.0f) {
_derivative = input_filt_change / _dt;
@ -123,7 +119,7 @@ void AC_PID::set_input_filter_d(float input)
// update filter and calculate derivative
if (_dt > 0.0f) {
float derivative = (input - _input) / _dt;
_derivative = _derivative + _filt_alpha * (derivative-_derivative);
_derivative = _derivative + get_filt_alpha() * (derivative-_derivative);
}
_input = input;
@ -159,13 +155,11 @@ float AC_PID::get_pi()
return get_p() + get_i();
}
float AC_PID::get_pid()
{
return get_p() + get_i() + get_d();
}
void AC_PID::reset_I()
{
_integrator = 0;
@ -179,9 +173,6 @@ void AC_PID::load_gains()
_imax.load();
_imax = fabs(_imax);
_filt_hz.load();
// calculate the input filter alpha
calc_filt_alpha();
}
// save_gains - save gains to eeprom
@ -203,14 +194,16 @@ void AC_PID::operator() (float p, float i, float d, float imaxval, float input_f
_imax = fabs(imaxval);
_filt_hz = input_filt_hz;
_dt = dt;
// calculate the input filter alpha
calc_filt_alpha();
}
// calc_filt_alpha - recalculate the input filter alpha
void AC_PID::calc_filt_alpha()
float AC_PID::get_filt_alpha() const
{
if (_filt_hz == 0.0f) {
return 1.0f;
}
// calculate alpha
float rc = 1/(2*PI*_filt_hz);
_filt_alpha = _dt / (_dt + rc);
return _dt / (_dt + rc);
}

6
libraries/AC_PID/AC_PID.h
View File

@ -63,7 +63,7 @@ public:
float kD() const { return _kd.get(); }
float imax() const { return _imax.get(); }
float filt_hz() const { return _filt_hz.get(); }
float get_filt_alpha() const { return _filt_alpha; }
float get_filt_alpha() const;
// set accessors
void kP(const float v) { _kp.set(v); }
@ -80,9 +80,6 @@ public:
protected:
// calc_filt_alpha - recalculate the input filter alpha
void calc_filt_alpha();
// parameters
AP_Float _kp;
AP_Float _ki;
@ -100,7 +97,6 @@ protected:
float _integrator; // integrator value
float _input; // last input for derivative
float _derivative; // last derivative for low-pass filter
float _filt_alpha; // input filter alpha
};
#endif // __AC_PID_H__