diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-09-10 22:58:44 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-09-10 22:58:44 +0200 |
commit | 90873474a9c52b66696f31c12b35b25ab0f6abd6 (patch) | |
tree | 33a4dc8ee54dfb6d4c32eaf35023a90855929e82 /src/modules/systemlib/pid | |
parent | c3b6cea77a1fe59e58b0019d1f8e5b95daf55494 (diff) | |
download | px4-firmware-90873474a9c52b66696f31c12b35b25ab0f6abd6.tar.gz px4-firmware-90873474a9c52b66696f31c12b35b25ab0f6abd6.tar.bz2 px4-firmware-90873474a9c52b66696f31c12b35b25ab0f6abd6.zip |
multirotor_pos_control: setpint reset rewritten
Diffstat (limited to 'src/modules/systemlib/pid')
-rw-r--r-- | src/modules/systemlib/pid/pid.c | 28 |
1 files changed, 17 insertions, 11 deletions
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 739503ed4..77c952f52 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -167,20 +167,26 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = 0.0f; } - // Calculate the error integral and check for saturation - i = pid->integral + (error * dt); - - if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || - fabsf(i) > pid->intmax) { - i = pid->integral; // If saturated then do not update integral value - pid->saturated = 1; + if (pid->ki > 0.0f) { + // Calculate the error integral and check for saturation + i = pid->integral + (error * dt); + + if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || + fabsf(i) > pid->intmax) { + i = pid->integral; // If saturated then do not update integral value + pid->saturated = 1; + + } else { + if (!isfinite(i)) { + i = 0.0f; + } - } else { - if (!isfinite(i)) { - i = 0.0f; + pid->integral = i; + pid->saturated = 0; } - pid->integral = i; + } else { + i = 0.0f; pid->saturated = 0; } |