diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 94d45fde61..7e5e3de374 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -5170,7 +5170,12 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.run_cmd_do_set_mode("ACRO") 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() success_start = 0 while True: @@ -5178,6 +5183,10 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): if now - tstart > timeout: 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''' 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.set_parameter('MNT1_RC_RATE', 10) 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.test_mount_pitch(-5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(-10, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(-15, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(-20, 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, constrained=False) + 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, constrained=False) self.set_rc(12, 1700) - self.test_mount_pitch(-15, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(-10, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(-5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(5, 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, constrained=False) + 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, constrained=False) + self.test_mount_pitch(5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False) self.progress("Reverting to angle mode") self.set_parameter('MNT1_RC_RATE', 0)