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

autotest: Handle pitch limits when testing achieved mount pitch angle

Browse Source
This commit is contained in:
Nick Exton 2024-05-20 11:55:07 +10:00 committed by Randy Mackay
parent 0af4649705
commit bb0d1cf7b8
1 changed files with 21 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
Tools/autotest/arducopter.py
View File

@ -5170,7 +5170,12 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.run_cmd_do_set_mode("ACRO") self.run_cmd_do_set_mode("ACRO")
self.wait_disarmed() self.wait_disarmed()
def test_mount_pitch(self, despitch, despitch_tolerance, mount_mode, timeout=10, hold=0): def constrained_mount_pitch(self, pitch_angle_deg, mount_instance=1):
PITCH_MIN = self.get_parameter("MNT%u_PITCH_MIN" % mount_instance)
PITCH_MAX = self.get_parameter("MNT%u_PITCH_MAX" % mount_instance)
return min(max(pitch_angle_deg, PITCH_MIN), PITCH_MAX)
def test_mount_pitch(self, despitch, despitch_tolerance, mount_mode, timeout=10, hold=0, constrained=True):
tstart = self.get_sim_time() tstart = self.get_sim_time()
success_start = 0 success_start = 0
while True: while True:
@ -5178,6 +5183,10 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
if now - tstart > timeout: if now - tstart > timeout:
raise NotAchievedException("Mount pitch not achieved") raise NotAchievedException("Mount pitch not achieved")
# We expect to achieve the desired pitch angle unless constrained by mount limits
if constrained:
despitch = self.constrained_mount_pitch(despitch)
'''retrieve latest angles from GIMBAL_DEVICE_ATTITUDE_STATUS''' '''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 = self.get_mount_roll_pitch_yaw_deg()
@ -5322,17 +5331,19 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.progress("Testing RC rate control") self.progress("Testing RC rate control")
self.set_parameter('MNT1_RC_RATE', 10) self.set_parameter('MNT1_RC_RATE', 10)
self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
# Note that we don't constrain the desired angle in the following so that we don't
# timeout due to fetching Mount pitch limit params.
self.set_rc(12, 1300) self.set_rc(12, 1300)
self.test_mount_pitch(-5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) self.test_mount_pitch(-5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)
self.test_mount_pitch(-10, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) self.test_mount_pitch(-10, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)
self.test_mount_pitch(-15, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) self.test_mount_pitch(-15, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)
self.test_mount_pitch(-20, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) self.test_mount_pitch(-20, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)
self.set_rc(12, 1700) self.set_rc(12, 1700)
self.test_mount_pitch(-15, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) self.test_mount_pitch(-15, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)
self.test_mount_pitch(-10, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) self.test_mount_pitch(-10, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)
self.test_mount_pitch(-5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) self.test_mount_pitch(-5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)
self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)
self.test_mount_pitch(5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) self.test_mount_pitch(5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)
self.progress("Reverting to angle mode") self.progress("Reverting to angle mode")
self.set_parameter('MNT1_RC_RATE', 0) self.set_parameter('MNT1_RC_RATE', 0)