Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-11 09:32:57 +08:00
Code Issues Actions 32 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;
src/modules/systemlib/pid/pid.h
+4 -1
View File
@@ -47,8 +47,11 @@
/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error
* val_dot in pid_calculate() will be ignored */
#define PID_MODE_DERIVATIV_CALC 0
/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored
* val_dot in pid_calculate() will be ignored */
#define PID_MODE_DERIVATIV_CALC_NO_SP 1
/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
#define PID_MODE_DERIVATIV_SET 1
#define PID_MODE_DERIVATIV_SET 2
// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID)
#define PID_MODE_DERIVATIV_NONE 9