diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-23 01:20:41 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-23 01:20:41 +0200 |
commit | de530d6ba1fcbcaf65fc78ac8cca3286fa52d624 (patch) | |
tree | b7de387487d99a24fc2913fa50475f66d8c8ce66 /apps/systemlib/pid | |
parent | 8b951ec417454353d61d19b3379e52b6da5dd6b6 (diff) | |
download | px4-firmware-de530d6ba1fcbcaf65fc78ac8cca3286fa52d624.tar.gz px4-firmware-de530d6ba1fcbcaf65fc78ac8cca3286fa52d624.tar.bz2 px4-firmware-de530d6ba1fcbcaf65fc78ac8cca3286fa52d624.zip |
General robustness improvements in PID struct, numerically close to bullet-proof, error reporting needs improvements still.
Diffstat (limited to 'apps/systemlib/pid')
-rw-r--r-- | apps/systemlib/pid/pid.c | 53 | ||||
-rw-r--r-- | apps/systemlib/pid/pid.h | 2 |
2 files changed, 40 insertions, 15 deletions
diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index fbf3edc33..61dd5757f 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -58,13 +58,36 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->error_previous = 0; pid->integral = 0; } -__EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax) +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax) { - pid->kp = kp; - pid->ki = ki; - pid->kd = kd; - pid->intmax = intmax; + int ret = 0; + + if (isfinite(kp)) { + pid->kp = kp; + } else { + ret = 1; + } + + if (isfinite(ki)) { + pid->ki = ki; + } else { + ret = 1; + } + + if (isfinite(kd)) { + pid->kd = kd; + } else { + ret = 1; + } + + if (isfinite(intmax)) { + pid->intmax = intmax; + } else { + ret = 1; + } + // pid->limit = limit; + return ret; } //void pid_set(PID_t *pid, float sp) @@ -133,19 +156,21 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = -val_dot; } else { - d = 0; + d = 0.0f; } - if (pid->kd == 0) { - d = 0; + if (pid->kd == 0.0f) { + d = 0.0f; } - if (pid->ki == 0) { + if (pid->ki == 0.0f) { i = 0; } - if (pid->kp == 0) { - p = 0; + float p; + + if (pid->kp == 0.0f) { + p = 0.0f; } else { p = error; } @@ -154,10 +179,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo pid->error_previous = error; } - float val = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); + float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); - if (isfinite(val)) { - last_output = val; + if (isfinite(output)) { + pid->last_output = output; } if (!isfinite(pid->integral)) { diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h index d4bbcaf31..098b2bef8 100644 --- a/apps/systemlib/pid/pid.h +++ b/apps/systemlib/pid/pid.h @@ -66,7 +66,7 @@ typedef struct { } PID_t; __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode); -__EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax); //void pid_set(PID_t *pid, float sp); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); |