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

AP_GyroFFT: only allow one harmonic notch filter to be linked to FFT

This commit is contained in:
Andrew Tridgell 2022-04-13 20:21:51 +10:00 committed by Randy Mackay
parent f015e827bf
commit 6c61ea970d
1 changed files with 13 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_GyroFFT/AP_GyroFFT.cpp
View File

@ -248,10 +248,21 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz)
return;
}
const uint8_t harmonics = _ins->get_gyro_harmonic_notch_harmonics();
// check for harmonics across all harmonic notch filters
// note that we only allow one harmonic notch filter linked to the FFT code
uint8_t harmonics = 0;
uint8_t num_notches = 0;
for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
if (_ins->get_gyro_harmonic_notch_tracking_mode(i) == HarmonicNotchDynamicMode::UpdateGyroFFT) {
harmonics = _ins->get_gyro_harmonic_notch_harmonics(i);
num_notches = _ins->get_num_gyro_dynamic_notches(i);
break;
}
}
// count the number of active harmonics or dynamic notchs
_tracked_peaks = constrain_int16(MAX(__builtin_popcount(harmonics),
_ins->get_num_gyro_dynamic_notches()), 1, FrequencyPeak::MAX_TRACKED_PEAKS);
num_notches), 1, FrequencyPeak::MAX_TRACKED_PEAKS);
// calculate harmonic multiplier. this assumes the harmonics configured on the
// harmonic notch reflect the multiples of the fundamental harmonic that should be tracked