Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

autotest: add Mount test for relative yaw

Browse Source
This commit is contained in:
Peter Barker 2023-03-25 18:04:38 +11:00 committed by Peter Barker
parent ba135b9008
commit 57d6815124
1 changed files with 28 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
Tools/autotest/arducopter.py
View File

@ -5256,7 +5256,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
despitch = self.constrained_mount_pitch(despitch)
'''retrieve latest angles from GIMBAL_DEVICE_ATTITUDE_STATUS'''
mount_roll, mount_pitch, mount_yaw = self.get_mount_roll_pitch_yaw_deg()
mount_roll, mount_pitch, mount_yaw, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg()
# self.progress("despitch=%f roll=%f pitch=%f yaw=%f" % (despitch, mount_roll, mount_pitch, mount_yaw))
if abs(despitch - mount_pitch) > despitch_tolerance:
@ -5314,10 +5314,11 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
# wait for gimbal attitude message
m = self.assert_receive_message('GIMBAL_DEVICE_ATTITUDE_STATUS', timeout=5)
yaw_is_absolute = m.flags & mavutil.mavlink.GIMBAL_DEVICE_FLAGS_YAW_LOCK
# convert quaternion to euler angles and return
q = quaternion.Quaternion(m.q)
euler = q.euler
return math.degrees(euler[0]), math.degrees(euler[1]), math.degrees(euler[2])
return math.degrees(euler[0]), math.degrees(euler[1]), math.degrees(euler[2]), yaw_is_absolute
def set_mount_mode(self, mount_mode):
'''set mount mode'''
@ -5435,7 +5436,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)
# test pitch is not neutral to start with
mount_roll_deg, mount_pitch_deg, mount_yaw_deg = self.get_mount_roll_pitch_yaw_deg()
mount_roll_deg, mount_pitch_deg, mount_yaw_deg, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg()
if mount_roll_deg != 0 or mount_pitch_deg != 0 or mount_yaw_deg != 0:
raise NotAchievedException("Mount not neutral")
@ -5451,7 +5452,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.wait_pitch(despitch, despitch_tolerance)
# check gimbal is still not stabilising
mount_roll_deg, mount_pitch_deg, mount_yaw_deg = self.get_mount_roll_pitch_yaw_deg()
mount_roll_deg, mount_pitch_deg, mount_yaw_deg, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg()
if mount_roll_deg != 0 or mount_pitch_deg != 0 or mount_yaw_deg != 0:
raise NotAchievedException("Mount stabilising when not requested")
@ -5657,6 +5658,28 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.disarm_vehicle(force=True)
self.test_mount_body_yaw()
def test_mount_body_yaw(self):
'''check reporting of yaw'''
# change mount to neutral mode (point forward, not stabilising)
self.takeoff(10, mode='GUIDED')
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)
for heading in 30, 45, 150:
self.guided_achieve_heading(heading)
r, p , y, yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg()
if yaw_is_absolute:
raise NotAchievedException("Expected a relative yaw")
if y > 1:
raise NotAchievedException("Bad yaw (y=%f)")
self.do_RTL()
def Mount(self):
'''test servo mount'''
self.setup_servo_mount()
@ -5680,7 +5703,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
def assert_mount_rpy(self, r, p, y, tolerance=1):
'''assert mount atttiude in degrees'''
got_r, got_p, got_y = self.get_mount_roll_pitch_yaw_deg()
got_r, got_p, got_y, yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg()
for (want, got, name) in (r, got_r, "roll"), (p, got_p, "pitch"), (y, got_y, "yaw"):
if abs(want - got) > tolerance:
raise NotAchievedException("%s incorrect; want=%f got=%f" %