From 7b5b1c4f23736fe6bc1c4b5f5cce77c2e6633cc4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 19 Feb 2018 16:10:53 +0100 Subject: [PATCH] mc_att_control: update & reset d-term lowpass filter only when frequency changes --- .../mc_att_control/mc_att_control_main.cpp | 15 +++++++++------ 1 file changed, 9 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 c4bfaacd82..0eb5c65d6d 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -558,12 +558,15 @@ MulticopterAttitudeControl::parameters_update() _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)); + + if (fabsf(_lp_filters_d[0].get_cutoff_freq() - _params.d_term_cutoff_freq) > 0.01f) { + _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);