yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

mc_att_control: update & reset d-term lowpass filter only when frequency changes

Browse Source
This commit is contained in:
Beat Küng
2018-02-19 16:10:53 +01:00
committed by Lorenz Meier
parent 5b7c062f67
commit 7b5b1c4f23
1 changed files with 9 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@@ -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);