Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: make sure dynamic notches always get updates so that slew limiting is not too aggressive

This commit is contained in:
Andy Piper 2022-09-20 08:08:26 +01:00 committed by Andrew Tridgell
parent b8f01323ec
commit 8037e10be7
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -1615,6 +1615,7 @@ void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool conv
const float center_freq = calculated_notch_freq_hz[0];
if (!is_equal(last_bandwidth_hz[instance], params.bandwidth_hz()) ||
!is_equal(last_attenuation_dB[instance], params.attenuation_dB()) ||
(params.tracking_mode() == HarmonicNotchDynamicMode::Fixed && !is_equal(last_center_freq_hz[instance], center_freq)) ||
converging) {
filter[instance].init(gyro_rate,
center_freq,
@ -1623,7 +1624,7 @@ void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool conv
last_center_freq_hz[instance] = center_freq;
last_bandwidth_hz[instance] = params.bandwidth_hz();
last_attenuation_dB[instance] = params.attenuation_dB();
} else if (!is_equal(last_center_freq_hz[instance], center_freq)) {
} else if (params.tracking_mode() != HarmonicNotchDynamicMode::Fixed) {
if (num_calculated_notch_frequencies > 1) {
filter[instance].update(num_calculated_notch_frequencies, calculated_notch_freq_hz);
} else {