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 | 22 |
1 files changed, 6 insertions, 16 deletions
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index ee8c37580..0a336be47 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -58,9 +58,6 @@ #include <systemlib/param/param.h> #include <systemlib/err.h> #include <drivers/drv_hrt.h> -#include <uORB/uORB.h> - - PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); @@ -155,8 +152,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[], struct actuator_controls_s *actuators, - const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug) + const float rates[], struct actuator_controls_s *actuators, bool reset_integral) { static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; @@ -176,13 +172,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static struct mc_rate_control_params p; static struct mc_rate_control_param_handles h; - float pitch_control_last = 0.0f; - float roll_control_last = 0.0f; - 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) { parameters_init(&h); @@ -202,21 +193,20 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); } - /* reset integral if on ground */ - if (rate_sp->thrust < 0.01f) { + /* reset integrals if needed */ + if (reset_integral) { pid_reset_integral(&pitch_rate_controller); pid_reset_integral(&roll_rate_controller); + // TODO pid_reset_integral(&yaw_rate_controller); } /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT, - &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); + rates[1], 0.0f, deltaT); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT, - &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); + rates[0], 0.0f, deltaT); /* control yaw rate */ //XXX use library here float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); |