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

autotest: return hover throttle from hover_for_interval

apm_2208
Peter Barker 3 years ago committed by Peter Barker
parent
ef29350a72
commit
1a9470d635
1 changed files with 45 additions and 51 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. 96
      Tools/autotest/arducopter.py

96
Tools/autotest/arducopter.py
Unescape 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)
self.do_RTL()
tstart, tend = self.hover_for_interval(hover_time) # why are we not checking the results from that flight? -pb20220613
self.do_RTL() # prevent update parameters from messing with the settings
# prevent update parameters from messing with the settings when we pop the context # 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:

Write Preview
Loading…
Cancel
Save