Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: use calculated number of notches for dynamic harmonic

Browse Source
This commit is contained in:
Andy Piper 2021-11-22 20:28:53 +00:00 committed by Andrew Tridgell
parent 5259272326
commit e1c75b2c15
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/system.cpp
View File

@ -264,7 +264,7 @@ void Copter::update_dynamic_notch()
// set the harmonic notch filter frequency scaled on measured frequency // set the harmonic notch filter frequency scaled on measured frequency
if (ins.has_harmonic_option(HarmonicNotchFilterParams::Options::DynamicHarmonic)) { if (ins.has_harmonic_option(HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
float notches[INS_MAX_NOTCHES]; float notches[INS_MAX_NOTCHES];
const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(INS_MAX_NOTCHES, notches); const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(ins.get_num_gyro_dynamic_notches(), notches);
for (uint8_t i = 0; i < num_notches; i++) { for (uint8_t i = 0; i < num_notches; i++) {
notches[i] = MAX(ref_freq, notches[i]); notches[i] = MAX(ref_freq, notches[i]);
@ -284,7 +284,7 @@ void Copter::update_dynamic_notch()
// set the harmonic notch filter frequency scaled on measured frequency // set the harmonic notch filter frequency scaled on measured frequency
if (ins.has_harmonic_option(HarmonicNotchFilterParams::Options::DynamicHarmonic)) { if (ins.has_harmonic_option(HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
float notches[INS_MAX_NOTCHES]; float notches[INS_MAX_NOTCHES];
const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(INS_MAX_NOTCHES, notches); const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(ins.get_num_gyro_dynamic_notches(), notches);
ins.update_harmonic_notch_frequencies_hz(peaks, notches); ins.update_harmonic_notch_frequencies_hz(peaks, notches);
} else { } else {