From 99f3b475393653a3b6969f96c431f08c6172ac63 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 15 May 2024 15:56:42 +0100 Subject: [PATCH 1/4] AP_Vehicle: add pre-arm notch check to prevent notches wider than f/sqrt(nsources) --- libraries/AP_Vehicle/AP_Vehicle.cpp | 35 +++++++++++++++++++++++++++++ libraries/AP_Vehicle/AP_Vehicle.h | 3 ++- 2 files changed, 37 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index b05851bdfe7f8..ceb117581e618 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -1111,6 +1111,41 @@ bool AP_Vehicle::block_GCS_mode_change(uint8_t mode_num, const uint8_t *mode_lis } #endif +#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED +/// pre_arm_check - returns true if all pre-takeoff checks have completed successfully +bool AP_Vehicle::pre_arm_check_notches() const +{ +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduSub) + AP_Motors* motors = AP::motors(); +#else // APM_BUILD_Rover + AP_MotorsUGV *motors = AP::motors_ugv(); +#endif + const uint8_t nmotors = motors == nullptr ? 1 : __builtin_popcount(motors->get_motor_mask()); + + for (auto ¬ch : ins.harmonic_notches) { + if (!notch.params.enabled() || notch.params.hasOption(HarmonicNotchFilterParams::Options::DisableNotchPreArm)) { + continue; + } + float nsources = 1.0f; + if (notch.params.hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) { + if (notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateBLHeli + || notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateThrottle) { + nsources = nmotors; + } + else if (notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) { + nsources = 3; + } + } + + if (notch.params.bandwidth_hz() > notch.params.center_freq_hz() / sqrtf(nsources)) { + return false; + } + } + + return true; +} +#endif + AP_Vehicle *AP_Vehicle::_singleton = nullptr; AP_Vehicle *AP_Vehicle::get_singleton() diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 8b22c9395d73d..8fbfcf5caf094 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -293,7 +293,8 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { virtual bool set_home_to_current_location(bool lock) WARN_IF_UNUSED { return false; } virtual bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED { return false; } #endif - + // notch pre-arm check + bool pre_arm_check_notches() const; protected: virtual void init_ardupilot() = 0; From 33610b29f491a568b6cacf837a8669d2ac67d9d2 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 15 May 2024 15:57:07 +0100 Subject: [PATCH 2/4] AP_Arming: call notches pre-arm check --- libraries/AP_Arming/AP_Arming.cpp | 16 +++++++++++++++- libraries/AP_Arming/AP_Arming.h | 2 ++ 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 5b1bd0d2f95ec..75ea3a7c78e15 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -1635,7 +1635,10 @@ bool AP_Arming::pre_arm_checks(bool report) & opendroneid_checks(report) #endif #if AP_ARMING_CRASHDUMP_ACK_ENABLED - & crashdump_checks(report) + & crashdump_checks(report) +#endif +#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED + & notch_checks(report) #endif & serial_protocol_checks(report) & estop_checks(report); @@ -1725,6 +1728,17 @@ bool AP_Arming::crashdump_checks(bool report) } #endif // AP_ARMING_CRASHDUMP_ACK_ENABLED +bool AP_Arming::notch_checks(bool report) +{ +#if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_Rover) + if (!AP::vehicle()->pre_arm_check_notches()) { + check_failed(ARMING_CHECK_NOTCHES, report, "Notch bandwidth too high"); + return false; + } +#endif + return true; +} + bool AP_Arming::mandatory_checks(bool report) { bool ret = true; diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h index be59921be1684..251bb8a1c3e39 100644 --- a/libraries/AP_Arming/AP_Arming.h +++ b/libraries/AP_Arming/AP_Arming.h @@ -43,6 +43,7 @@ class AP_Arming { ARMING_CHECK_VISION = (1U << 18), ARMING_CHECK_FFT = (1U << 19), ARMING_CHECK_OSD = (1U << 20), + ARMING_CHECK_NOTCHES = (1U << 21), }; enum class Method { @@ -235,6 +236,7 @@ class AP_Arming { #if AP_ARMING_CRASHDUMP_ACK_ENABLED bool crashdump_checks(bool report); #endif + bool notch_checks(bool report); virtual bool system_checks(bool report); From f0a95fbafd227632fe8000fcda3097dbe2ad1cb4 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 15 May 2024 16:51:07 +0100 Subject: [PATCH 3/4] autotest: adjust for notch blocking --- Tools/autotest/arducopter.py | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 6bd736a1db26c..1555507b50724 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -6119,8 +6119,11 @@ def DynamicRpmNotches(self): esc_hz = self.get_average_esc_frequency() esc_peakdb1 = psd["X"][int(esc_hz)] - # now add notch-per motor and check that the peak is squashed - self.set_parameter("INS_HNTCH_OPTS", 2) + # now add notch-per motor at f/4 and check that the peak is squashed + self.set_parameters({ + "INS_HNTCH_OPTS": 2, + "INS_HNTCH_BW": 20, + }) self.reboot_sitl() freq, hover_throttle, peakdb2, psd = \ @@ -6147,6 +6150,8 @@ def DynamicRpmNotches(self): try: self.progress("Flying Octacopter with ESC telemetry driven dynamic notches") self.set_parameter("INS_HNTCH_OPTS", 0) + self.set_parameter("INS_HNTCH_BW", 40) + self.customise_SITL_commandline( [], defaults_filepath=','.join(self.model_defaults_filepath("octa")), @@ -6159,8 +6164,11 @@ def DynamicRpmNotches(self): esc_peakdb1 = psd["X"][int(esc_hz)] # now add notch-per motor and check that the peak is squashed - self.set_parameter("INS_HNTCH_HMNCS", 1) - self.set_parameter("INS_HNTCH_OPTS", 2) + self.set_parameters({ + "INS_HNTCH_OPTS": 2, + "INS_HNTCH_BW": 20, + "INS_HNTCH_HMNCS": 1 + }) self.reboot_sitl() freq, hover_throttle, peakdb2, psd = \ From 984edff59f705724b52bdcbf7bbed237ab6c2254 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 25 May 2024 20:15:56 +0100 Subject: [PATCH 4/4] Filter: add DisableNotchPreArm to HarmonicNotchFilter --- libraries/Filter/HarmonicNotchFilter.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h index 3a5704d67d6cc..cb50ee4dba3d4 100644 --- a/libraries/Filter/HarmonicNotchFilter.h +++ b/libraries/Filter/HarmonicNotchFilter.h @@ -114,6 +114,7 @@ class HarmonicNotchFilterParams : public NotchFilterParams { EnableOnAllIMUs = 1<<3, TripleNotch = 1<<4, TreatLowAsMin = 1<<5, + DisableNotchPreArm = 1<<6, }; HarmonicNotchFilterParams(void);