diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2013-11-26 10:28:30 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2013-11-26 10:28:30 +0400 |
| commit | d8e95de9bf3249290524ae21204e87e2b75a3bd9 (patch) | |
| tree | c735dfe6dd6ab22155a1250b8c463a3d696883b2 /src/modules/systemlib | |
| parent | 310de1df963143da40ff6551d5823d40df282af4 (diff) | |
| download | px4-firmware-d8e95de9bf3249290524ae21204e87e2b75a3bd9.tar.gz px4-firmware-d8e95de9bf3249290524ae21204e87e2b75a3bd9.tar.bz2 px4-firmware-d8e95de9bf3249290524ae21204e87e2b75a3bd9.zip | |
pid lib fixes
Diffstat (limited to 'src/modules/systemlib')
| -rw-r--r-- | src/modules/systemlib/pid/pid.c | 26 |
1 files changed, 8 insertions, 18 deletions
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index cab33282c..0bec72495 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -143,28 +143,18 @@ __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); - if (pid->ki > SIGMA) { - /* calculate error integral and check for saturation */ - float i = pid->integral + (error * dt); - - /* fail-safe */ - if (!isfinite(i)) { - i = 0.0f; - } - - if ((pid->output_limit > SIGMA && (fabsf(output + (i * pid->ki)) > pid->output_limit)) || - fabsf(i) > pid->integral_limit) { - /* saturated, do not update integral value */ - i = pid->integral; - - } else { + /* 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 += i * pid->ki; } + /* add I component to output */ + output += pid->integral * pid-> ki; + /* limit output */ if (isfinite(output)) { if (pid->output_limit > SIGMA) { |