From f346b00ee0b975fcea9d1d826f523192f498c0e8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 19 Feb 2018 13:08:42 +0100 Subject: [PATCH] mc_att_control rate controller: use a butterworth lowpass filter on the D-term The filter is disabled by default, thus the behavior is unchanged. --- .../mc_att_control/mc_att_control_main.cpp | 66 +++++++++++++++++-- .../mc_att_control/mc_att_control_params.c | 17 +++++ 2 files changed, 77 insertions(+), 6 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 8c95a3309d..a0b5e17a98 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -60,6 +60,7 @@ #include #include #include +#include #include #include #include @@ -167,11 +168,16 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _controller_latency_perf; - math::Vector<3> _rates_prev; /**< angular rates on previous step */ - math::Vector<3> _rates_sp_prev; /**< previous rates setpoint */ + math::LowPassFilter2p _lp_filters_d[3]; /**< low-pass filters for D-term (roll, pitch & yaw) */ + static constexpr const float initial_update_rate_hz = 250.f; /**< loop update rate used for initialization */ + float _loop_update_rate_hz; /**< current rate-controller loop update rate in [Hz] */ + + math::Vector<3> _rates_prev; /**< angular rates on previous step */ + math::Vector<3> _rates_prev_filtered; /**< angular rates on previous step (low-pass filtered) */ + math::Vector<3> _rates_sp_prev; /**< previous rates setpoint */ math::Vector<3> _rates_sp; /**< angular rates setpoint */ math::Vector<3> _rates_int; /**< angular rates integral error */ - float _thrust_sp; /**< thrust setpoint */ + float _thrust_sp; /**< thrust setpoint */ math::Vector<3> _att_control; /**< attitude control vector */ math::Matrix<3, 3> _I; /**< identity matrix */ @@ -191,6 +197,7 @@ private: param_t pitch_rate_integ_lim; param_t pitch_rate_d; param_t pitch_rate_ff; + param_t d_term_cutoff_freq; param_t tpa_breakpoint_p; param_t tpa_breakpoint_i; param_t tpa_breakpoint_d; @@ -240,6 +247,8 @@ private: math::Vector<3> rate_ff; /**< Feedforward gain for desired rates */ float yaw_ff; /**< yaw control feed-forward */ + float d_term_cutoff_freq; /**< Cutoff frequency for the D-term filter */ + float tpa_breakpoint_p; /**< Throttle PID Attenuation breakpoint */ float tpa_breakpoint_i; /**< Throttle PID Attenuation breakpoint */ float tpa_breakpoint_d; /**< Throttle PID Attenuation breakpoint */ @@ -368,7 +377,14 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _saturation_status{}, /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")), - _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")) + _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), + + _lp_filters_d{ + {initial_update_rate_hz, 50.f}, + {initial_update_rate_hz, 50.f}, + {initial_update_rate_hz, 50.f}}, // will be initialized correctly when params are loaded + +_loop_update_rate_hz(initial_update_rate_hz) { for (uint8_t i = 0; i < MAX_GYRO_COUNT; i++) { _sensor_gyro_sub[i] = -1; @@ -400,6 +416,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.board_offset[2] = 0.0f; _rates_prev.zero(); + _rates_prev_filtered.zero(); _rates_sp.zero(); _rates_sp_prev.zero(); _rates_int.zero(); @@ -423,6 +440,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); _params_handles.pitch_rate_ff = param_find("MC_PITCHRATE_FF"); + _params_handles.d_term_cutoff_freq = param_find("MC_DTERM_CUTOFF"); + _params_handles.tpa_breakpoint_p = param_find("MC_TPA_BREAK_P"); _params_handles.tpa_breakpoint_i = param_find("MC_TPA_BREAK_I"); _params_handles.tpa_breakpoint_d = param_find("MC_TPA_BREAK_D"); @@ -540,6 +559,14 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.pitch_rate_ff, &v); _params.rate_ff(1) = v; + param_get(_params_handles.d_term_cutoff_freq, &_params.d_term_cutoff_freq); + _lp_filters_d[0].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq); + _lp_filters_d[1].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq); + _lp_filters_d[2].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq); + _lp_filters_d[0].reset(_rates_prev(0)); + _lp_filters_d[1].reset(_rates_prev(1)); + _lp_filters_d[2].reset(_rates_prev(2)); + param_get(_params_handles.tpa_breakpoint_p, &_params.tpa_breakpoint_p); param_get(_params_handles.tpa_breakpoint_i, &_params.tpa_breakpoint_i); param_get(_params_handles.tpa_breakpoint_d, &_params.tpa_breakpoint_d); @@ -998,13 +1025,20 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; + /* apply low-pass filtering to the rates for D-term */ + math::Vector<3> rates_filtered( + _lp_filters_d[0].apply(rates(0)), + _lp_filters_d[1].apply(rates(1)), + _lp_filters_d[2].apply(rates(2))); + _att_control = rates_p_scaled.emult(rates_err) + - _rates_int + - rates_d_scaled.emult(_rates_prev - rates) / dt + + _rates_int - + rates_d_scaled.emult(rates_filtered - _rates_prev_filtered) / dt + _params.rate_ff.emult(_rates_sp); _rates_sp_prev = _rates_sp; _rates_prev = rates; + _rates_prev_filtered = rates_filtered; /* update integral only if motors are providing enough thrust to be effective */ if (_thrust_sp > MIN_TAKEOFF_THRUST) { @@ -1093,7 +1127,10 @@ MulticopterAttitudeControl::task_main() px4_pollfd_struct_t poll_fds = {}; poll_fds.events = POLLIN; + const hrt_abstime task_start = hrt_absolute_time(); hrt_abstime last_run = task_start; + float dt_accumulator = 0.f; + int loop_counter = 0; while (!_task_should_exit) { @@ -1299,6 +1336,23 @@ MulticopterAttitudeControl::task_main() } } } + + /* calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) */ + if (!_v_control_mode.flag_armed || (now - task_start) < 3300000) { + dt_accumulator += dt; + ++loop_counter; + + if (dt_accumulator > 1.f) { + const float loop_update_rate = (float)loop_counter / dt_accumulator; + _loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f; + dt_accumulator = 0; + loop_counter = 0; + _lp_filters_d[0].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq); + _lp_filters_d[1].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq); + _lp_filters_d[2].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq); + } + } + } perf_end(_loop_perf); diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 14890edb7b..225ca6ae09 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -554,3 +554,20 @@ PARAM_DEFINE_FLOAT(MC_TPA_RATE_I, 0.0f); * @group Multicopter Attitude Control */ PARAM_DEFINE_FLOAT(MC_TPA_RATE_D, 0.0f); + +/** + * Cutoff frequency for the low pass filter on the D-term in the rate controller + * + * The D-term uses the derivative of the rate and thus is the most susceptible to noise. + * Therefore, using a D-term filter allows to decrease the driver-level filtering, which + * leads to reduced control latency and permits to increase the P gains. + * A value of 0 disables the filter. + * + * @unit Hz + * @min 0 + * @max 1000 + * @decimal 0 + * @increment 10 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_DTERM_CUTOFF, 0.f);