zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

autotest: remove retry loop from within GyroFFTHarmonic

zr-v5.1
Peter Barker 4 years ago committed by Peter Barker
parent
4928ec18af
commit
57abaf0dec
1 changed files with 113 additions and 108 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 221
      Tools/autotest/arducopter.py

221
Tools/autotest/arducopter.py
Unescape View File

@ -4477,131 +4477,136 @@ class AutoTestCopter(AutoTest):
self.context_push() self.context_push()
ex = None ex = None
# we are dealing with probabalistic scenarios involving threads, have two bites at the cherry # we are dealing with probabalistic scenarios involving threads, have two bites at the cherry
for loop in ["first", "second"]: try:
try: self.start_subtest("Hover to calculate approximate hover frequency")
self.start_subtest("Hover to calculate approximate hover frequency") # magic tridge EKF type that dramatically speeds up the test
self.set_rc_default() self.set_parameters({
# magic tridge EKF type that dramatically speeds up the test "AHRS_EKF_TYPE": 10,
self.set_parameter("AHRS_EKF_TYPE", 10) "EK2_ENABLE": 0,
self.set_parameter("EK2_ENABLE", 0) "EK3_ENABLE": 0,
self.set_parameter("EK3_ENABLE", 0) "INS_LOG_BAT_MASK": 3,
self.set_parameter("INS_LOG_BAT_MASK", 3) "INS_LOG_BAT_OPT": 0,
self.set_parameter("INS_LOG_BAT_OPT", 0) "INS_GYRO_FILTER": 100,
self.set_parameter("INS_GYRO_FILTER", 100) "INS_FAST_SAMPLE": 0,
self.set_parameter("INS_FAST_SAMPLE", 0) "LOG_BITMASK": 958,
self.set_parameter("LOG_BITMASK", 958) "LOG_DISARMED": 0,
self.set_parameter("LOG_DISARMED", 0) "SIM_DRIFT_SPEED": 0,
self.set_parameter("SIM_DRIFT_SPEED", 0) "SIM_DRIFT_TIME": 0,
self.set_parameter("SIM_DRIFT_TIME", 0) "FFT_THR_REF": self.get_parameter("MOT_THST_HOVER"),
self.set_parameter("FFT_THR_REF", self.get_parameter("MOT_THST_HOVER")) "SIM_GYR1_RND": 20, # enable a noisy gyro
# enable a noisy motor peak })
self.set_parameter("SIM_GYR1_RND", 20) # motor peak enabling FFT will also enable the arming
# enabling FFT will also enable the arming check, self-testing the functionality # check, self-testing the functionality
self.set_parameter("FFT_ENABLE", 1) self.set_parameters({
self.set_parameter("FFT_MINHZ", 50) "FFT_ENABLE": 1,
self.set_parameter("FFT_MAXHZ", 450) "FFT_MINHZ": 50,
self.set_parameter("FFT_SNR_REF", 10) "FFT_MAXHZ": 450,
"FFT_SNR_REF": 10,
})
# Step 1: inject actual motor noise and use the 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_parameters({
self.set_parameter("FFT_WINDOW_SIZE", 64) "SIM_VIB_MOT_MAX": 250, # gives a motor peak at about 175Hz
self.set_parameter("FFT_WINDOW_OLAP", 0.75) "FFT_WINDOW_SIZE": 64,
"FFT_WINDOW_OLAP": 0.75,
})
self.reboot_sitl() self.reboot_sitl()
freq = self.hover_and_check_matched_frequency(-15, 100, 250, 64) freq = self.hover_and_check_matched_frequency(-15, 100, 250, 64)
# 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_Y", freq * 2)
self.set_parameter("SIM_VIB_FREQ_Z", freq * 2)
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) # 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_parameters({
"SIM_VIB_FREQ_X": freq * 2,
"SIM_VIB_FREQ_Y": freq * 2,
"SIM_VIB_FREQ_Z": freq * 2,
"SIM_VIB_MOT_MULT": 0.25, # halve the motor noise so that the higher harmonic dominates
})
self.reboot_sitl()
# Step 3: switch harmonics mid flight and check for tracking self.hover_and_check_matched_frequency(-15, 100, 250, 64, None)
self.start_subtest("Switch harmonics mid flight and check the right harmonic is found")
self.set_parameter("FFT_HMNC_PEAK", 0)
self.reboot_sitl()
self.takeoff(10, mode="ALT_HOLD") # Step 3: switch harmonics mid flight and check for tracking
self.start_subtest("Switch harmonics mid flight and check the right harmonic is found")
self.set_parameter("FFT_HMNC_PEAK", 0)
self.reboot_sitl()
hover_time = 10 self.takeoff(10, mode="ALT_HOLD")
tstart = self.get_sim_time()
self.progress("Hovering for %u seconds" % hover_time)
while self.get_sim_time_cached() < tstart + hover_time:
self.mav.recv_match(type='ATTITUDE', blocking=True)
vfr_hud = self.mav.recv_match(type='VFR_HUD', blocking=True)
self.set_parameter("SIM_VIB_MOT_MULT", 5.0) hover_time = 10
tstart = self.get_sim_time()
self.progress("Hovering for %u seconds" % hover_time)
while self.get_sim_time_cached() < tstart + hover_time:
self.mav.recv_match(type='ATTITUDE', blocking=True)
vfr_hud = self.mav.recv_match(type='VFR_HUD', blocking=True)
self.progress("Hovering for %u seconds" % hover_time) self.set_parameter("SIM_VIB_MOT_MULT", 5.0)
while self.get_sim_time_cached() < tstart + hover_time:
self.mav.recv_match(type='ATTITUDE', blocking=True)
vfr_hud = self.mav.recv_match(type='VFR_HUD', blocking=True)
tend = self.get_sim_time()
self.do_RTL() self.progress("Hovering for %u seconds" % hover_time)
while self.get_sim_time_cached() < tstart + hover_time:
self.mav.recv_match(type='ATTITUDE', blocking=True)
vfr_hud = self.mav.recv_match(type='VFR_HUD', blocking=True)
tend = self.get_sim_time()
self.do_RTL()
mlog = self.dfreader_for_current_onboard_log() mlog = self.dfreader_for_current_onboard_log()
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:
freqs.append(m.PkAvg)
m = mlog.recv_match( m = mlog.recv_match(
type='FTN1', type='FTN1',
blocking=False, blocking=False,
condition="FTN1.TimeUS>%u and FTN1.TimeUS<%u" % (tstart * 1.0e6, tend * 1.0e6)) condition="FTN1.TimeUS>%u and FTN1.TimeUS<%u" % (tstart * 1.0e6, tend * 1.0e6))
freqs = []
while m is not None:
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, the highest energy peak switched but our detection should not
pkAvg = numpy.median(numpy.asarray(freqs))
freqDelta = 1000. / self.get_parameter("FFT_WINDOW_SIZE")
if abs(pkAvg - freq) > freqDelta:
raise NotAchievedException("FFT did not detect a harmonic motor peak, found %f, wanted %f" % (pkAvg, freq))
# Step 4: dynamic harmonic
self.start_subtest("Enable dynamic harmonics and make sure both frequency peaks are attenuated")
# find a motor peak
freq, vfr_hud, peakdb = self.hover_and_check_matched_frequency_with_fft(-15, 100, 350)
# now add a dynamic notch and check that the peak is squashed # peak within resolution of FFT length, the highest energy peak switched but our detection should not
self.set_parameter("INS_LOG_BAT_OPT", 2) pkAvg = numpy.median(numpy.asarray(freqs))
self.set_parameter("INS_HNTCH_ENABLE", 1) freqDelta = 1000. / self.get_parameter("FFT_WINDOW_SIZE")
self.set_parameter("INS_HNTCH_HMNCS", 3)
self.set_parameter("INS_HNTCH_MODE", 4)
self.set_parameter("INS_HNTCH_FREQ", freq)
# self.set_parameter("INS_HNTCH_REF", 1.0)
self.set_parameter("INS_HNTCH_REF", vfr_hud.throttle/100.)
self.set_parameter("INS_HNTCH_ATT", 100)
self.set_parameter("INS_HNTCH_BW", freq/2)
self.set_parameter("INS_HNTCH_OPTS", 3)
self.reboot_sitl()
# 5db is far in excess of the attenuation that the double dynamic-harmonic notch is able if abs(pkAvg - freq) > freqDelta:
# to provide (-7dB on average), but without the notch the peak is around 20dB so still a safe test raise NotAchievedException("FFT did not detect a harmonic motor peak, found %f, wanted %f" % (pkAvg, freq))
self.hover_and_check_matched_frequency_with_fft(5, 100, 350, reverse=True)
self.set_parameter("SIM_VIB_FREQ_X", 0) # Step 4: dynamic harmonic
self.set_parameter("SIM_VIB_FREQ_Y", 0) self.start_subtest("Enable dynamic harmonics and make sure both frequency peaks are attenuated")
self.set_parameter("SIM_VIB_FREQ_Z", 0) # find a motor peak
self.set_parameter("SIM_VIB_MOT_MULT", 1.) freq, vfr_hud, peakdb = self.hover_and_check_matched_frequency_with_fft(-15, 100, 350)
# prevent update parameters from messing with the settings when we pop the context
self.set_parameter("FFT_ENABLE", 0)
self.reboot_sitl()
except Exception as e: # now add a dynamic notch and check that the peak is squashed
self.print_exception_caught(e) self.set_parameters({
self.progress("Exception caught in %s loop" % (loop, )) "INS_LOG_BAT_OPT": 2,
if loop != "second": "INS_HNTCH_ENABLE": 1,
continue "INS_HNTCH_HMNCS": 3,
ex = e "INS_HNTCH_MODE": 4,
break "INS_HNTCH_FREQ": freq,
"INS_HNTCH_REF": vfr_hud.throttle/100.0,
"INS_HNTCH_ATT": 100,
"INS_HNTCH_BW": freq/2,
"INS_HNTCH_OPTS": 3,
})
self.reboot_sitl()
# 5db is far in excess of the attenuation that the double dynamic-harmonic notch is able
# to provide (-7dB on average), but without the notch the peak is around 20dB so still a safe test
self.hover_and_check_matched_frequency_with_fft(5, 100, 350, reverse=True)
self.set_parameters({
"SIM_VIB_FREQ_X": 0,
"SIM_VIB_FREQ_Y": 0,
"SIM_VIB_FREQ_Z": 0,
"SIM_VIB_MOT_MULT": 1.0,
})
# prevent update parameters from messing with the settings when we pop the context
self.set_parameter("FFT_ENABLE", 0)
self.reboot_sitl()
except Exception as e:
self.print_exception_caught(e)
ex = e
self.context_pop() self.context_pop()
@ -6957,7 +6962,7 @@ class AutoTestCopter(AutoTest):
Test("GyroFFTHarmonic", Test("GyroFFTHarmonic",
"Fly Gyro FFT Harmonic Matching", "Fly Gyro FFT Harmonic Matching",
self.fly_gyro_fft_harmonic, self.fly_gyro_fft_harmonic,
attempts=4), attempts=8),
Test("CompassReordering", Test("CompassReordering",
"Test Compass reordering when priorities are changed", "Test Compass reordering when priorities are changed",

Write Preview
Loading…
Cancel
Save