From 2cea3d21b5bac3e198acd9a5ce5d3dc9b8c2d704 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 13:36:18 +1000 Subject: [PATCH] Plane: support two full harmonic notch filters --- ArduPlane/Plane.h | 2 +- ArduPlane/system.cpp | 40 ++++++++++++++++++++-------------------- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 8a74bb1e3c..bbc05e3cca 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1056,7 +1056,7 @@ private: void startup_INS_ground(void); bool should_log(uint32_t mask); int8_t throttle_percentage(void); - void update_dynamic_notch() override; + void update_dynamic_notch(uint8_t idx) override; void notify_mode(const Mode& mode); // takeoff.cpp diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 205dda4445..9bf33a7320 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -447,25 +447,25 @@ int8_t Plane::throttle_percentage(void) } // update the harmonic notch filter center frequency dynamically -void Plane::update_dynamic_notch() +void Plane::update_dynamic_notch(uint8_t idx) { - if (!ins.gyro_harmonic_notch_enabled()) { + if (!ins.gyro_harmonic_notch_enabled(idx)) { return; } - const float ref_freq = ins.get_gyro_harmonic_notch_center_freq_hz(); - const float ref = ins.get_gyro_harmonic_notch_reference(); + const float ref_freq = ins.get_gyro_harmonic_notch_center_freq_hz(idx); + const float ref = ins.get_gyro_harmonic_notch_reference(idx); if (is_zero(ref)) { - ins.update_harmonic_notch_freq_hz(ref_freq); + ins.update_harmonic_notch_freq_hz(idx, ref_freq); return; } - switch (ins.get_gyro_harmonic_notch_tracking_mode()) { + switch (ins.get_gyro_harmonic_notch_tracking_mode(idx)) { case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle #if HAL_QUADPLANE_ENABLED if (quadplane.available()) { - ins.update_harmonic_notch_freq_hz(ref_freq * MAX(1.0f, sqrtf(quadplane.motors->get_throttle_out() / ref))); + ins.update_harmonic_notch_freq_hz(idx, ref_freq * MAX(1.0f, sqrtf(quadplane.motors->get_throttle_out() / ref))); } #endif break; @@ -474,51 +474,51 @@ void Plane::update_dynamic_notch() float rpm; if (rpm_sensor.get_rpm(0, rpm)) { // set the harmonic notch filter frequency from the main rotor rpm - ins.update_harmonic_notch_freq_hz(MAX(ref_freq, rpm * ref / 60.0f)); + ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, rpm * ref * (1/60.0))); } else { - ins.update_harmonic_notch_freq_hz(ref_freq); + ins.update_harmonic_notch_freq_hz(idx, ref_freq); } break; #if HAL_WITH_ESC_TELEM case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking // set the harmonic notch filter frequency scaled on measured frequency - if (ins.has_harmonic_option(HarmonicNotchFilterParams::Options::DynamicHarmonic)) { + if (ins.has_harmonic_option(idx, HarmonicNotchFilterParams::Options::DynamicHarmonic)) { float notches[INS_MAX_NOTCHES]; - const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(ins.get_num_gyro_dynamic_notches(), notches); + const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(ins.get_num_gyro_dynamic_notches(idx), notches); for (uint8_t i = 0; i < num_notches; i++) { notches[i] = MAX(ref_freq, notches[i]); } if (num_notches > 0) { - ins.update_harmonic_notch_frequencies_hz(num_notches, notches); + ins.update_harmonic_notch_frequencies_hz(idx, num_notches, notches); #if HAL_QUADPLANE_ENABLED } else if (quadplane.available()) { // throttle fallback - ins.update_harmonic_notch_freq_hz(ref_freq * MAX(1.0f, sqrtf(quadplane.motors->get_throttle_out() / ref))); + ins.update_harmonic_notch_freq_hz(idx, ref_freq * MAX(1.0f, sqrtf(quadplane.motors->get_throttle_out() / ref))); #endif } else { - ins.update_harmonic_notch_freq_hz(ref_freq); + ins.update_harmonic_notch_freq_hz(idx, ref_freq); } } else { - ins.update_harmonic_notch_freq_hz(MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref)); + ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref)); } break; #endif #if HAL_GYROFFT_ENABLED case HarmonicNotchDynamicMode::UpdateGyroFFT: // FFT based tracking // set the harmonic notch filter frequency scaled on measured frequency - if (ins.has_harmonic_option(HarmonicNotchFilterParams::Options::DynamicHarmonic)) { + if (ins.has_harmonic_option(idx, HarmonicNotchFilterParams::Options::DynamicHarmonic)) { float notches[INS_MAX_NOTCHES]; - const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(ins.get_num_gyro_dynamic_notches(), notches); + const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(ins.get_num_gyro_dynamic_notches(idx), notches); - ins.update_harmonic_notch_frequencies_hz(peaks, notches); + ins.update_harmonic_notch_frequencies_hz(idx, peaks, notches); } else { - ins.update_harmonic_notch_freq_hz(gyro_fft.get_weighted_noise_center_freq_hz()); + ins.update_harmonic_notch_freq_hz(idx, gyro_fft.get_weighted_noise_center_freq_hz()); } break; #endif case HarmonicNotchDynamicMode::Fixed: // static default: - ins.update_harmonic_notch_freq_hz(ref_freq); + ins.update_harmonic_notch_freq_hz(idx, ref_freq); break; } }