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

autotest: test 256 FT windows and fix calculation of log-based FFT

Browse Source 
fix quadplane FFT reference calculation
re-enable harmonic test
use median for measuring in-flight FFT average as it's much more reliable
relax quadplane filter restriction
This commit is contained in:
Andy Piper 2020-03-13 16:13:54 +00:00 committed by Peter Barker
parent 1c7354c321
commit db4a612c13
2 changed files with 32 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
Tools/autotest/arducopter.py
View File

@ -3759,8 +3759,11 @@ class AutoTestCopter(AutoTest):
psd = self.mavfft_fttd(1, 0, 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.)
peakdb = numpy.amax(psd["X"][minhz:maxhz])
scale = 1000. / 1024.
sminhz = int(minhz * scale)
smaxhz = int(maxhz * scale)
freq = psd["F"][numpy.argmax(psd["X"][sminhz:smaxhz]) + sminhz]
peakdb = numpy.amax(psd["X"][sminhz:smaxhz])
if peakdb < dblevel or (peakhz is not None and abs(freq - peakhz) / peakhz > 0.05):
raise NotAchievedException("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb))
else:
@ -3776,13 +3779,15 @@ class AutoTestCopter(AutoTest):
m = mlog.recv_match(type='FTN1', blocking=False,
condition="FTN1.TimeUS>%u and FTN1.TimeUS<%u" % (tstart * 1.0e6, tend * 1.0e6))
freqs = []
while m is not None:
nmessages = nmessages + 1
pkAvg = pkAvg + (0.1 * (m.PkAvg - pkAvg))
freqs.append(m.PkAvg)
m = mlog.recv_match(type='FTN1', blocking=False,
condition="FTN1.TimeUS>%u and FTN1.TimeUS<%u" % (tstart * 1.0e6, tend * 1.0e6))
# peak within resolution of FFT length
pkAvg = numpy.median(numpy.asarray(freqs))
self.progress("Detected motor peak at %fHz processing %d messages" % (pkAvg, nmessages))
# peak within 5%
@ -3812,6 +3817,7 @@ class AutoTestCopter(AutoTest):
self.set_parameter("LOG_DISARMED", 0)
self.set_parameter("SIM_DRIFT_SPEED", 0)
self.set_parameter("SIM_DRIFT_TIME", 0)
self.set_parameter("FFT_THR_REF", self.get_parameter("MOT_THST_HOVER"))
# enable a noisy motor peak
self.set_parameter("SIM_GYR_RND", 20)
# enabling FFT will also enable the arming check, self-testing the functionality
@ -3833,12 +3839,11 @@ class AutoTestCopter(AutoTest):
self.set_parameter("SIM_VIB_FREQ_X", freq * 2)
self.set_parameter("SIM_VIB_FREQ_Y", 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.reboot_sitl()
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_Y", 0)
self.set_parameter("SIM_VIB_FREQ_Z", 0)
@ -3899,6 +3904,17 @@ class AutoTestCopter(AutoTest):
# find a motor peak
self.hover_and_check_matched_frequency(-15, 100, 350, 128, 250)
# Step 1b: run the same test with an FFT length of 256 which is needed to flush out a
# whole host of bugs related to uint8_t. This also tests very accurately the frequency resolution
self.set_parameter("FFT_WINDOW_SIZE", 256)
self.start_subtest("Inject noise at 250Hz and check the FFT can find the noise")
self.reboot_sitl()
# find a motor peak
self.hover_and_check_matched_frequency(-15, 100, 350, 256, 250)
self.set_parameter("FFT_WINDOW_SIZE", 128)
# 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)
@ -5071,7 +5087,6 @@ class AutoTestCopter(AutoTest):
"Parachute": "See https://github.com/ArduPilot/ardupilot/issues/4702",
"HorizontalAvoidFence": "See https://github.com/ArduPilot/ardupilot/issues/11525",
"BeaconPosition": "See https://github.com/ArduPilot/ardupilot/issues/11689",
"GyroFFTHarmonic": "See https://github.com/ArduPilot/ardupilot/issues/13736",
}
class AutoTestHeli(AutoTestCopter):

14
Tools/autotest/quadplane.py
View File

@ -249,8 +249,11 @@ class AutoTestQuadPlane(AutoTest):
psd = self.mavfft_fttd(1, 0, 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.)
peakdb = numpy.amax(psd["X"][minhz:maxhz])
scale = 1000. / 1024.
sminhz = int(minhz * scale)
smaxhz = int(maxhz * scale)
freq = psd["F"][numpy.argmax(psd["X"][sminhz:smaxhz]) + sminhz]
peakdb = numpy.amax(psd["X"][sminhz:smaxhz])
if peakdb < dblevel or (peakhz is not None and abs(freq - peakhz) / peakhz > 0.05):
raise NotAchievedException("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb))
else:
@ -262,6 +265,7 @@ class AutoTestQuadPlane(AutoTest):
# 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
freqs = []
while True:
m = mlog.recv_match(
@ -270,8 +274,10 @@ class AutoTestQuadPlane(AutoTest):
condition="FTN1.TimeUS>%u and FTN1.TimeUS<%u" % (tstart * 1.0e6, tend * 1.0e6))
if m is None:
break
pkAvg = pkAvg + (0.1 * (m.PkAvg - pkAvg))
freqs.append(m.PkAvg)
# peak within resolution of FFT length
pkAvg = numpy.median(numpy.asarray(freqs))
if abs(pkAvg - freq) > freqDelta:
raise NotAchievedException("FFT did not detect a motor peak at %f, found %f, wanted %f" % (dblevel, pkAvg, freq))
@ -350,7 +356,7 @@ class AutoTestQuadPlane(AutoTest):
self.do_RTL()
psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)
freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins]
if numpy.amax(psd["X"][ignore_bins:]) < -15:
if numpy.amax(psd["X"][ignore_bins:]) < -10:
self.progress("Did not detect a motor peak, found %f at %f dB" % (freq, numpy.amax(psd["X"][ignore_bins:])))
else:
raise NotAchievedException("Detected motor peak at %f Hz" % (freq))