Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

autotest: separate out harmonic matching test

Browse Source 
be more forgiving about harmonic fit detection due to imperfect simulation
use the fft length to determine how accurate the frequency match should be
This commit is contained in:
Andy Piper 2020-02-24 21:56:36 +00:00 committed by Peter Barker
parent f1d6588954
commit 7c1757bab4
1 changed files with 25 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
Tools/autotest/arducopter.py
View File

@ -3661,7 +3661,7 @@ class AutoTestCopter(AutoTest):
self.do_RTL() self.do_RTL()
def hover_and_check_matched_frequency(self, dblevel=-15, minhz=200, maxhz=300, peakhz=None, freqMatch=0.05): def hover_and_check_matched_frequency(self, dblevel=-15, minhz=200, maxhz=300, fftLength=32, peakhz=None):
# find a motor peak # find a motor peak
self.takeoff(10, mode="ALT_HOLD") self.takeoff(10, mode="ALT_HOLD")
@ -3687,6 +3687,8 @@ class AutoTestCopter(AutoTest):
# we have a peak make sure that the FFT detected something close # we have a peak make sure that the FFT detected something close
# logging is at 10Hz # logging is at 10Hz
mlog = self.dfreader_for_current_onboard_log() mlog = self.dfreader_for_current_onboard_log()
# accuracy is determined by sample rate and fft length, given our use of quinn we could probably use half of this
freqDelta = 1000. / fftLength
pkAvg = freq pkAvg = freq
nmessages = 1 nmessages = 1
@ -3702,19 +3704,20 @@ class AutoTestCopter(AutoTest):
self.progress("Detected motor peak at %fHz processing %d messages" % (pkAvg, nmessages)) self.progress("Detected motor peak at %fHz processing %d messages" % (pkAvg, nmessages))
# peak within 5% # peak within 5%
if abs(pkAvg - freq) / freq > freqMatch: if abs(pkAvg - freq) > freqDelta:
raise NotAchievedException("FFT did not detect a motor peak at %f, found %f, wanted %f" % (dblevel, pkAvg, freq)) raise NotAchievedException("FFT did not detect a motor peak at %f, found %f, wanted %f" % (dblevel, pkAvg, freq))
return freq return freq
def fly_gyro_fft_harmonic(self): def fly_gyro_fft_harmonic(self):
"""Use dynamic harmonic notch to control motor noise.""" """Use dynamic harmonic notch to control motor noise with harmonic matching of the first harmonic."""
# basic gyro sample rate test # basic gyro sample rate test
self.progress("Flying with gyro FFT - Gyro sample rate") self.progress("Flying with gyro FFT harmonic - Gyro sample rate")
self.context_push() self.context_push()
ex = None ex = None
try: try:
self.start_subtest("Hover to calculate approximate hover frequency")
self.set_rc_default() self.set_rc_default()
# magic tridge EKF type that dramatically speeds up the test # magic tridge EKF type that dramatically speeds up the test
self.set_parameter("AHRS_EKF_TYPE", 10) self.set_parameter("AHRS_EKF_TYPE", 10)
@ -3735,22 +3738,25 @@ class AutoTestCopter(AutoTest):
self.set_parameter("FFT_MAXHZ", 450) self.set_parameter("FFT_MAXHZ", 450)
self.set_parameter("FFT_SNR_REF", 10) self.set_parameter("FFT_SNR_REF", 10)
# Step 2: inject actual motor noise and use the standard length FFT to track it # Step 1: inject actual motor noise and use the FFT to track it
self.set_parameter("SIM_VIB_MOT_MAX", 250) # gives a motor peak at about 175Hz self.set_parameter("SIM_VIB_MOT_MAX", 250) # gives a motor peak at about 175Hz
self.set_parameter("FFT_WINDOW_SIZE", 32) self.set_parameter("FFT_WINDOW_SIZE", 64)
self.set_parameter("FFT_WINDOW_OLAP", 0.5) self.set_parameter("FFT_WINDOW_OLAP", 0.75)
self.reboot_sitl() self.reboot_sitl()
freq = self.hover_and_check_matched_frequency(-15, 100, 250) freq = self.hover_and_check_matched_frequency(-15, 100, 250, 64)
# Step 2a: add a second harmonic and check the first is still tracked # Step 2: add a second harmonic and check the first is still tracked
self.start_subtest("Add a fixed frequency harmonic at twice the hover frequency and check the right harmonic is found")
self.set_parameter("SIM_VIB_FREQ_X", freq * 2) self.set_parameter("SIM_VIB_FREQ_X", freq * 2)
self.set_parameter("SIM_VIB_FREQ_Y", freq * 2) self.set_parameter("SIM_VIB_FREQ_Y", freq * 2)
self.set_parameter("SIM_VIB_FREQ_Z", freq * 2) self.set_parameter("SIM_VIB_FREQ_Z", freq * 2)
# this is artificially high to cope with the fact that our simulation is imperfect
self.set_parameter("FFT_HMNC_REF", 20.0)
self.set_parameter("SIM_VIB_MOT_MULT", 0.25) # halve the motor noise so that the higher harmonic dominates self.set_parameter("SIM_VIB_MOT_MULT", 0.25) # halve the motor noise so that the higher harmonic dominates
self.reboot_sitl() self.reboot_sitl()
self.hover_and_check_matched_frequency(-15, 100, 250, None, 0.15) self.hover_and_check_matched_frequency(-15, 100, 250, 64, None)
self.set_parameter("SIM_VIB_FREQ_X", 0) self.set_parameter("SIM_VIB_FREQ_X", 0)
self.set_parameter("SIM_VIB_FREQ_Y", 0) self.set_parameter("SIM_VIB_FREQ_Y", 0)
self.set_parameter("SIM_VIB_FREQ_Z", 0) self.set_parameter("SIM_VIB_FREQ_Z", 0)
@ -3760,6 +3766,7 @@ class AutoTestCopter(AutoTest):
self.reboot_sitl() self.reboot_sitl()
except Exception as e: except Exception as e:
self.progress("Exception caught: %s" % (self.get_exception_stacktrace(e)))
ex = e ex = e
self.context_pop() self.context_pop()
@ -3800,6 +3807,7 @@ class AutoTestCopter(AutoTest):
# Step 1: inject a very precise noise peak at 250hz and make sure the in-flight fft # Step 1: inject a very precise noise peak at 250hz and make sure the in-flight fft
# can detect it really accurately. For a 128 FFT the frequency resolution is 8Hz so # can detect it really accurately. For a 128 FFT the frequency resolution is 8Hz so
# a 250Hz peak should be detectable within 5% # a 250Hz peak should be detectable within 5%
self.start_subtest("Inject noise at 250Hz and check the FFT can find the noise")
self.set_parameter("SIM_VIB_FREQ_X", 250) self.set_parameter("SIM_VIB_FREQ_X", 250)
self.set_parameter("SIM_VIB_FREQ_Y", 250) self.set_parameter("SIM_VIB_FREQ_Y", 250)
self.set_parameter("SIM_VIB_FREQ_Z", 250) self.set_parameter("SIM_VIB_FREQ_Z", 250)
@ -3807,9 +3815,10 @@ class AutoTestCopter(AutoTest):
self.reboot_sitl() self.reboot_sitl()
# find a motor peak # find a motor peak
self.hover_and_check_matched_frequency(-15, 100, 350, 250) self.hover_and_check_matched_frequency(-15, 100, 350, 128, 250)
# Step 2: inject actual motor noise and use the standard length FFT to track it # Step 2: inject actual motor noise and use the standard length FFT to track it
self.start_subtest("Hover and check that the FFT can find the motor noise")
self.set_parameter("SIM_VIB_FREQ_X", 0) self.set_parameter("SIM_VIB_FREQ_X", 0)
self.set_parameter("SIM_VIB_FREQ_Y", 0) self.set_parameter("SIM_VIB_FREQ_Y", 0)
self.set_parameter("SIM_VIB_FREQ_Z", 0) self.set_parameter("SIM_VIB_FREQ_Z", 0)
@ -3818,11 +3827,12 @@ class AutoTestCopter(AutoTest):
self.set_parameter("FFT_WINDOW_OLAP", 0.5) self.set_parameter("FFT_WINDOW_OLAP", 0.5)
self.reboot_sitl() self.reboot_sitl()
freq = self.hover_and_check_matched_frequency(-15, 100, 250) freq = self.hover_and_check_matched_frequency(-15, 100, 250, 32)
self.set_parameter("SIM_VIB_MOT_MULT", 1.) self.set_parameter("SIM_VIB_MOT_MULT", 1.)
# Step 3: add a FFT dynamic notch and check that the peak is squashed # Step 3: add a FFT dynamic notch and check that the peak is squashed
self.start_subtest("Add a dynamic notch, hover and check that the noise peak is now gone")
self.set_parameter("INS_LOG_BAT_OPT", 2) self.set_parameter("INS_LOG_BAT_OPT", 2)
self.set_parameter("INS_HNTCH_ENABLE", 1) self.set_parameter("INS_HNTCH_ENABLE", 1)
self.set_parameter("INS_HNTCH_FREQ", freq) self.set_parameter("INS_HNTCH_FREQ", freq)
@ -3852,19 +3862,14 @@ class AutoTestCopter(AutoTest):
psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6) psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)
freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins] freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins]
dblevel = numpy.amax(psd["X"][ignore_bins:]) dblevel = numpy.amax(psd["X"][ignore_bins:])
'''
AUTOTEST: FAILED: "GyroFFT (Fly Gyro FFT)": NotAchievedException('Detected 178.703498Hz motor peak at -9.713545dB',) (see /home/travis/build/ArduPilot/buildlogs/ArduCopter-GyroFFT.txt)
That's pretty close to the -10 threshold; do we need to loosen the threshold?
Andy, 08:28
yeah, if you are seeing test failures it would make sense. The harmonic matching logic is not great right now and I am working on improvements.
'''
if dblevel < -9: if dblevel < -9:
self.progress("Did not detect a motor peak, found %fHz at %fdB" % (freq, dblevel)) self.progress("Did not detect a motor peak, found %fHz at %fdB" % (freq, dblevel))
else: else:
raise NotAchievedException("Detected %fHz motor peak at %fdB" % (freq, dblevel)) raise NotAchievedException("Detected %fHz motor peak at %fdB" % (freq, dblevel))
# Step 4: loop sample rate test with larger window # Step 4: loop sample rate test with larger window
self.progress("Flying with gyro FFT - Fast loop rate") self.start_subtest("Hover and check that the FFT can find the motor noise when running at fast loop rate")
# we are limited to half the loop rate for frequency detection # we are limited to half the loop rate for frequency detection
self.set_parameter("FFT_MAXHZ", 185) self.set_parameter("FFT_MAXHZ", 185)
self.set_parameter("INS_LOG_BAT_OPT", 0) self.set_parameter("INS_LOG_BAT_OPT", 0)
@ -3888,6 +3893,7 @@ yeah, if you are seeing test failures it would make sense. The harmonic matching
self.reboot_sitl() self.reboot_sitl()
except Exception as e: except Exception as e:
self.progress("Exception caught: %s" % (self.get_exception_stacktrace(e)))
ex = e ex = e
self.context_pop() self.context_pop()
@ -4937,7 +4943,6 @@ yeah, if you are seeing test failures it would make sense. The harmonic matching
"Parachute": "See https://github.com/ArduPilot/ardupilot/issues/4702", "Parachute": "See https://github.com/ArduPilot/ardupilot/issues/4702",
"HorizontalAvoidFence": "See https://github.com/ArduPilot/ardupilot/issues/11525", "HorizontalAvoidFence": "See https://github.com/ArduPilot/ardupilot/issues/11525",
"BeaconPosition": "See https://github.com/ArduPilot/ardupilot/issues/11689", "BeaconPosition": "See https://github.com/ArduPilot/ardupilot/issues/11689",
"GyroFFTHarmonic": "See https://github.com/ArduPilot/ardupilot/issues/13736",
} }
class AutoTestHeli(AutoTestCopter): class AutoTestHeli(AutoTestCopter):