aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

autotest: add basic FFT post-filter test

Browse Source 
record gyro data in FFT postfilter test
check post-filter gyro logging and change notch settings for post-filter FFT
add motor noise test
FFT SNR now has sensible default for post-filter
This commit is contained in:
Andy Piper 2022-09-22 15:37:05 +01:00 committed by Andrew Tridgell
parent 9eb561639b
commit 60dcb0423a
1 changed files with 193 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

199
Tools/autotest/arducopter.py
View File

@ -5944,6 +5944,191 @@ class AutoTestCopter(AutoTest):
if ex is not None:
raise ex
def GyroFFTPostFilter(self):
"""Use FFT-driven dynamic harmonic notch to control post-RPM filter motor noise."""
# basic gyro sample rate test
self.progress("Flying with gyro FFT post-filter supression - Gyro sample rate")
self.context_push()
ex = None
try:
# This set of parameters creates two noise peaks one at the motor frequency and one at 250Hz
# we then use ESC telemetry to drive the notch to clean up the motor noise and a post-filter
# FFT notch to clean up the remaining 250Hz. If either notch fails then the test will be failed
# due to too much noise being present
self.set_parameters({
"AHRS_EKF_TYPE": 10, # magic tridge EKF type that dramatically speeds up the test
"EK2_ENABLE": 0,
"EK3_ENABLE": 0,
"INS_LOG_BAT_MASK": 3,
"INS_LOG_BAT_OPT": 4,
"INS_GYRO_FILTER": 100,
"INS_FAST_SAMPLE": 3,
"LOG_BITMASK": 958,
"LOG_DISARMED": 0,
"SIM_DRIFT_SPEED": 0,
"SIM_DRIFT_TIME": 0,
"SIM_GYR1_RND": 20, # enable a noisy gyro
"INS_HNTCH_ENABLE": 1,
"INS_HNTCH_FREQ": 80,
"INS_HNTCH_REF": 1.0,
"INS_HNTCH_HMNCS": 1, # first harmonic
"INS_HNTCH_ATT": 50,
"INS_HNTCH_BW": 30,
"INS_HNTCH_MODE": 3, # ESC telemetry
"INS_HNTCH_OPTS": 2, # notch-per-motor
"INS_HNTC2_ENABLE": 1,
"INS_HNTC2_FREQ": 80,
"INS_HNTC2_REF": 1.0,
"INS_HNTC2_HMNCS": 1,
"INS_HNTC2_ATT": 50,
"INS_HNTC2_BW": 40,
"INS_HNTC2_MODE": 4, # in-flight FFT
"INS_HNTC2_OPTS": 18, # triple-notch, notch-per-FFT peak
"FFT_ENABLE": 1,
"FFT_WINDOW_SIZE": 64, # not the default, but makes the test more reliable
"FFT_OPTIONS": 1,
"FFT_MINHZ": 50,
"FFT_MAXHZ": 450,
"SIM_VIB_MOT_MAX": 250, # gives a motor peak at about 145Hz
"SIM_VIB_FREQ_X": 250, # create another peak at 250hz
"SIM_VIB_FREQ_Y": 250,
"SIM_VIB_FREQ_Z": 250,
"SIM_GYR_FILE_RW": 2, # write data to a file
})
self.reboot_sitl()
# do test flight:
self.takeoff(10, mode="ALT_HOLD")
tstart, tend, hover_throttle = self.hover_for_interval(60)
# fly fast forrest!
self.set_rc(3, 1900)
self.set_rc(2, 1200)
self.wait_groundspeed(5, 1000)
self.set_rc(3, 1500)
self.set_rc(2, 1500)
self.do_RTL()
psd = self.mavfft_fttd(1, 2, tstart * 1.0e6, tend * 1.0e6)
# batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin
scale = 1000. / 1024.
sminhz = int(100 * scale)
smaxhz = int(350 * scale)
freq = psd["F"][numpy.argmax(psd["X"][sminhz:smaxhz]) + sminhz]
peakdb = numpy.amax(psd["X"][sminhz:smaxhz])
if peakdb < -5:
self.progress("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb))
else:
raise NotAchievedException("Detected %fHz motor peak at %fdB" % (freq, peakdb))
# prevent update parameters from messing with the settings when we pop the context
self.set_parameters({
"SIM_VIB_FREQ_X": 0,
"SIM_VIB_FREQ_Y": 0,
"SIM_VIB_FREQ_Z": 0,
"SIM_VIB_MOT_MULT": 1.0,
"SIM_GYR_FILE_RW": 0, # stop writing data
"FFT_ENABLE": 0,
})
self.reboot_sitl()
except Exception as e:
self.print_exception_caught(e)
ex = e
self.context_pop()
# need a final reboot because weird things happen to your
# vehicle state when switching back from EKF type 10!
self.reboot_sitl()
if ex is not None:
raise ex
def GyroFFTMotorNoiseCheck(self):
"""Use FFT to detect post-filter motor noise."""
# basic gyro sample rate test
self.progress("Flying with FFT motor-noise detection - Gyro sample rate")
self.context_push()
ex = None
try:
# This set of parameters creates two noise peaks one at the motor frequency and one at 250Hz
# we then use ESC telemetry to drive the notch to clean up the motor noise and a post-filter
# FFT notch to clean up the remaining 250Hz. If either notch fails then the test will be failed
# due to too much noise being present
self.set_parameters({
"AHRS_EKF_TYPE": 10, # magic tridge EKF type that dramatically speeds up the test
"EK2_ENABLE": 0,
"EK3_ENABLE": 0,
"INS_LOG_BAT_MASK": 3,
"INS_LOG_BAT_OPT": 4,
"INS_GYRO_FILTER": 100,
"INS_FAST_SAMPLE": 3,
"LOG_BITMASK": 958,
"LOG_DISARMED": 0,
"SIM_DRIFT_SPEED": 0,
"SIM_DRIFT_TIME": 0,
"SIM_GYR1_RND": 200, # enable a noisy gyro
"INS_HNTCH_ENABLE": 1,
"INS_HNTCH_FREQ": 80,
"INS_HNTCH_REF": 1.0,
"INS_HNTCH_HMNCS": 1, # first harmonic
"INS_HNTCH_ATT": 50,
"INS_HNTCH_BW": 30,
"INS_HNTCH_MODE": 3, # ESC telemetry
"INS_HNTCH_OPTS": 2, # notch-per-motor
"INS_HNTC2_ENABLE": 1,
"INS_HNTC2_FREQ": 80,
"INS_HNTC2_REF": 1.0,
"INS_HNTC2_HMNCS": 1,
"INS_HNTC2_ATT": 50,
"INS_HNTC2_BW": 40,
"INS_HNTC2_MODE": 0, # istatic notch
"INS_HNTC2_OPTS": 16, # triple-notch
"FFT_ENABLE": 1,
"FFT_WINDOW_SIZE": 64, # not the default, but makes the test more reliable
"FFT_OPTIONS": 3,
"FFT_MINHZ": 50,
"FFT_MAXHZ": 450,
"SIM_VIB_MOT_MAX": 250, # gives a motor peak at about 145Hz
"SIM_VIB_FREQ_X": 250, # create another peak at 250hz
"SIM_VIB_FREQ_Y": 250,
"SIM_VIB_FREQ_Z": 250,
"SIM_GYR_FILE_RW": 2, # write data to a file
})
self.reboot_sitl()
# do test flight:
self.takeoff(10, mode="ALT_HOLD")
tstart, tend, hover_throttle = self.hover_for_interval(10)
self.wait_statustext("Noise ", timeout=20)
self.set_parameter("SIM_GYR1_RND", 0) # stop noise so that we can get home
self.do_RTL()
# prevent update parameters from messing with the settings when we pop the context
self.set_parameters({
"SIM_VIB_FREQ_X": 0,
"SIM_VIB_FREQ_Y": 0,
"SIM_VIB_FREQ_Z": 0,
"SIM_VIB_MOT_MULT": 1.0,
"SIM_GYR_FILE_RW": 0, # stop writing data
"FFT_ENABLE": 0,
})
self.reboot_sitl()
except Exception as e:
self.print_exception_caught(e)
ex = e
self.context_pop()
# need a final reboot because weird things happen to your
# vehicle state when switching back from EKF type 10!
self.reboot_sitl()
if ex is not None:
raise ex
def BrakeMode(self):
'''Fly Brake Mode'''
# test brake mode
@ -9294,14 +9479,16 @@ class AutoTestCopter(AutoTest):
'''return list of all tests'''
ret = ([
self.MotorVibration,
Test(self.DynamicNotches, attempts=8),
Test(self.DynamicNotches, attempts=4),
self.PositionWhenGPSIsZero,
Test(self.DynamicRpmNotches, attempts=8),
Test(self.DynamicRpmNotches, attempts=4),
self.RefindGPS,
Test(self.GyroFFT, attempts=8),
Test(self.GyroFFTHarmonic, attempts=16, speedup=8),
self.GyroFFTAverage,
Test(self.GyroFFTContinuousAveraging, attempts=8, speedup=8),
Test(self.GyroFFT, attempts=1, speedup=8),
Test(self.GyroFFTHarmonic, attempts=4, speedup=8),
Test(self.GyroFFTAverage, attempts=1, speedup=8),
Test(self.GyroFFTContinuousAveraging, attempts=4, speedup=8),
self.GyroFFTPostFilter,
self.GyroFFTMotorNoiseCheck,
self.CompassReordering,
self.CRSF,
self.MotorTest,