|
|
|
@ -4356,58 +4356,53 @@ class AutoTestCopter(AutoTest):
@@ -4356,58 +4356,53 @@ class AutoTestCopter(AutoTest):
|
|
|
|
|
self.context_push() |
|
|
|
|
|
|
|
|
|
ex = None |
|
|
|
|
# we are dealing with probabalistic scenarios involving threads, have two bites at the cherry |
|
|
|
|
for loop in ["first", "second"]: |
|
|
|
|
try: |
|
|
|
|
self.set_rc_default() |
|
|
|
|
self.set_parameter("AHRS_EKF_TYPE", 10) |
|
|
|
|
self.set_parameter("INS_LOG_BAT_MASK", 3) |
|
|
|
|
self.set_parameter("INS_LOG_BAT_OPT", 0) |
|
|
|
|
# set the gyro filter high so we can observe behaviour |
|
|
|
|
self.set_parameter("INS_GYRO_FILTER", 100) |
|
|
|
|
self.set_parameter("LOG_BITMASK", 958) |
|
|
|
|
self.set_parameter("LOG_DISARMED", 0) |
|
|
|
|
self.set_parameter("SIM_VIB_MOT_MAX", 350) |
|
|
|
|
self.set_parameter("SIM_GYR1_RND", 20) |
|
|
|
|
self.reboot_sitl() |
|
|
|
|
try: |
|
|
|
|
self.set_parameters({ |
|
|
|
|
"AHRS_EKF_TYPE": 10, |
|
|
|
|
"INS_LOG_BAT_MASK": 3, |
|
|
|
|
"INS_LOG_BAT_OPT": 0, |
|
|
|
|
"INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour |
|
|
|
|
"LOG_BITMASK": 958, |
|
|
|
|
"LOG_DISARMED": 0, |
|
|
|
|
"SIM_VIB_MOT_MAX": 350, |
|
|
|
|
"SIM_GYR1_RND": 20, |
|
|
|
|
}) |
|
|
|
|
self.reboot_sitl() |
|
|
|
|
|
|
|
|
|
self.takeoff(10, mode="ALT_HOLD") |
|
|
|
|
self.takeoff(10, mode="ALT_HOLD") |
|
|
|
|
|
|
|
|
|
# find a motor peak |
|
|
|
|
freq, vfr_hud, peakdb = self.hover_and_check_matched_frequency_with_fft(-15, 200, 300) |
|
|
|
|
# find a motor peak |
|
|
|
|
freq, vfr_hud, peakdb = self.hover_and_check_matched_frequency_with_fft(-15, 200, 300) |
|
|
|
|
|
|
|
|
|
# now add a dynamic notch and check that the peak is squashed |
|
|
|
|
self.set_parameter("INS_LOG_BAT_OPT", 2) |
|
|
|
|
self.set_parameter("INS_HNTCH_ENABLE", 1) |
|
|
|
|
self.set_parameter("INS_HNTCH_FREQ", freq) |
|
|
|
|
self.set_parameter("INS_HNTCH_REF", vfr_hud.throttle/100.) |
|
|
|
|
# first and third harmonic |
|
|
|
|
self.set_parameter("INS_HNTCH_HMNCS", 5) |
|
|
|
|
self.set_parameter("INS_HNTCH_ATT", 50) |
|
|
|
|
self.set_parameter("INS_HNTCH_BW", freq/2) |
|
|
|
|
self.reboot_sitl() |
|
|
|
|
# now add a dynamic notch and check that the peak is squashed |
|
|
|
|
self.set_parameters({ |
|
|
|
|
"INS_LOG_BAT_OPT": 2, |
|
|
|
|
"INS_HNTCH_ENABLE": 1, |
|
|
|
|
"INS_HNTCH_FREQ": freq, |
|
|
|
|
"INS_HNTCH_REF": vfr_hud.throttle/100., |
|
|
|
|
"INS_HNTCH_HMNCS": 5, # first and third harmonic |
|
|
|
|
"INS_HNTCH_ATT": 50, |
|
|
|
|
"INS_HNTCH_BW": freq/2, |
|
|
|
|
}) |
|
|
|
|
self.reboot_sitl() |
|
|
|
|
|
|
|
|
|
freq, vfr_hud, peakdb1 = self.hover_and_check_matched_frequency_with_fft(-10, 20, 350, reverse=True) |
|
|
|
|
freq, vfr_hud, 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 |
|
|
|
|
self.set_parameter("INS_HNTCH_OPTS", 1) |
|
|
|
|
self.reboot_sitl() |
|
|
|
|
# now add double dynamic notches and check that the peak is squashed |
|
|
|
|
self.set_parameter("INS_HNTCH_OPTS", 1) |
|
|
|
|
self.reboot_sitl() |
|
|
|
|
|
|
|
|
|
freq, vfr_hud, peakdb2 = self.hover_and_check_matched_frequency_with_fft(-15, 20, 350, reverse=True) |
|
|
|
|
freq, vfr_hud, peakdb2 = self.hover_and_check_matched_frequency_with_fft(-15, 20, 350, reverse=True) |
|
|
|
|
|
|
|
|
|
# double-notch should do better, but check for within 5% |
|
|
|
|
if peakdb2 * 1.05 > peakdb1: |
|
|
|
|
raise NotAchievedException( |
|
|
|
|
"Double-notch peak was higher than single-notch peak %fdB > %fdB" % |
|
|
|
|
(peakdb2, peakdb1)) |
|
|
|
|
# double-notch should do better, but check for within 5% |
|
|
|
|
if peakdb2 * 1.05 > peakdb1: |
|
|
|
|
raise NotAchievedException( |
|
|
|
|
"Double-notch peak was higher than single-notch peak %fdB > %fdB" % |
|
|
|
|
(peakdb2, peakdb1)) |
|
|
|
|
|
|
|
|
|
except Exception as e: |
|
|
|
|
self.print_exception_caught(e) |
|
|
|
|
self.progress("Exception caught in %s loop" % (loop,)) |
|
|
|
|
if loop != "second": |
|
|
|
|
continue |
|
|
|
|
ex = e |
|
|
|
|
break |
|
|
|
|
except Exception as e: |
|
|
|
|
self.print_exception_caught(e) |
|
|
|
|
ex = e |
|
|
|
|
|
|
|
|
|
self.context_pop() |
|
|
|
|
|
|
|
|
@ -6948,7 +6943,7 @@ class AutoTestCopter(AutoTest):
@@ -6948,7 +6943,7 @@ class AutoTestCopter(AutoTest):
|
|
|
|
|
Test("DynamicNotches", |
|
|
|
|
"Fly Dynamic Notches", |
|
|
|
|
self.fly_dynamic_notches, |
|
|
|
|
attempts=4), |
|
|
|
|
attempts=8), |
|
|
|
|
|
|
|
|
|
Test("PositionWhenGPSIsZero", |
|
|
|
|
"Ensure position doesn't zero when GPS lost", |
|
|
|
|