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

AP_GyroFFT: added defaults for FFT with no notch 

allow for testing with FFT enabled, and defaulting number of
frequencies to look for
This commit is contained in:
Andrew Tridgell 2022-04-17 17:24:45 +10:00 committed by Randy Mackay
parent 9fba96c8d0
commit 7fd478fc4a
1 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_GyroFFT/AP_GyroFFT.cpp
View File

@ -253,12 +253,17 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz)
uint8_t harmonics = 0;
uint8_t num_notches = 0;
for (auto &notch : _ins->harmonic_notches) {
if (notch.params.enabled() && notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) {
if (notch.params.enabled()) {
harmonics |= notch.params.harmonics();
num_notches = MAX(num_notches, notch.num_dynamic_notches);
}
}
if (harmonics == 0) {
// this allows use of FFT to find peaks with all notch filters disabled
harmonics = 3;
}
// count the number of active harmonics or dynamic notchs
_tracked_peaks = constrain_int16(MAX(__builtin_popcount(harmonics),
num_notches), 1, FrequencyPeak::MAX_TRACKED_PEAKS);
@ -384,7 +389,7 @@ void AP_GyroFFT::update()
if (!_rpy_health.x && !_rpy_health.y) {
_health = 0;
} else {
uint8_t num_notches = 0;
uint8_t num_notches = 1;
for (auto &notch : _ins->harmonic_notches) {
if (notch.params.enabled()) {
num_notches = MAX(num_notches, notch.num_dynamic_notches);