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

AP_InertialSensor: fixed Q calculation for notch filters 

when doing the init() we must use the reference frequency, not the
current frequency. Using the current frequency leaves us with an
incorrect value for Q. If the current frequency is below the reference
frequency (which shouldn't happen in 4.5, but has been seen in 4.3)
then the Q can be much too low and massive phase lag can happen.

The vulnerability in 4.5.x is that the current frequency could be well
above the reference frequency. For example, the user may be doing a
motor test at 30s after boot, which is when we stop the
sensors_converging() test, and thus is the last time we call
init(). In that case we can end up with a Q which is much larger than
the one that should come from INS_HNTCH_FREQ and INS_HNTCH_BW, and
thus end up with a filter that produces a lot less attenuation than is
desired, potentially leading to instability due to high noise.

There are other scenarios that can cause this - for example a motor
test of a fwd motor at 30s after boot, or a spurious FFT peak due to
someone knocking the aircraft, or the vibration of a IC engine.
This commit is contained in:
Andrew Tridgell 2024-11-28 16:43:56 +11:00 committed by Randy Mackay
parent 2a3dc4b7bf
commit 71fd9a4a08
1 changed files with 13 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -1059,8 +1059,10 @@ AP_InertialSensor::init(uint16_t loop_rate)
notch.filter[i].allocate_filters(notch.num_dynamic_notches, notch.filter[i].allocate_filters(notch.num_dynamic_notches,
notch.params.harmonics(), double_notch ? 2 : triple_notch ? 3 : 1); notch.params.harmonics(), double_notch ? 2 : triple_notch ? 3 : 1);
// initialise default settings, these will be subsequently changed in AP_InertialSensor_Backend::update_gyro() // initialise default settings, these will be subsequently changed in AP_InertialSensor_Backend::update_gyro()
notch.filter[i].init(_gyro_raw_sample_rates[i], notch.calculated_notch_freq_hz[0], notch.filter[i].init(_gyro_raw_sample_rates[i],
notch.params.bandwidth_hz(), notch.params.attenuation_dB()); notch.params.center_freq_hz(),
notch.params.bandwidth_hz(),
notch.params.attenuation_dB());
} }
} }
} }
@ -1783,25 +1785,27 @@ void AP_InertialSensor::_save_gyro_calibration()
*/ */
void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool converging, float gyro_rate) void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool converging, float gyro_rate)
{ {
const float center_freq = calculated_notch_freq_hz[0];
if (!is_equal(last_bandwidth_hz[instance], params.bandwidth_hz()) || if (!is_equal(last_bandwidth_hz[instance], params.bandwidth_hz()) ||
!is_equal(last_attenuation_dB[instance], params.attenuation_dB()) || !is_equal(last_attenuation_dB[instance], params.attenuation_dB()) ||
(params.tracking_mode() == HarmonicNotchDynamicMode::Fixed && !is_equal(last_center_freq_hz[instance], center_freq)) || !is_equal(last_center_freq_hz[instance], params.center_freq_hz()) ||
converging) { converging) {
filter[instance].init(gyro_rate, filter[instance].init(gyro_rate,
center_freq, params.center_freq_hz(),
params.bandwidth_hz(), params.bandwidth_hz(),
params.attenuation_dB()); params.attenuation_dB());
last_center_freq_hz[instance] = center_freq; last_center_freq_hz[instance] = params.center_freq_hz();
last_bandwidth_hz[instance] = params.bandwidth_hz(); last_bandwidth_hz[instance] = params.bandwidth_hz();
last_attenuation_dB[instance] = params.attenuation_dB(); last_attenuation_dB[instance] = params.attenuation_dB();
} else if (params.tracking_mode() != HarmonicNotchDynamicMode::Fixed) { }
if (params.tracking_mode() != HarmonicNotchDynamicMode::Fixed) {
// we always call the update for non-fixed notches, even if we
// just called init() as the update may use a different
// frequency to the init
if (num_calculated_notch_frequencies > 1) { if (num_calculated_notch_frequencies > 1) {
filter[instance].update(num_calculated_notch_frequencies, calculated_notch_freq_hz); filter[instance].update(num_calculated_notch_frequencies, calculated_notch_freq_hz);
} else { } else {
filter[instance].update(center_freq); filter[instance].update(calculated_notch_freq_hz[0]);
} }
last_center_freq_hz[instance] = center_freq;
} }
} }