diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 9ba4aaad15..a04026b39b 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -5898,8 +5898,8 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): (tdelta, max_good_tdelta)) self.progress("Vehicle returned") - def hover_and_check_matched_frequency_with_fft(self, dblevel=-15, minhz=200, maxhz=300, peakhz=None, - reverse=None, takeoff=True): + 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): # find a motor peak if takeoff: self.takeoff(10, mode="ALT_HOLD") @@ -5907,7 +5907,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): tstart, tend, hover_throttle = self.hover_for_interval(15) self.do_RTL() - psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6) + psd = self.mavfft_fttd(1, instance, tstart * 1.0e6, tend * 1.0e6) # batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin freq = psd["F"][numpy.argmax(psd["X"][minhz:maxhz]) + minhz] * (1000. / 1024.) @@ -5926,8 +5926,34 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.progress("Detected motor peak at %fHz, throttle %f%%, %fdB" % (freq, hover_throttle, peakdb)) + return freq, hover_throttle, peakdb, psd + + def hover_and_check_matched_frequency_with_fft(self, dblevel=-15, minhz=200, maxhz=300, peakhz=None, + reverse=None, takeoff=True, instance=0): + freq, hover_throttle, peakdb, psd = \ + self.hover_and_check_matched_frequency_with_fft_and_psd(dblevel, minhz, + maxhz, peakhz, reverse, takeoff, instance) return freq, hover_throttle, peakdb + def get_average_esc_frequency(self): + mlog = self.dfreader_for_current_onboard_log() + rpm_total = 0 + rpm_count = 0 + tho = 0 + while True: + m = mlog.recv_match() + if m is None: + break + msg_type = m.get_type() + if msg_type == "CTUN": + tho = m.ThO + elif msg_type == "ESC" and tho > 0.1: + rpm_total += m.RPM + rpm_count += 1 + + esc_hz = rpm_total / (rpm_count * 60) + return esc_hz + def DynamicNotches(self): """Use dynamic harmonic notch to control motor noise.""" self.progress("Flying with dynamic notches") @@ -5964,13 +5990,15 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): }) self.reboot_sitl() - freq, hover_throttle, peakdb1 = self.hover_and_check_matched_frequency_with_fft(-10, 20, 350, reverse=True) + freq, hover_throttle, peakdb1 = \ + self.hover_and_check_matched_frequency_with_fft(-10, 20, 350, reverse=True) # now add double dynamic notches and check that the peak is squashed self.set_parameter("INS_HNTCH_OPTS", 1) self.reboot_sitl() - freq, hover_throttle, peakdb2 = self.hover_and_check_matched_frequency_with_fft(-15, 20, 350, reverse=True) + freq, hover_throttle, peakdb2 = \ + self.hover_and_check_matched_frequency_with_fft(-15, 20, 350, reverse=True) # double-notch should do better, but check for within 5% if peakdb2 * 1.05 > peakdb1: @@ -5982,7 +6010,8 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.set_parameter("INS_HNTCH_OPTS", 16) self.reboot_sitl() - freq, hover_throttle, peakdb2 = self.hover_and_check_matched_frequency_with_fft(-15, 20, 350, reverse=True) + freq, hover_throttle, peakdb2 = \ + self.hover_and_check_matched_frequency_with_fft(-15, 20, 350, reverse=True) # triple-notch should do better, but check for within 5% if peakdb2 * 1.05 > peakdb1: @@ -6008,7 +6037,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): "AHRS_EKF_TYPE": 10, "INS_LOG_BAT_MASK": 3, "INS_LOG_BAT_OPT": 0, - "INS_GYRO_FILTER": 100, # set gyro filter high so we can observe behaviour + "INS_GYRO_FILTER": 300, # set gyro filter high so we can observe behaviour "LOG_BITMASK": 958, "LOG_DISARMED": 0, "SIM_VIB_MOT_MAX": 350, @@ -6019,12 +6048,14 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.takeoff(10, mode="ALT_HOLD") - # find a motor peak - freq, hover_throttle, peakdb = self.hover_and_check_matched_frequency_with_fft(-15, 200, 300) + # find a motor peak, the peak is at about 190Hz, so checking between 50 and 320Hz should be safe. + # there is a second harmonic at 380Hz which should be avoided to make the test reliable + # detect at -5dB so we don't pick some random noise as the peak. The actual peak is about +15dB + freq, hover_throttle, peakdb = self.hover_and_check_matched_frequency_with_fft(-5, 50, 320) # now add a dynamic notch and check that the peak is squashed self.set_parameters({ - "INS_LOG_BAT_OPT": 2, + "INS_LOG_BAT_OPT": 4, "INS_HNTCH_ENABLE": 1, "INS_HNTCH_FREQ": 80, "INS_HNTCH_REF": 1.0, @@ -6035,19 +6066,34 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): }) self.reboot_sitl() - freq, hover_throttle, peakdb1 = self.hover_and_check_matched_frequency_with_fft(-10, 20, 350, reverse=True) + # -10dB is pretty conservative - actual is about -25dB + freq, hover_throttle, peakdb1, psd = \ + self.hover_and_check_matched_frequency_with_fft_and_psd(-10, 50, 320, reverse=True, instance=2) + # find the noise at the motor frequency + 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) self.reboot_sitl() - freq, hover_throttle, peakdb2 = self.hover_and_check_matched_frequency_with_fft(-15, 20, 350, reverse=True) + freq, hover_throttle, peakdb2, psd = \ + self.hover_and_check_matched_frequency_with_fft_and_psd(-10, 50, 320, reverse=True, instance=2) + # find the noise at the motor frequency + esc_hz = self.get_average_esc_frequency() + esc_peakdb2 = psd["X"][int(esc_hz)] - # notch-per-motor should do better, but check for within 10%. ( its mostly within 5%, but does vary a bit) - if peakdb2 * 1.10 > peakdb1: + # notch-per-motor will be better at the average ESC frequency + if esc_peakdb2 > esc_peakdb1: raise NotAchievedException( "Notch-per-motor peak was higher than single-notch peak %fdB > %fdB" % - (peakdb2, peakdb1)) + (esc_peakdb2, esc_peakdb1)) + + # check that the noise is being squashed at all. this needs to be an aggresive check so that failure happens easily + # testing shows this to be -58dB on average + if esc_peakdb2 > -25: + raise NotAchievedException( + "Notch-per-motor had a peak of %fdB there should be none" % esc_peakdb2) # Now do it again for an octacopter self.context_push() @@ -6060,20 +6106,29 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): defaults_filepath=','.join(self.model_defaults_filepath("octa")), model="octa" ) - freq, hover_throttle, peakdb1 = self.hover_and_check_matched_frequency_with_fft(-10, 20, 350, reverse=True) + freq, hover_throttle, peakdb1, psd = \ + self.hover_and_check_matched_frequency_with_fft_and_psd(-10, 50, 320, reverse=True, instance=2) + # find the noise at the motor frequency + 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_HMNCS", 1) self.set_parameter("INS_HNTCH_OPTS", 2) self.reboot_sitl() - freq, hover_throttle, peakdb2 = self.hover_and_check_matched_frequency_with_fft(-15, 20, 350, reverse=True) + freq, hover_throttle, peakdb2, psd = \ + self.hover_and_check_matched_frequency_with_fft_and_psd(-15, 50, 320, reverse=True, instance=2) + # find the noise at the motor frequency + esc_hz = self.get_average_esc_frequency() + esc_peakdb2 = psd["X"][int(esc_hz)] - # notch-per-motor should do better, but check for within 5% - if peakdb2 * 1.05 > peakdb1: + # notch-per-motor will be better at the average ESC frequency + if esc_peakdb2 > esc_peakdb1: raise NotAchievedException( "Notch-per-motor peak was higher than single-notch peak %fdB > %fdB" % - (peakdb2, peakdb1)) + (esc_peakdb2, esc_peakdb1)) + except Exception as e: self.print_exception_caught(e) ex = e @@ -11253,7 +11308,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.MotorVibration, Test(self.DynamicNotches, attempts=4), self.PositionWhenGPSIsZero, - Test(self.DynamicRpmNotches, attempts=4), + self.DynamicRpmNotches, self.PIDNotches, self.RefindGPS, Test(self.GyroFFT, attempts=1, speedup=8),