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

Att rate PID fix

Browse Source
This commit is contained in:
Anton Babushkin
2013-06-20 11:30:10 +04:00
parent c1049483a8
commit 3bfc4ed517
3 changed files with 17 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/systemlib/pid/pid.c
+8 -4
View File
@@ -144,15 +144,19 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
// Calculated current error value
float error = pid->sp - val;
float error_filtered = pid->diff_filter_factor*error + (1.0f-pid->diff_filter_factor)*pid->error_previous_filtered;
float error_filtered;
// Calculate or measured current error derivative
if (pid->mode == PID_MODE_DERIVATIV_CALC) {
// d = (error_filtered - pid->error_previous_filtered) / dt;
d = error_filtered - pid->error_previous_filtered ;
error_filtered = pid->error_previous_filtered + (error - pid->error_previous_filtered) * pid->diff_filter_factor;
d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt
pid->error_previous_filtered = error_filtered;
} else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) {
error_filtered = pid->error_previous_filtered + (val - pid->error_previous_filtered) * pid->diff_filter_factor;
d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt
pid->error_previous_filtered = error_filtered;
} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
d = -val_dot;