aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Vehicle: constrain FFT notch updates to obey reference frequency

Browse Source
This commit is contained in:
Andy Piper 2022-12-24 16:07:03 +00:00 committed by Andrew Tridgell
parent 90cdc1aa52
commit d29ccba3f7
1 changed files with 14 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Vehicle/AP_Vehicle.cpp
View File

@ -576,9 +576,21 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch)
float notches[INS_MAX_NOTCHES];
const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(notch.num_dynamic_notches, notches);
notch.update_frequencies_hz(peaks, notches);
if (peaks > 0) {
for (uint8_t i = 0; i < peaks; i++) {
notches[i] = MAX(ref_freq, notches[i]);
}
notch.update_frequencies_hz(peaks, notches);
} else { // since FFT can be used post-filter it is better to disable the notch when there is no data
notch.set_inactive(true);
}
} else {
notch.update_freq_hz(gyro_fft.get_weighted_noise_center_freq_hz());
float center_freq = gyro_fft.get_weighted_noise_center_freq_hz();
if (!is_zero(center_freq)) {
notch.update_freq_hz(MAX(ref_freq, center_freq));
} else { // since FFT can be used post-filter it is better to disable the notch when there is no data
notch.set_inactive(true);
}
}
break;
#endif