forked from Archive/PX4-Autopilot
Att rate PID fix
This commit is contained in:
parent
c1049483a8
commit
3bfc4ed517
|
@ -179,6 +179,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
|||
|
||||
static bool initialized = false;
|
||||
|
||||
float diff_filter_factor = 1.0f;
|
||||
|
||||
/* initialize the pid controllers when the function is called for the first time */
|
||||
if (initialized == false) {
|
||||
|
@ -186,8 +187,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
|||
parameters_update(&h, &p);
|
||||
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(&roll_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, 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) {
|
||||
/* update parameters from storage */
|
||||
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(&roll_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, diff_filter_factor);
|
||||
}
|
||||
|
||||
/* reset integral if on ground */
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
Loading…
Reference in New Issue