autotest: test Solo mount

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()
+    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'''
-        except Exception as e:
+        if True:
-            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
-
             # 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.takeoff(30, mode='GUIDED')
-            self.wait_ready_to_arm()
-            self.arm_vehicle()
-
-            self.user_takeoff()
 
             # 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.disarm_vehicle(force=True)
-            self.print_exception_caught(e)
-            ex = e
 
-        self.mav.mav.srcSystem = old_srcSystem
+    def Mount(self):
-        self.disarm_vehicle(force=True)
+        '''test servo mount'''
+        self.setup_servo_mount()
+        self.reboot_sitl() # to handle MNT_TYPE changing
+        self.mount_test_body()
 
-        self.context_pop()
+    def MountSolo(self):
-
+        '''test type=2, a "Solo" mount'''
-        self.reboot_sitl() # to handle MNT1_TYPE changing
+        self.set_parameters({
-
+            "MNT1_TYPE": 2,
-        if ex is not None:
+            "RC6_OPTION": 213,  # MOUNT1_PITCH
-            raise ex
+        })
+        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,