diff options
Diffstat (limited to 'src/modules/multirotor_att_control/multirotor_rate_control.c')
| -rw-r--r-- | src/modules/multirotor_att_control/multirotor_rate_control.c | 15 |
1 files changed, 8 insertions, 7 deletions
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index a0266e1b3..641d833f0 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -151,7 +151,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, + const float rates[], struct actuator_controls_s *actuators, const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug) { static uint64_t last_run = 0; @@ -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 */ @@ -207,11 +208,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); + rates[1], 0.0f, deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], rates_acc[0], deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); + rates[0], 0.0f, deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { |