fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 19:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

multirotor_pos_control: setpint reset rewritten

Browse Source
This commit is contained in:
Anton Babushkin
2013-09-10 22:58:44 +02:00
parent c3b6cea77a
commit 90873474a9
2 changed files with 85 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/systemlib/pid/pid.c
+16 -10
View File
@@ -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->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;
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;
}