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

AC_PID: Create Leaky Integrator Function.

This commit is contained in:
Robert Lefebvre 2012-11-26 21:01:20 -05:00
parent ca23a7ba76
commit abd2a60036
2 changed files with 20 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AC_PID/AC_PID.cpp
View File

@ -33,6 +33,25 @@ int32_t AC_PID::get_i(int32_t error, float dt)
return 0;
}
// This is an integrator which tends to decay to zero naturally
// if the error is zero.
int32_t AC_PID::get_leaky_i(int32_t error, float dt, float leak_rate)
{
if((_ki != 0) && (dt != 0)){
_integrator -= (float)_integrator * leak_rate;
_integrator += ((float)error * _ki) * dt;
if (_integrator < -_imax) {
_integrator = -_imax;
} else if (_integrator > _imax) {
_integrator = _imax;
}
return _integrator;
}
return 0;
}
int32_t AC_PID::get_d(int32_t input, float dt)
{
if ((_kd != 0) && (dt != 0)) {

1
libraries/AC_PID/AC_PID.h
View File

@ -54,6 +54,7 @@ public:
int32_t get_p(int32_t error);
int32_t get_i(int32_t error, float dt);
int32_t get_d(int32_t error, float dt);
int32_t get_leaky_i(int32_t error, float dt, float leak_rate);
/// Reset the PID integrator