From 06ef30a812f86e079e7c37c7d57d8c48e3c8adc4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 20 Aug 2024 14:08:00 +1000 Subject: [PATCH] autotest: check altitude in GPSWeightedBlending test --- Tools/autotest/arducopter.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 04f5609de075a5..09f12aee14f7bf 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -9140,15 +9140,16 @@ def GPSWeightedBlending(self): if m.TimeUS != current_ts: current_ts = None continue - measurements[m.I] = (m.Lat, m.Lng) + measurements[m.I] = (m.Lat, m.Lng, m.Alt) if len(measurements) == 3: # check lat: - for n in 0, 1: + for n in 0, 1, 2: expected_blended = 0.8*measurements[0][n] + 0.2*measurements[1][n] - epsilon = 0.0000002 + axis_epsilons = [0.0000002, 0.0000002, 0.2] + epsilon = axis_epsilons[n] error = abs(measurements[2][n] - expected_blended) if error > epsilon: - raise NotAchievedException(f"Blended diverged {measurements[0][n]=} {measurements[1][n]=}") + raise NotAchievedException(f"Blended diverged {n=} {measurements[0][n]=} {measurements[1][n]=}") current_ts = None self.context_pop()