mc_att_control: update & reset d-term lowpass filter only when freque…

…ncy changes
PX4 · Feb 19, 2018 · 7b5b1c4 · 7b5b1c4
1 parent 5b7c062
commit 7b5b1c4
Showing 1 changed file with 9 additions and 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
@@ -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);