aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Tools: add support for ATTITUDE_QUATERNION

Browse Source
This commit is contained in:
Peter Barker 2021-03-03 17:42:45 +11:00 committed by Peter Barker
parent 5123b05b2b
commit eb702b0f82
1 changed files with 23 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
Tools/autotest/arduplane.py
View File

@ -1446,6 +1446,7 @@ class AutoTestPlane(AutoTest):
# check attitude
simstate = self.mav.recv_match(type='SIMSTATE', blocking=True, timeout=1)
attitude_quaternion = self.poll_message('ATTITUDE_QUATERNION')
if simstate is None:
raise NotAchievedException("Did not receive SIMSTATE message")
self.progress("SIMSTATE: %s" % str(simstate))
@ -1460,6 +1461,28 @@ class AutoTestPlane(AutoTest):
raise NotAchievedException("Secondary pitch looks bad (want=%f got=%f)" %
(want, got))
q = quaternion.Quaternion([
attitude_quaternion.q1,
attitude_quaternion.q2,
attitude_quaternion.q3,
attitude_quaternion.q4
])
euler = q.euler
self.progress("attquat:%s q:%s euler:%s" % (
str(attitude_quaternion), q, euler))
want = math.degrees(simstate.roll)
got = math.degrees(euler[0])
if want - got > 5:
raise NotAchievedException("quat roll differs from attitude roll; want=%f got=%f" %
(want, got))
want = math.degrees(simstate.pitch)
got = math.degrees(euler[1])
if want - got > 5:
raise NotAchievedException("quat pitch differs from attitude pitch; want=%f got=%f" %
(want, got))
def test_main_flight(self):
self.change_mode('MANUAL')