diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-15 12:34:56 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-15 12:34:56 +0400 |
| commit | f5c24c6e71bc918ac1b92df88f4ba8f3cdecd57a (patch) | |
| tree | a4479ff03ab4bae6eb6d9f702beef44b8df66abc | |
| parent | faa3826de6c47c5516df7b4c01b70c96f9e9d9d4 (diff) | |
| download | px4-firmware-f5c24c6e71bc918ac1b92df88f4ba8f3cdecd57a.tar.gz px4-firmware-f5c24c6e71bc918ac1b92df88f4ba8f3cdecd57a.tar.bz2 px4-firmware-f5c24c6e71bc918ac1b92df88f4ba8f3cdecd57a.zip | |
pid library fix
| -rw-r--r-- | src/modules/systemlib/pid/pid.c | 23 |
1 files changed, 14 insertions, 9 deletions
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 0bec72495..6a4e9392a 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -143,17 +143,22 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo /* calculate PD output */ float output = (error * pid->kp) + (d * pid->kd); - /* check for saturation */ - if (isfinite(i)) { - if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) && - fabsf(i) <= pid->integral_limit) { - /* not saturated, use new integral value */ - pid->integral = i; + if (pid->ki > SIGMA) { + // Calculate the error integral and check for saturation + i = pid->integral + (error * dt); + + /* check for saturation */ + if (isfinite(i)) { + if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) && + fabsf(i) <= pid->integral_limit) { + /* not saturated, use new integral value */ + pid->integral = i; + } } - } - /* add I component to output */ - output += pid->integral * pid-> ki; + /* add I component to output */ + output += pid->integral * pid->ki; + } /* limit output */ if (isfinite(output)) { |