From 430e02bd5fa0c45e5bde858974bfc46ee72cf6cc Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 7 Sep 2024 16:45:10 +0100 Subject: [PATCH 1/3] Filter: ensure harmonic notch filters are always working --- libraries/Filter/HarmonicNotchFilter.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index ea42cf249e14f..06a2400a15ff3 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -191,6 +191,8 @@ void HarmonicNotchFilter::init(float sample_freq_hz, HarmonicNotchFilterParam NotchFilter::calculate_A_and_Q(center_freq_hz, bandwidth_hz / _composite_notches, attenuation_dB, _A, _Q); _initialised = true; + // ensure static notches are allocated and working + update(center_freq_hz); } /* From 67011d1c9c9129576dfd78e0b05c6ffed1be3ea9 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 7 Sep 2024 16:45:49 +0100 Subject: [PATCH 2/3] autotest: static notch test --- Tools/autotest/arducopter.py | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index e970300da682a..02618ec5a494c 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -6011,6 +6011,7 @@ def ThrowMode(self): def hover_and_check_matched_frequency_with_fft_and_psd(self, dblevel=-15, minhz=200, maxhz=300, peakhz=None, reverse=None, takeoff=True, instance=0): + '''Takeoff and hover, checking the noise against the provided db level and returning psd''' # find a motor peak if takeoff: self.takeoff(10, mode="ALT_HOLD") @@ -6041,6 +6042,7 @@ def hover_and_check_matched_frequency_with_fft_and_psd(self, dblevel=-15, minhz= def hover_and_check_matched_frequency_with_fft(self, dblevel=-15, minhz=200, maxhz=300, peakhz=None, reverse=None, takeoff=True, instance=0): + '''Takeoff and hover, checking the noise against the provided db level and returning peak db''' freq, hover_throttle, peakdb, psd = \ self.hover_and_check_matched_frequency_with_fft_and_psd(dblevel, minhz, maxhz, peakhz, reverse, takeoff, instance) @@ -6335,6 +6337,33 @@ def PIDNotches(self): self.hover_and_check_matched_frequency_with_fft(dblevel=5, minhz=20, maxhz=350, reverse=True) + def StaticNotches(self): + """Use static harmonic notch to control motor noise.""" + self.progress("Flying with Static notches") + self.set_parameters({ + "AHRS_EKF_TYPE": 10, + "INS_LOG_BAT_MASK": 3, + "INS_LOG_BAT_OPT": 4, + "INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour + "LOG_BITMASK": 65535, + "LOG_DISARMED": 0, + "SIM_VIB_FREQ_X": 120, # roll + "SIM_VIB_FREQ_Y": 120, # pitch + "SIM_VIB_FREQ_Z": 120, # yaw + "SIM_VIB_MOT_MULT": 0, + "SIM_GYR1_RND": 5, + "INS_HNTCH_ENABLE": 1, + "INS_HNTCH_FREQ": 120, + "INS_HNTCH_REF": 1.0, + "INS_HNTCH_HMNCS": 3, # first and second harmonic + "INS_HNTCH_ATT": 50, + "INS_HNTCH_BW": 40, + "INS_HNTCH_MODE": 0, # static notch + }) + self.reboot_sitl() + + self.hover_and_check_matched_frequency_with_fft(dblevel=-15, minhz=20, maxhz=350, reverse=True, instance=2) + def ThrottleGainBoost(self): """Use PD and Angle P boost for anti-gravity.""" # basic gyro sample rate test @@ -11848,6 +11877,7 @@ def tests2b(self): # this block currently around 9.5mins here self.PositionWhenGPSIsZero, self.DynamicRpmNotches, # Do not add attempts to this - failure is sign of a bug self.PIDNotches, + self.StaticNotches, self.RefindGPS, Test(self.GyroFFT, attempts=1, speedup=8), Test(self.GyroFFTHarmonic, attempts=4, speedup=8), From ab0db13b3b0f84f494bcdcd10013bbb538847bf1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Sep 2024 18:10:57 +1000 Subject: [PATCH 3/3] Filter: only update notch on init if fixed --- libraries/Filter/HarmonicNotchFilter.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index 06a2400a15ff3..c98a50eeac4ea 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -191,8 +191,11 @@ void HarmonicNotchFilter::init(float sample_freq_hz, HarmonicNotchFilterParam NotchFilter::calculate_A_and_Q(center_freq_hz, bandwidth_hz / _composite_notches, attenuation_dB, _A, _Q); _initialised = true; + // ensure static notches are allocated and working - update(center_freq_hz); + if (params->tracking_mode() == HarmonicNotchDynamicMode::Fixed) { + update(center_freq_hz); + } } /*