From 027f924e169798e00b0da2286850d1480614a8e1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 13:36:18 +1000 Subject: [PATCH] Copter: support two full harmonic notch filters --- ArduCopter/Copter.h | 2 +- ArduCopter/system.cpp | 38 +++++++++++++++++++------------------- 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 48b1ca783f..cb4283278b 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -879,7 +879,7 @@ private: // system.cpp void init_ardupilot() override; void startup_INS_ground(); - void update_dynamic_notch() override; + void update_dynamic_notch(uint8_t idx) override; bool position_ok() const; bool ekf_has_absolute_position() const; bool ekf_has_relative_position() const; diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index dd5213b88b..158132f743 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -234,24 +234,24 @@ void Copter::startup_INS_ground() } // update the harmonic notch filter center frequency dynamically -void Copter::update_dynamic_notch() +void Copter::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; } const float throttle_freq = ref_freq * MAX(1.0f, sqrtf(motors->get_throttle_out() / ref)); - 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 - ins.update_harmonic_notch_freq_hz(throttle_freq); + ins.update_harmonic_notch_freq_hz(idx, throttle_freq); break; #if RPM_ENABLED == ENABLED @@ -259,48 +259,48 @@ void Copter::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 / 60.0f)); } else { - ins.update_harmonic_notch_freq_hz(ref_freq); + ins.update_harmonic_notch_freq_hz(idx, ref_freq); } break; #endif #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); } else { // throttle fallback - ins.update_harmonic_notch_freq_hz(throttle_freq); + ins.update_harmonic_notch_freq_hz(idx, throttle_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; } }