Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GyroFFT: correctly count the number of active harmonics

Browse Source
This commit is contained in:
Andy Piper 2020-07-12 20:09:21 +01:00 committed by Andrew Tridgell
parent 05366e9aa9
commit adb9f70bc9
1 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_GyroFFT/AP_GyroFFT.cpp
View File

@ -249,17 +249,23 @@ void AP_GyroFFT::init(uint32_t target_looptime_us)
return;
}
// count the number of active harmonics
for (uint8_t i = 0; i < HNF_MAX_HARMONICS; i++) {
if (_ins->get_gyro_harmonic_notch_harmonics() & (1<<i)) {
_harmonics++;
}
}
_harmonics = constrain_int16(_harmonics, 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
uint8_t first_harmonic = 0;
_harmonics = 1; // always search for 1
if (_harmonic_fit > 0) {
for (uint8_t i = 0; i < HNF_MAX_HARMONICS; i++) {
if (_ins->get_gyro_harmonic_notch_harmonics() & (1<<i)) {
if (first_harmonic == 0) {
first_harmonic = i + 1;
} else {
_harmonics++;
_harmonic_multiplier = float(i + 1) / first_harmonic;
break;
}