mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
autotest: tighten DynamicRpmNotches so that failure means failure
check notch attenuation at motor frequency
This commit is contained in:
Andy Piper
Andrew Tridgell
parent
f78eb58fb4
commit
95c06d418e
@ -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),
|
||||
|
||||
Loading…
Reference in New Issue
Block a user