From 61e91dde35751b91f9fee88dddc9aa108b686580 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Thu, 27 Jul 2023 18:34:43 +0200
Subject: [PATCH] autotest: add PID notches test

---
 Tools/autotest/arducopter.py | 44 ++++++++++++++++++++++++++++++++++++
 Tools/autotest/helicopter.py | 44 ++++++++++++++++++++++++++++++++++++
 2 files changed, 88 insertions(+)

diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index f77e3cb5b9..701c3d50eb 100644
--- a/Tools/autotest/arducopter.py
+++ b/Tools/autotest/arducopter.py
@@ -5960,6 +5960,49 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
             freqs.append(m.PkAvg)
         return numpy.median(numpy.asarray(freqs)), len(freqs)
 
+    def PIDNotches(self):
+        """Use dynamic harmonic notch to control motor noise."""
+        self.progress("Flying with PID notches")
+        self.context_push()
+
+        ex = None
+        try:
+            self.set_parameters({
+                "AHRS_EKF_TYPE": 10,
+                "INS_LOG_BAT_MASK": 3,
+                "INS_LOG_BAT_OPT": 0,
+                "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": 180,  # yaw
+                "ATC_RAT_RLL_ADV": 1,
+                "ATC_RAT_RLL_NEF": 120,
+                "ATC_RAT_RLL_NBW": 50,
+                "ATC_RAT_PIT_ADV": 1,
+                "ATC_RAT_PIT_NEF": 120,
+                "ATC_RAT_PIT_NBW": 50,
+                "ATC_RAT_YAW_ADV": 1,
+                "ATC_RAT_YAW_NEF": 180,
+                "ATC_RAT_YAW_NBW": 50,
+                "SIM_GYR1_RND": 5,
+            })
+            self.reboot_sitl()
+
+            self.takeoff(10, mode="ALT_HOLD")
+
+            freq, hover_throttle, peakdb1 = self.hover_and_check_matched_frequency_with_fft(5, 20, 350, reverse=True)
+
+        except Exception as e:
+            self.print_exception_caught(e)
+            ex = e
+
+        self.context_pop()
+
+        if ex is not None:
+            raise ex
+
     def ThrottleGainBoost(self):
         """Use PD and Angle P boost for anti-gravity."""
         # basic gyro sample rate test
@@ -10561,6 +10604,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
             Test(self.DynamicNotches, attempts=4),
             self.PositionWhenGPSIsZero,
             Test(self.DynamicRpmNotches, attempts=4),
+            self.PIDNotches,
             self.RefindGPS,
             Test(self.GyroFFT, attempts=1, speedup=8),
             Test(self.GyroFFTHarmonic, attempts=4, speedup=8),
diff --git a/Tools/autotest/helicopter.py b/Tools/autotest/helicopter.py
index 0485556eb1..d3634ed416 100644
--- a/Tools/autotest/helicopter.py
+++ b/Tools/autotest/helicopter.py
@@ -801,6 +801,49 @@ class AutoTestHelicopter(AutoTestCopter):
         self.set_rc(8, 1000)
         self.disarm_vehicle()
 
+    def PIDNotches(self):
+        """Use dynamic harmonic notch to control motor noise."""
+        self.progress("Flying with PID notches")
+        self.context_push()
+
+        ex = None
+        try:
+            self.set_parameters({
+                "AHRS_EKF_TYPE": 10,
+                "INS_LOG_BAT_MASK": 3,
+                "INS_LOG_BAT_OPT": 0,
+                "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": 180,  # yaw
+                "ATC_RAT_RLL_ADV": 1,
+                "ATC_RAT_RLL_NEF": 120,
+                "ATC_RAT_RLL_NBW": 50,
+                "ATC_RAT_PIT_ADV": 1,
+                "ATC_RAT_PIT_NEF": 120,
+                "ATC_RAT_PIT_NBW": 50,
+                "ATC_RAT_YAW_ADV": 1,
+                "ATC_RAT_YAW_NEF": 180,
+                "ATC_RAT_YAW_NBW": 50,
+                "SIM_GYR1_RND": 5,
+            })
+            self.reboot_sitl()
+
+            self.takeoff(10, mode="ALT_HOLD")
+
+            freq, hover_throttle, peakdb1 = self.hover_and_check_matched_frequency_with_fft(5, 20, 350, reverse=True)
+
+        except Exception as e:
+            self.print_exception_caught(e)
+            ex = e
+
+        self.context_pop()
+
+        if ex is not None:
+            raise ex
+
     def tests(self):
         '''return list of all tests'''
         ret = vehicle_test_suite.TestSuite.tests(self)
@@ -817,6 +860,7 @@ class AutoTestHelicopter(AutoTestCopter):
             self.AirspeedDrivers,
             self.TurbineStart,
             self.NastyMission,
+            self.PIDNotches,
         ])
         return ret