Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-10 09:53:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GyroFFT: use HarmonicNotch class

Browse Source
This commit is contained in:
Andrew Tridgell 2022-04-15 17:39:30 +10:00 committed by Randy Mackay
parent ef31de5fc5
commit 15dcfed927
1 changed files with 9 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_GyroFFT/AP_GyroFFT.cpp
View File

@ -252,10 +252,10 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz)
// note that we only allow one harmonic notch filter linked to the FFT code // note that we only allow one harmonic notch filter linked to the FFT code
uint8_t harmonics = 0; uint8_t harmonics = 0;
uint8_t num_notches = 0; uint8_t num_notches = 0;
for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) { for (auto &notch : _ins->harmonic_notches) {
if (_ins->get_gyro_harmonic_notch_tracking_mode(i) == HarmonicNotchDynamicMode::UpdateGyroFFT) { if (notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) {
harmonics = _ins->get_gyro_harmonic_notch_harmonics(i); harmonics = notch.params.harmonics();
num_notches = _ins->get_num_gyro_dynamic_notches(i); num_notches = notch.num_dynamic_notches;
break; break;
} }
} }
@ -385,7 +385,11 @@ void AP_GyroFFT::update()
if (!_rpy_health.x && !_rpy_health.y) { if (!_rpy_health.x && !_rpy_health.y) {
_health = 0; _health = 0;
} else { } else {
_health = MIN(_global_state._health, _ins->get_num_gyro_dynamic_notches()); uint8_t num_notches = 0;
for (auto &notch : _ins->harmonic_notches) {
num_notches = MAX(num_notches, notch.num_dynamic_notches);
}
_health = MIN(_global_state._health, num_notches);
} }
} }