mirror of https://github.com/ArduPilot/ardupilot
I am Adding A "PI" loop to the mix. This is a need for AC2. We can optimize the internals later, but I basically duped the get_PID and removed the D term internals.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@2338 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
92545b1ffb
commit
7bfeab7a6d
|
@ -51,6 +51,33 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler)
|
|||
return output;
|
||||
}
|
||||
|
||||
|
||||
long
|
||||
PID::get_pi(int32_t error, uint16_t dt, float scaler)
|
||||
{
|
||||
float output = 0;
|
||||
float delta_time = (float)dt / 1000.0;
|
||||
|
||||
// Compute proportional component
|
||||
output += error * _kp;
|
||||
|
||||
// scale the P components
|
||||
output *= scaler;
|
||||
|
||||
// Compute integral component if time has elapsed
|
||||
if ((fabs(_ki) > 0) && (dt > 0)) {
|
||||
_integrator += (error * _ki) * scaler * delta_time;
|
||||
if (_integrator < -_imax) {
|
||||
_integrator = -_imax;
|
||||
} else if (_integrator > _imax) {
|
||||
_integrator = _imax;
|
||||
}
|
||||
output += _integrator;
|
||||
}
|
||||
|
||||
return output;
|
||||
}
|
||||
|
||||
void
|
||||
PID::reset_I()
|
||||
{
|
||||
|
|
|
@ -85,6 +85,9 @@ public:
|
|||
///
|
||||
long get_pid(int32_t error, uint16_t dt, float scaler = 1.0);
|
||||
|
||||
long get_pi(int32_t error, uint16_t dt, float scaler = 1.0);
|
||||
|
||||
|
||||
/// Reset the PID integrator
|
||||
///
|
||||
void reset_I();
|
||||
|
|
Loading…
Reference in New Issue