diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-02 08:05:09 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-02 08:05:09 +0200 |
| commit | 38f9c25e9332cf266d38aea521a2ab9e140ec506 (patch) | |
| tree | 263c238795f50616b1eb1ebd135fca860e6e1cbd | |
| parent | bda85d54e442bc49088fa617ff982f5a3f2dbe36 (diff) | |
| parent | e553087d10e7187c12e5e0f28937585a732a46da (diff) | |
| download | px4-firmware-38f9c25e9332cf266d38aea521a2ab9e140ec506.tar.gz px4-firmware-38f9c25e9332cf266d38aea521a2ab9e140ec506.tar.bz2 px4-firmware-38f9c25e9332cf266d38aea521a2ab9e140ec506.zip | |
Merge branch 'pid_fix' of github.com:PX4/Firmware
| -rw-r--r-- | src/modules/systemlib/pid/pid.c | 16 |
1 files changed, 10 insertions, 6 deletions
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 4996a8f66..739503ed4 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -51,6 +51,8 @@ #include "pid.h" #include <math.h> +#define SIGMA 0.000001f + __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min) { @@ -168,8 +170,8 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculate the error integral and check for saturation i = pid->integral + (error * dt); - if (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || - fabsf(i) > pid->intmax) { + 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; @@ -186,11 +188,13 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); if (isfinite(output)) { - if (output > pid->limit) { - output = pid->limit; + if (pid->limit > SIGMA) { + if (output > pid->limit) { + output = pid->limit; - } else if (output < -pid->limit) { - output = -pid->limit; + } else if (output < -pid->limit) { + output = -pid->limit; + } } pid->last_output = output; |