From 317181922c747ac675c70496f98b17555c6955de Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 2 Mar 2021 15:43:37 +1100 Subject: [PATCH] autotest: ensure we're getting vaguely sane data in AHRS2 message --- Tools/autotest/arduplane.py | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 0071977d70..0a0e915acb 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -1293,6 +1293,33 @@ class AutoTestPlane(AutoTest): mavproxy:''' self.clear_fence_using_mavproxy() + def fly_ahrs2_test(self): + '''check secondary estimator is looking OK''' + + ahrs2 = self.mav.recv_match(type='AHRS2', blocking=True, timeout=1) + if ahrs2 is None: + raise NotAchievedException("Did not receive AHRS2 message") + + # check location + gpi = self.mav.recv_match( + type='GLOBAL_POSITION_INT', + blocking=True, + timeout=5 + ) + if gpi is None: + raise NotAchievedException("Did not receive GLOBAL_POSITION_INT message") + if self.get_distance_int(gpi, ahrs2) > 10: + raise NotAchievedException("Secondary location looks bad") + + # check attitude + attitude = self.mav.recv_match(type='ATTITUDE', blocking=True, timeout=1) + if attitude is None: + raise NotAchievedException("Did not receive ATTITUDE message") + if abs(math.degrees(attitude.roll) - ahrs2.roll) > 5: + raise NotAchievedException("Secondary roll looks bad") + if abs(math.degrees(attitude.pitch) - ahrs2.pitch) > 5: + raise NotAchievedException("Secondary pitch looks bad") + def test_main_flight(self): self.change_mode('MANUAL') @@ -1328,6 +1355,8 @@ class AutoTestPlane(AutoTest): self.run_subtest("CIRCLE test", self.fly_CIRCLE) + self.run_subtest("AHRS2 test", self.fly_ahrs2_test) + self.run_subtest("Mission test", lambda: self.fly_mission("ap1.txt", strict=False))