From 16b499d0f6ff82264b6d9e8a1e7fd2ee6391a672 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Wed, 26 Aug 2020 16:22:34 +0200 Subject: [PATCH] Autotest: allow to select which GPS flags we want to wait for --- Tools/autotest/arducopter.py | 2 +- Tools/autotest/common.py | 8 ++++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 05c1bd7991..3c649e8cce 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -5185,7 +5185,7 @@ class AutoTestCopter(AutoTest): ex = None try: self.set_parameter("EK2_ALT_SOURCE", 2) - self.wait_gps_disable() + self.wait_gps_disable(position_vertical=True) self.change_mode("ALT_HOLD") self.assert_prearm_failure("Need Alt Estimate") self.change_mode("STABILIZE") diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 00a02435ac..2473b24db2 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -3839,7 +3839,7 @@ Also, ignores heartbeats not from our target system''' raise AutoTestTimeoutException("Failed to get EKF.flags=%u" % required_value) - def wait_gps_disable(self, timeout=30): + def wait_gps_disable(self, position_horizontal=True, position_vertical=False, timeout=30): """Disable GPS and wait for EKF to report the end of assistance from GPS.""" self.set_parameter("SIM_GPS_DISABLE", 1) tstart = self.get_sim_time() @@ -3850,7 +3850,11 @@ Also, ignores heartbeats not from our target system''' return # all of these must NOT be set for arming NOT to happen: - not_required_value = mavutil.mavlink.ESTIMATOR_POS_HORIZ_REL + not_required_value = 0 + if position_horizontal: + not_required_value |= mavutil.mavlink.ESTIMATOR_POS_HORIZ_REL + if position_vertical: + not_required_value |= mavutil.mavlink.ESTIMATOR_POS_VERT_AGL self.progress("Waiting for EKF not having bits %u" % not_required_value) last_print_time = 0 while timeout is None or self.get_sim_time_cached() < tstart + timeout: