Phase lag pre-arm check

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 = \

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;

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);
 

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 &notch : 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()

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;

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);