From a235246628526ee7c219ed9adf9a1990d0bb76b9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 3 Mar 2021 16:51:10 +1100 Subject: [PATCH] autotest: correct units in new AHRS2 test --- Tools/autotest/arduplane.py | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index b1189d88b0..ad7a31cc09 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -1314,16 +1314,20 @@ class AutoTestPlane(AutoTest): 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") - self.progress("ATTITUDE: %s" % str(attitude)) - if abs(math.degrees(attitude.roll) - ahrs2.roll) > 5: + simstate = self.mav.recv_match(type='SIMSTATE', blocking=True, timeout=1) + if simstate is None: + raise NotAchievedException("Did not receive SIMSTATE message") + self.progress("SIMSTATE: %s" % str(simstate)) + want = math.degrees(simstate.roll) + got = math.degrees(ahrs2.roll) + if abs(want - got) > 5: raise NotAchievedException("Secondary roll looks bad (want=%f got=%f)" % - (math.degrees(attitude.roll), ahrs2.roll)) - if abs(math.degrees(attitude.pitch) - ahrs2.pitch) > 5: + (want, got)) + want = math.degrees(simstate.pitch) + got = math.degrees(ahrs2.pitch) + if abs(want - got) > 5: raise NotAchievedException("Secondary pitch looks bad (want=%f got=%f)" % - (math.degrees(attitude.pitch), ahrs2.pitch)) + (want, got)) def test_main_flight(self):