Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

autotest: factor out extract_median_FTN1_PkAvg_from_current_onboard_log

This commit is contained in:
Peter Barker 2022-06-13 14:15:56 +10:00 committed by Peter Barker
parent d6dc0464dd
commit abc1b7b644
1 changed files with 19 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
Tools/autotest/arducopter.py
View File

@ -5249,23 +5249,12 @@ class AutoTestCopter(AutoTest):
# we have a peak make sure that the FFT detected something close
# logging is at 10Hz
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
freqs = []
while True:
m = mlog.recv_match(
type='FTN1',
blocking=False,
condition="FTN1.TimeUS>%u and FTN1.TimeUS<%u" % (tstart * 1.0e6, tend * 1.0e6)
)
if m is None:
break
freqs.append(m.PkAvg)
# peak within resolution of FFT length
pkAvg = numpy.median(numpy.asarray(freqs))
self.progress("Detected motor peak at %fHz processing %d messages" % (pkAvg, nmessages))
pkAvg, nmessages = self.extract_median_FTN1_PkAvg_from_current_onboard_log(tstart, tend)
self.progress("Onboard-FFT detected motor peak at %fHz (processed %d FTN1 messages)" % (pkAvg, nmessages))
# peak within 5%
if abs(pkAvg - freq) > freqDelta:
@ -5273,6 +5262,21 @@ class AutoTestCopter(AutoTest):
return freq
def extract_median_FTN1_PkAvg_from_current_onboard_log(self, tstart, tend):
'''extracts FTN1 messages from log, returns median of pkAvg values and
the number of samples'''
mlog = self.dfreader_for_current_onboard_log()
freqs = []
while True:
m = mlog.recv_match(
type='FTN1',
blocking=False,
condition="FTN1.TimeUS>%u and FTN1.TimeUS<%u" % (tstart * 1.0e6, tend * 1.0e6))
if m is None:
break
freqs.append(m.PkAvg)
return numpy.median(numpy.asarray(freqs)), len(freqs)
def test_gyro_fft_harmonic(self, averaging):
"""Use dynamic harmonic notch to control motor noise with harmonic matching of the first harmonic."""
# basic gyro sample rate test
@ -5350,19 +5354,9 @@ class AutoTestCopter(AutoTest):
self.do_RTL()
mlog = self.dfreader_for_current_onboard_log()
freqs = []
while True:
m = mlog.recv_match(
type='FTN1',
blocking=False,
condition="FTN1.TimeUS>%u and FTN1.TimeUS<%u" % (tstart * 1.0e6, tend * 1.0e6))
if m is None:
break
freqs.append(m.PkAvg)
# peak within resolution of FFT length, the highest energy peak switched but our detection should not
pkAvg = numpy.median(numpy.asarray(freqs))
pkAvg, nmessages = self.extract_median_FTN1_PkAvg_from_current_onboard_log(tstart, tend)
freqDelta = 1000. / self.get_parameter("FFT_WINDOW_SIZE")
if abs(pkAvg - freq) > freqDelta: