autotest: add basic FFT post-filter test
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
Andrew Tridgell
parent
9eb561639b
commit
60dcb0423a
@ -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,
|
||||
|
||||
Loading…
Reference in New Issue
Block a user