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

autotest: return hover throttle from hover_for_interval

Browse Source
This commit is contained in:
Peter Barker 2022-06-13 12:56:37 +10:00 committed by Peter Barker
parent ef29350a72
commit 1a9470d635
1 changed files with 46 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
Tools/autotest/arducopter.py
View File

@ -2629,13 +2629,15 @@ class AutoTestCopter(AutoTest):
def hover_for_interval(self, hover_time): def hover_for_interval(self, hover_time):
'''hovers for an interval of hover_time seconds. Returns the bookend '''hovers for an interval of hover_time seconds. Returns the bookend
times for that interval times for that interval (in time-since-boot frame), and the
output throttle level at the end of the period.
''' '''
self.progress("Hovering for %u seconds" % hover_time) self.progress("Hovering for %u seconds" % hover_time)
tstart = self.get_sim_time() tstart = self.get_sim_time()
self.delay_sim_time(hover_time) self.delay_sim_time(hover_time)
vfr_hud = self.poll_message('VFR_HUD')
tend = self.get_sim_time() tend = self.get_sim_time()
return tstart, tend return tstart, tend, vfr_hud.throttle
def fly_motor_vibration(self): def fly_motor_vibration(self):
"""Test flight with motor vibration""" """Test flight with motor vibration"""
@ -2660,15 +2662,14 @@ class AutoTestCopter(AutoTest):
}) })
self.reboot_sitl() self.reboot_sitl()
# do a simple up-and-down flight to gather data:
self.takeoff(15, mode="ALT_HOLD") self.takeoff(15, mode="ALT_HOLD")
tstart, tend, hover_throttle = self.hover_for_interval(15)
hover_time = 15
tstart, tend = self.hover_for_interval(hover_time)
# if we don't reduce vibes here then the landing detector # if we don't reduce vibes here then the landing detector
# may not trigger # may not trigger
self.set_parameter("SIM_VIB_MOT_MAX", 0) self.set_parameter("SIM_VIB_MOT_MAX", 0)
self.do_RTL() self.do_RTL()
psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6) psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)
# ignore the first 20Hz and look for a peak at -15dB or more # ignore the first 20Hz and look for a peak at -15dB or more
ignore_bins = 20 ignore_bins = 20
@ -2692,12 +2693,12 @@ class AutoTestCopter(AutoTest):
}) })
self.reboot_sitl() self.reboot_sitl()
# do a simple up-and-down flight to gather data:
self.takeoff(15, mode="ALT_HOLD") self.takeoff(15, mode="ALT_HOLD")
tstart, tend, hover_throttle = self.hover_for_interval(15)
tstart, tend = self.hover_for_interval(hover_time)
self.set_parameter("SIM_VIB_MOT_MAX", 0) self.set_parameter("SIM_VIB_MOT_MAX", 0)
self.do_RTL() self.do_RTL()
psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6) psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)
freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins] freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins]
peakdB = numpy.amax(psd["X"][ignore_bins:]) peakdB = numpy.amax(psd["X"][ignore_bins:])
@ -5059,11 +5060,9 @@ class AutoTestCopter(AutoTest):
if takeoff: if takeoff:
self.takeoff(10, mode="ALT_HOLD") self.takeoff(10, mode="ALT_HOLD")
hover_time = 15 tstart, tend, hover_throttle = self.hover_for_interval(15)
tstart, tend = self.hover_for_interval(hover_time)
vfr_hud = self.mav.recv_match(type='VFR_HUD', blocking=True)
self.do_RTL() self.do_RTL()
psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6) 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 # batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin
@ -5078,11 +5077,12 @@ class AutoTestCopter(AutoTest):
if reverse is not None: if reverse is not None:
raise NotAchievedException( raise NotAchievedException(
"Detected motor peak at %fHz, throttle %f%%, %fdB" % "Detected motor peak at %fHz, throttle %f%%, %fdB" %
(freq, vfr_hud.throttle, peakdb)) (freq, hover_throttle, peakdb))
else: else:
self.progress("Detected motor peak at %fHz, throttle %f%%, %fdB" % (freq, vfr_hud.throttle, peakdb)) self.progress("Detected motor peak at %fHz, throttle %f%%, %fdB" %
(freq, hover_throttle, peakdb))
return freq, vfr_hud, peakdb return freq, hover_throttle, peakdb
def fly_dynamic_notches(self): def fly_dynamic_notches(self):
"""Use dynamic harmonic notch to control motor noise.""" """Use dynamic harmonic notch to control motor noise."""
@ -5106,27 +5106,27 @@ class AutoTestCopter(AutoTest):
self.takeoff(10, mode="ALT_HOLD") self.takeoff(10, mode="ALT_HOLD")
# find a motor peak # find a motor peak
freq, vfr_hud, peakdb = self.hover_and_check_matched_frequency_with_fft(-15, 200, 300) freq, hover_throttle, peakdb = self.hover_and_check_matched_frequency_with_fft(-15, 200, 300)
# now add a dynamic notch and check that the peak is squashed # now add a dynamic notch and check that the peak is squashed
self.set_parameters({ self.set_parameters({
"INS_LOG_BAT_OPT": 2, "INS_LOG_BAT_OPT": 2,
"INS_HNTCH_ENABLE": 1, "INS_HNTCH_ENABLE": 1,
"INS_HNTCH_FREQ": freq, "INS_HNTCH_FREQ": freq,
"INS_HNTCH_REF": vfr_hud.throttle/100., "INS_HNTCH_REF": hover_throttle/100.,
"INS_HNTCH_HMNCS": 5, # first and third harmonic "INS_HNTCH_HMNCS": 5, # first and third harmonic
"INS_HNTCH_ATT": 50, "INS_HNTCH_ATT": 50,
"INS_HNTCH_BW": freq/2, "INS_HNTCH_BW": freq/2,
}) })
self.reboot_sitl() self.reboot_sitl()
freq, vfr_hud, peakdb1 = self.hover_and_check_matched_frequency_with_fft(-10, 20, 350, reverse=True) freq, hover_throttle, peakdb1 = self.hover_and_check_matched_frequency_with_fft(-10, 20, 350, reverse=True)
# now add double dynamic notches and check that the peak is squashed # now add double dynamic notches and check that the peak is squashed
self.set_parameter("INS_HNTCH_OPTS", 1) self.set_parameter("INS_HNTCH_OPTS", 1)
self.reboot_sitl() self.reboot_sitl()
freq, vfr_hud, peakdb2 = self.hover_and_check_matched_frequency_with_fft(-15, 20, 350, reverse=True) freq, hover_throttle, peakdb2 = self.hover_and_check_matched_frequency_with_fft(-15, 20, 350, reverse=True)
# double-notch should do better, but check for within 5% # double-notch should do better, but check for within 5%
if peakdb2 * 1.05 > peakdb1: if peakdb2 * 1.05 > peakdb1:
@ -5164,7 +5164,7 @@ class AutoTestCopter(AutoTest):
self.takeoff(10, mode="ALT_HOLD") self.takeoff(10, mode="ALT_HOLD")
# find a motor peak # find a motor peak
freq, vfr_hud, peakdb = self.hover_and_check_matched_frequency_with_fft(-15, 200, 300) freq, hover_throttle, peakdb = self.hover_and_check_matched_frequency_with_fft(-15, 200, 300)
# now add a dynamic notch and check that the peak is squashed # now add a dynamic notch and check that the peak is squashed
self.set_parameters({ self.set_parameters({
@ -5179,13 +5179,13 @@ class AutoTestCopter(AutoTest):
}) })
self.reboot_sitl() self.reboot_sitl()
freq, vfr_hud, peakdb1 = self.hover_and_check_matched_frequency_with_fft(-10, 20, 350, reverse=True) freq, hover_throttle, peakdb1 = self.hover_and_check_matched_frequency_with_fft(-10, 20, 350, reverse=True)
# now add notch-per motor and check that the peak is squashed # now add notch-per motor and check that the peak is squashed
self.set_parameter("INS_HNTCH_OPTS", 2) self.set_parameter("INS_HNTCH_OPTS", 2)
self.reboot_sitl() self.reboot_sitl()
freq, vfr_hud, peakdb2 = self.hover_and_check_matched_frequency_with_fft(-15, 20, 350, reverse=True) freq, hover_throttle, peakdb2 = self.hover_and_check_matched_frequency_with_fft(-15, 20, 350, reverse=True)
# notch-per-motor should do better, but check for within 5% # notch-per-motor should do better, but check for within 5%
if peakdb2 * 1.05 > peakdb1: if peakdb2 * 1.05 > peakdb1:
@ -5204,14 +5204,14 @@ class AutoTestCopter(AutoTest):
defaults_filepath=','.join(self.model_defaults_filepath("octa")), defaults_filepath=','.join(self.model_defaults_filepath("octa")),
model="octa" model="octa"
) )
freq, vfr_hud, peakdb1 = self.hover_and_check_matched_frequency_with_fft(-10, 20, 350, reverse=True) freq, hover_throttle, peakdb1 = self.hover_and_check_matched_frequency_with_fft(-10, 20, 350, reverse=True)
# now add notch-per motor and check that the peak is squashed # now add notch-per motor and check that the peak is squashed
self.set_parameter("INS_HNTCH_HMNCS", 1) self.set_parameter("INS_HNTCH_HMNCS", 1)
self.set_parameter("INS_HNTCH_OPTS", 2) self.set_parameter("INS_HNTCH_OPTS", 2)
self.reboot_sitl() self.reboot_sitl()
freq, vfr_hud, peakdb2 = self.hover_and_check_matched_frequency_with_fft(-15, 20, 350, reverse=True) freq, hover_throttle, peakdb2 = self.hover_and_check_matched_frequency_with_fft(-15, 20, 350, reverse=True)
# notch-per-motor should do better, but check for within 5% # notch-per-motor should do better, but check for within 5%
if peakdb2 * 1.05 > peakdb1: if peakdb2 * 1.05 > peakdb1:
@ -5229,13 +5229,9 @@ class AutoTestCopter(AutoTest):
def hover_and_check_matched_frequency(self, dblevel=-15, minhz=200, maxhz=300, fftLength=32, peakhz=None): def hover_and_check_matched_frequency(self, dblevel=-15, minhz=200, maxhz=300, fftLength=32, peakhz=None):
# find a motor peak # find a motor peak
self.takeoff(10, mode="ALT_HOLD") self.takeoff(10, mode="ALT_HOLD")
tstart, tend, hover_throttle = self.hover_for_interval(15)
hover_time = 15
tstart, tend = self.hover_for_interval(hover_time)
vfr_hud = self.mav.recv_match(type='VFR_HUD', blocking=True)
self.do_RTL() self.do_RTL()
psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6) 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 # batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin
@ -5249,7 +5245,7 @@ class AutoTestCopter(AutoTest):
elif peakhz is not None and abs(freq - peakhz) / peakhz > 0.05: elif peakhz is not None and abs(freq - peakhz) / peakhz > 0.05:
raise NotAchievedException("Did not detect a motor peak at %fHz, found %fHz at %fdB" % (peakhz, freq, peakdb)) raise NotAchievedException("Did not detect a motor peak at %fHz, found %fHz at %fdB" % (peakhz, freq, peakdb))
else: else:
self.progress("Detected motor peak at %fHz, throttle %f%%, %fdB" % (freq, vfr_hud.throttle, peakdb)) self.progress("Detected motor peak at %fHz, throttle %f%%, %fdB" % (freq, hover_throttle, peakdb))
# we have a peak make sure that the FFT detected something close # we have a peak make sure that the FFT detected something close
# logging is at 10Hz # logging is at 10Hz
@ -5352,12 +5348,12 @@ class AutoTestCopter(AutoTest):
self.takeoff(10, mode="ALT_HOLD") self.takeoff(10, mode="ALT_HOLD")
hover_time = 10 hover_time = 10
tstart, tend_unused = self.hover_for_interval(hover_time) tstart, tend_unused, hover_throttle = self.hover_for_interval(hover_time)
self.progress("Switching motor vibration multiplier") self.progress("Switching motor vibration multiplier")
self.set_parameter("SIM_VIB_MOT_MULT", 5.0) self.set_parameter("SIM_VIB_MOT_MULT", 5.0)
tstart_unused, tend = self.hover_for_interval(hover_time) tstart_unused, tend, hover_throttle = self.hover_for_interval(hover_time)
self.do_RTL() self.do_RTL()
@ -5384,7 +5380,7 @@ class AutoTestCopter(AutoTest):
# Step 4: dynamic harmonic # Step 4: dynamic harmonic
self.start_subtest("Enable dynamic harmonics and make sure both frequency peaks are attenuated") self.start_subtest("Enable dynamic harmonics and make sure both frequency peaks are attenuated")
# find a motor peak # find a motor peak
freq, vfr_hud, peakdb = self.hover_and_check_matched_frequency_with_fft(-15, 100, 350) freq, hover_throttle, peakdb = self.hover_and_check_matched_frequency_with_fft(-15, 100, 350)
# now add a dynamic notch and check that the peak is squashed # now add a dynamic notch and check that the peak is squashed
self.set_parameters({ self.set_parameters({
@ -5393,7 +5389,7 @@ class AutoTestCopter(AutoTest):
"INS_HNTCH_HMNCS": 1, "INS_HNTCH_HMNCS": 1,
"INS_HNTCH_MODE": 4, "INS_HNTCH_MODE": 4,
"INS_HNTCH_FREQ": freq, "INS_HNTCH_FREQ": freq,
"INS_HNTCH_REF": vfr_hud.throttle/100.0, "INS_HNTCH_REF": hover_throttle/100.0,
"INS_HNTCH_ATT": 100, "INS_HNTCH_ATT": 100,
"INS_HNTCH_BW": freq/2, "INS_HNTCH_BW": freq/2,
"INS_HNTCH_OPTS": 3, "INS_HNTCH_OPTS": 3,
@ -5526,18 +5522,17 @@ class AutoTestCopter(AutoTest):
}) })
self.reboot_sitl() self.reboot_sitl()
# do test flight:
self.takeoff(10, mode="ALT_HOLD") self.takeoff(10, mode="ALT_HOLD")
hover_time = 15 tstart, tend, hover_throttle = self.hover_for_interval(15)
tstart, tend = self.hover_for_interval(hover_time)
# fly fast forrest! # fly fast forrest!
self.set_rc(3, 1900) self.set_rc(3, 1900)
self.set_rc(2, 1200) self.set_rc(2, 1200)
self.wait_groundspeed(5, 1000) self.wait_groundspeed(5, 1000)
self.set_rc(3, 1500) self.set_rc(3, 1500)
self.set_rc(2, 1500) self.set_rc(2, 1500)
self.do_RTL() self.do_RTL()
psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6) 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 # batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin
@ -5564,12 +5559,15 @@ class AutoTestCopter(AutoTest):
}) })
self.reboot_sitl() self.reboot_sitl()
# do test flight:
self.takeoff(10, mode="ALT_HOLD") self.takeoff(10, mode="ALT_HOLD")
tstart, tend, hover_throttle = self.hover_for_interval(15)
tstart, tend = self.hover_for_interval(hover_time)
self.do_RTL() self.do_RTL()
# prevent update parameters from messing with the settings when we pop the context
# why are we not checking the results from that flight? -pb20220613
# prevent update parameters from messing with the settings
# when we pop the context
self.set_parameter("FFT_ENABLE", 0) self.set_parameter("FFT_ENABLE", 0)
self.reboot_sitl() self.reboot_sitl()
@ -5634,9 +5632,7 @@ class AutoTestCopter(AutoTest):
# start the tune # start the tune
self.set_rc(7, 2000) self.set_rc(7, 2000)
tstart, tend = self.hover_for_interval(hover_time) tstart, tend, hover_throttle = self.hover_for_interval(hover_time)
vfr_hud = self.mav.recv_match(type='VFR_HUD', blocking=True)
# finish the tune # finish the tune
self.set_rc(7, 1000) self.set_rc(7, 1000)
@ -5651,7 +5647,7 @@ class AutoTestCopter(AutoTest):
self.progress("FFT detected parameters were %fHz, ref %f" % (detected_freq, detected_ref)) self.progress("FFT detected parameters were %fHz, ref %f" % (detected_freq, detected_ref))
# approximate the scaled frequency # approximate the scaled frequency
scaled_freq_at_hover = math.sqrt((vfr_hud.throttle / 100.) / detected_ref) * detected_freq scaled_freq_at_hover = math.sqrt((hover_throttle / 100.) / detected_ref) * detected_freq
# Check we matched # Check we matched
if abs(scaled_freq_at_hover - freq) / scaled_freq_at_hover > 0.05: if abs(scaled_freq_at_hover - freq) / scaled_freq_at_hover > 0.05:
@ -5684,9 +5680,7 @@ class AutoTestCopter(AutoTest):
# start the tune # start the tune
self.set_rc(7, 2000) self.set_rc(7, 2000)
self.hover_for_interval(hover_time) tstart, tend, hover_throttle = self.hover_for_interval(hover_time)
vfr_hud = self.mav.recv_match(type='VFR_HUD', blocking=True)
# finish the tune # finish the tune
self.set_rc(7, 1000) self.set_rc(7, 1000)
@ -5698,7 +5692,7 @@ class AutoTestCopter(AutoTest):
self.progress("FFT detected parameters were %fHz, ref %f" % (detected_freq, detected_ref)) self.progress("FFT detected parameters were %fHz, ref %f" % (detected_freq, detected_ref))
# approximate the scaled frequency # approximate the scaled frequency
scaled_freq_at_hover = math.sqrt((vfr_hud.throttle / 100.) / detected_ref) * detected_freq scaled_freq_at_hover = math.sqrt((hover_throttle / 100.) / detected_ref) * detected_freq
# Check we matched # Check we matched
if abs(scaled_freq_at_hover - freq) / scaled_freq_at_hover > 0.05: if abs(scaled_freq_at_hover - freq) / scaled_freq_at_hover > 0.05: