Add autotest for Solo mount

Tools/autotest/arducopter.py

@@ -5245,6 +5245,7 @@ def constrained_mount_pitch(self, pitch_angle_deg, mount_instance=1):
     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 @@ def test_mount_pitch(self, despitch, despitch_tolerance, mount_mode, timeout=10,
             '''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 @@ def set_mount_mode(self, mount_mode):
             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 @@ def test_mount_rc_targetting(self):
 
             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 @@ def test_mount_rc_targetting(self):
             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")
-
-            self.change_mode('GUIDED')
-            self.wait_ready_to_arm()
-            self.arm_vehicle()
+                raise NotAchievedException("Mount not neutral")
 
-            self.user_takeoff()
+            self.takeoff(30, mode='GUIDED')
 
             # pitch vehicle back and confirm gimbal is still not stabilising
             despitch = 10
@@ -5474,7 +5457,7 @@ def Mount(self):
 
             # 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 @@ def Mount(self):
             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 @@ def Mount(self):
                 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.mav.mav.srcSystem = old_srcSystem
-        self.disarm_vehicle(force=True)
-
-        self.context_pop()
+            self.disarm_vehicle(force=True)
 
-        self.reboot_sitl() # to handle MNT1_TYPE changing
+    def Mount(self):
+        '''test servo mount'''
+        self.setup_servo_mount()
+        self.reboot_sitl() # to handle MNT_TYPE changing
+        self.mount_test_body()
 
-        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'''
@@ -11678,6 +11679,7 @@ def tests2b(self):  # this block currently around 9.5mins here
             self.TerrainDBPreArm,
             self.ThrottleGainBoost,
             self.ScriptMountPOI,
+            self.MountSolo,
             self.FlyMissionTwice,
             self.FlyMissionTwiceWithReset,
             self.MissionIndexValidity,

libraries/AP_HAL/board/sitl.h

@@ -90,3 +90,5 @@
 #ifndef AP_FILTER_ENABLED
 #define AP_FILTER_ENABLED 1
 #endif
+
+#define HAL_SOLO_GIMBAL_ENABLED 1

libraries/AP_Mount/AP_Mount_SoloGimbal.cpp

@@ -1,6 +1,9 @@
-#include "AP_Mount_SoloGimbal.h"
+#include "AP_Mount_config.h"
+
 #if HAL_SOLO_GIMBAL_ENABLED
 
+#include "AP_Mount_SoloGimbal.h"
+
 #include "SoloGimbal.h"
 #include <AP_Logger/AP_Logger.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>

libraries/AP_Mount/AP_Mount_SoloGimbal.h

@@ -3,18 +3,16 @@
  */
 #pragma once
 
+#include "AP_Mount_config.h"
 
-#include <AP_HAL/AP_HAL.h>
+#if HAL_SOLO_GIMBAL_ENABLED
 
 #include "AP_Mount_Backend.h"
-#if HAL_SOLO_GIMBAL_ENABLED
 #include <AP_Math/AP_Math.h>
 #include <AP_Common/AP_Common.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
-#include <RC_Channel/RC_Channel.h>
 #include "SoloGimbal.h"
 
-
 class AP_Mount_SoloGimbal : public AP_Mount_Backend
 {
 

libraries/SITL/SIM_Gimbal.cpp

@@ -237,7 +237,7 @@ void Gimbal::param_send(const struct gimbal_param *p)
     param_value.param_type = MAV_PARAM_TYPE_REAL32;
 
     uint16_t len = mavlink_msg_param_value_encode_status(vehicle_system_id,
-                                                         vehicle_component_id,
+                                                         gimbal_component_id,
                                                          &mavlink.status,
                                                          &msg, &param_value);
 
@@ -364,7 +364,7 @@ void Gimbal::send_report(void)
         heartbeat.custom_mode = 0;
 
         len = mavlink_msg_heartbeat_encode_status(vehicle_system_id,
-                                                  vehicle_component_id,
+                                                  gimbal_component_id,
                                                   &mavlink.status,
                                                   &msg, &heartbeat);
 
@@ -394,7 +394,7 @@ void Gimbal::send_report(void)
         gimbal_report.joint_az = joint_angles.z;
 
         len = mavlink_msg_gimbal_report_encode_status(vehicle_system_id,
-                                                      vehicle_component_id,
+                                                      gimbal_component_id,
                                                       &mavlink.status,
                                                       &msg, &gimbal_report);
 

libraries/SITL/SIM_Gimbal.h

@@ -14,6 +14,11 @@
  */
 /*
   gimbal simulator class
+
+./Tools/autotest/sim_vehicle.py -D -G -v ArduCopter --mavlink-gimbal
+param set MNT1_TYPE 2
+param set RC6_OPTION 213  # MOUNT1_PITCH
+rc 6 1818  # for neutral pitch input
 */
 
 #pragma once
@@ -113,6 +118,9 @@ class Gimbal {
     uint32_t param_send_last_ms;
     uint8_t param_send_idx;
 
+    // component ID we send from:
+    const uint8_t gimbal_component_id = 154;  // MAV_COMP_ID_GIMBAL
+
     void send_report(void);
     void param_send(const struct gimbal_param *p);
     struct gimbal_param *param_find(const char *name);