fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 02:53:34 +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:38:19 +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/multirotor_att_control/multirotor_rate_control.c
+5 -4
View File
@@ -179,6 +179,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
static bool initialized = false; static bool initialized = false;
float diff_filter_factor = 1.0f;
/* initialize the pid controllers when the function is called for the first time */ /* initialize the pid controllers when the function is called for the first time */
if (initialized == false) { if (initialized == false) {
@@ -186,8 +187,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
parameters_update(&h, &p); parameters_update(&h, &p);
initialized = true; initialized = true;
pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor, PID_MODE_DERIVATIV_CALC_NO_SP);
pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor, PID_MODE_DERIVATIV_CALC_NO_SP);
} }
@@ -195,8 +196,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
if (motor_skip_counter % 2500 == 0) { if (motor_skip_counter % 2500 == 0) {
/* update parameters from storage */ /* update parameters from storage */
parameters_update(&h, &p); parameters_update(&h, &p);
pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor);
pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor);
} }
/* reset integral if on ground */ /* reset integral if on ground */
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 // Calculated current error value
float error = pid->sp - val; float error = pid->sp - val;
float error_filtered;
float error_filtered = pid->diff_filter_factor*error + (1.0f-pid->diff_filter_factor)*pid->error_previous_filtered;
// Calculate or measured current error derivative // Calculate or measured current error derivative
if (pid->mode == PID_MODE_DERIVATIV_CALC) { if (pid->mode == PID_MODE_DERIVATIV_CALC) {
// d = (error_filtered - pid->error_previous_filtered) / dt; error_filtered = pid->error_previous_filtered + (error - pid->error_previous_filtered) * pid->diff_filter_factor;
d = error_filtered - pid->error_previous_filtered ; 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; pid->error_previous_filtered = error_filtered;
} else if (pid->mode == PID_MODE_DERIVATIV_SET) { } else if (pid->mode == PID_MODE_DERIVATIV_SET) {
d = -val_dot; 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 /* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error
* val_dot in pid_calculate() will be ignored */ * val_dot in pid_calculate() will be ignored */
#define PID_MODE_DERIVATIV_CALC 0 #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) */ /* 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) // Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID)
#define PID_MODE_DERIVATIV_NONE 9 #define PID_MODE_DERIVATIV_NONE 9