fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-04 03:21:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

pid library fix

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-15 12:34:56 +04:00
parent faa3826de6
commit f5c24c6e71
1 changed files with 15 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/systemlib/pid/pid.c
+15 -10
View File
@@ -143,17 +143,22 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
/* calculate PD output */ /* calculate PD output */
float output = (error * pid->kp) + (d * pid->kd); float output = (error * pid->kp) + (d * pid->kd);
/* check for saturation */ if (pid->ki > SIGMA) {
if (isfinite(i)) { // Calculate the error integral and check for saturation
if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) && i = pid->integral + (error * dt);
fabsf(i) <= pid->integral_limit) {
/* not saturated, use new integral value */
pid->integral = i;
}
}
/* add I component to output */ /* check for saturation */
output += pid->integral * pid-> ki; 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;
}
/* limit output */ /* limit output */
if (isfinite(output)) { if (isfinite(output)) {