diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index f733d92183..ade0324672 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -5245,6 +5245,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): 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: now = self.get_sim_time_cached() if now - tstart > timeout: @@ -5257,7 +5258,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): '''retrieve latest angles from GIMBAL_DEVICE_ATTITUDE_STATUS''' mount_roll, mount_pitch, mount_yaw = self.get_mount_roll_pitch_yaw_deg() - self.progress("despitch=%f roll=%f pitch=%f yaw=%f" % (despitch, mount_roll, mount_pitch, mount_yaw)) + # self.progress("despitch=%f roll=%f pitch=%f yaw=%f" % (despitch, mount_roll, mount_pitch, mount_yaw)) if abs(despitch - mount_pitch) > despitch_tolerance: self.progress("Mount pitch incorrect: got=%f want=%f (+/- %f)" % (mount_pitch, despitch, despitch_tolerance)) @@ -5333,9 +5334,9 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): p3=0, # stabilize pitch (unsupported) ) - def test_mount_rc_targetting(self): + def test_mount_rc_targetting(self, pitch_rc_neutral=1500, do_rate_tests=True): '''called in multipleplaces to make sure that mount RC targetting works''' - try: + if True: self.context_push() self.set_parameters({ 'RC6_OPTION': 0, @@ -5395,6 +5396,13 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.set_rc(12, 1500) + if do_rate_tests: + self.test_mount_rc_targetting_rate_control() + + self.context_pop() + + def test_mount_rc_targetting_rate_control(self, pitch_rc_neutral=1500): + if True: 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) @@ -5417,46 +5425,21 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.set_rc(12, 1500) self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.context_pop() - - except Exception as e: - self.print_exception_caught(e) - self.context_pop() - raise e - - def Mount(self): - '''Test Camera/Antenna Mount''' - ex = None - self.context_push() - old_srcSystem = self.mav.mav.srcSystem - self.mav.mav.srcSystem = 250 - self.set_parameter("DISARM_DELAY", 0) - try: - '''start by disabling GCS failsafe, otherwise we immediately disarm - due to (apparently) not receiving traffic from the GCS for - too long. This is probably a function of --speedup''' - self.set_parameter("FS_GCS_ENABLE", 0) - - # setup mount parameters - self.setup_servo_mount() - self.reboot_sitl() # to handle MNT_TYPE changing - + def mount_test_body(self, pitch_rc_neutral=1500, do_rate_tests=True, constrain_sysid_target=True): + '''Test Camera/Antenna Mount - assumes a camera is set up and ready to go''' + if True: # make sure we're getting gimbal device attitude status - self.assert_receive_message('GIMBAL_DEVICE_ATTITUDE_STATUS', timeout=5) + self.assert_receive_message('GIMBAL_DEVICE_ATTITUDE_STATUS', timeout=5, very_verbose=True) # change mount to neutral mode (point forward, not stabilising) self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL) - # test pitch is not stabilising + # test pitch is not neutral to start with mount_roll_deg, mount_pitch_deg, mount_yaw_deg = 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") + raise NotAchievedException("Mount not neutral") - self.change_mode('GUIDED') - self.wait_ready_to_arm() - self.arm_vehicle() - - self.user_takeoff() + self.takeoff(30, mode='GUIDED') # pitch vehicle back and confirm gimbal is still not stabilising despitch = 10 @@ -5474,7 +5457,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): # center RC tilt control and change mount to RC_TARGETING mode self.progress("Gimbal to RC Targetting mode") - self.set_rc(6, 1500) + self.set_rc(6, pitch_rc_neutral) self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) # pitch vehicle back and confirm gimbal is stabilising @@ -5520,7 +5503,10 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.progress("Testing mount RC targetting") self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_rc_targetting() + self.test_mount_rc_targetting( + pitch_rc_neutral=pitch_rc_neutral, + do_rate_tests=do_rate_tests, + ) self.progress("Testing mount ROI behaviour") self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) @@ -5658,24 +5644,39 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): 0, # vz 0 # heading ) - self.test_mount_pitch(68, 5, mavutil.mavlink.MAV_MOUNT_MODE_SYSID_TARGET, hold=2) + self.test_mount_pitch( + 68, + 5, + mavutil.mavlink.MAV_MOUNT_MODE_SYSID_TARGET, + hold=2, + constrained=constrain_sysid_target, + ) self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL) self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL) - except Exception as e: - self.print_exception_caught(e) - ex = e + self.disarm_vehicle(force=True) - self.mav.mav.srcSystem = old_srcSystem - self.disarm_vehicle(force=True) + def Mount(self): + '''test servo mount''' + self.setup_servo_mount() + self.reboot_sitl() # to handle MNT_TYPE changing + self.mount_test_body() - self.context_pop() - - self.reboot_sitl() # to handle MNT1_TYPE changing - - if ex is not None: - raise ex + def MountSolo(self): + '''test type=2, a "Solo" mount''' + self.set_parameters({ + "MNT1_TYPE": 2, + "RC6_OPTION": 213, # MOUNT1_PITCH + }) + self.customise_SITL_commandline([ + "--gimbal" # connects on port 5762 + ]) + self.mount_test_body( + pitch_rc_neutral=1818, + do_rate_tests=False, # solo can't do rate control (yet?) + constrain_sysid_target=False, # not everything constrains all angles + ) def assert_mount_rpy(self, r, p, y, tolerance=1): '''assert mount atttiude in degrees''' @@ -11730,6 +11731,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.TerrainDBPreArm, self.ThrottleGainBoost, self.ScriptMountPOI, + self.MountSolo, self.FlyMissionTwice, self.FlyMissionTwiceWithReset, self.MissionIndexValidity,