zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Plane: use calculated number of notches for dynamic harmonic

gps-1.3.1
Andy Piper 3 years ago committed by Andrew Tridgell
parent
e1c75b2c15
commit
aefc97fafa
1 changed files with 2 additions and 2 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      ArduPlane/system.cpp

4
ArduPlane/system.cpp
Unescape View File

@ -491,7 +491,7 @@ void Plane::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]);
@ -515,7 +515,7 @@ void Plane::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 {

Write Preview
Loading…
Cancel
Save