fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-08 12:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

multirotor_pos_control fixes, systemlib/pid now accepts limit = 0.0, means no limit

Browse Source
This commit is contained in:
Anton Babushkin
2013-07-12 21:59:49 +04:00
parent eb5af244b9
commit 8778230050
2 changed files with 103 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/systemlib/pid/pid.c
+8 -6
View File
@@ -168,8 +168,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 != 0.0f && (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 +186,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 != 0.0f) {
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;