Skydroid driver for C11 and C13 gimbals

Tools/autotest/arducopter.py

@@ -7786,7 +7786,8 @@ def test_mount_rc_targetting(self, pitch_rc_neutral=1500, do_rate_tests=True, pi
         pitch_tolerance defaults to the original tight 0.1deg check; backends whose
         actuator has a coarser confirmed physical resolution (e.g. a rate-only
         actuator closing an angle loop via a quantized speed command) may need to
-        pass a wider value'''
+        pass a wider value - see MountSkyDroid()'s use of this for a concrete
+        example with the reasoning'''
         if True:
             self.context_push()
             self.set_parameters({
@@ -8460,6 +8461,172 @@ def MountTopotek(self):
         self.mount_test_body(pitch_rc_neutral=1818, do_rate_tests=False,
                              constrain_sysid_target=False)
 
+    def MountSkyDroid(self):
+        '''test SkyDroid gimbal using SIM_SkyDroid simulator'''
+        # pitch_rc_neutral=1818: with RC6 min=1000 max=2000 trim=1500 and
+        # default MNT1_PITCH_MIN=-90 / MNT1_PITCH_MAX=20, norm_input=0.636
+        # maps to exactly 0 deg pitch.
+        pitch_rc_neutral = 1818
+        # centre RC6 *before* the parameter changes below reboot the FC.  MNT1's
+        # default mode is RC_TARGETING, so without this the mount starts driving
+        # toward whatever angle RC6's un-centred default value maps to the moment
+        # it boots, well before the test gets to explicitly select NEUTRAL mode.
+        # Harmless for backends with a fast control loop (they recover from that
+        # transient inside the neutral check's 5s budget), but avoid causing a large
+        # transient in the first place anyway rather than rely on recovering from it
+        self.set_rc(6, pitch_rc_neutral)
+        self.set_parameters({
+            "MNT1_TYPE": 15,      # SkyDroid
+            "CAM1_TYPE": 4,       # Mount
+            "SERIAL5_PROTOCOL": 8,  # gimbal
+            "RC6_OPTION": 213,    # MOUNT1_PITCH
+        })
+        self.customise_SITL_commandline(["--serial5=sim:skydroid:"])
+        # version "V1.0.0" from SIM_SkyDroid: major=1 | (minor=0)<<8 | (patch=0)<<16 = 1
+        # cap flags: CAPTURE_VIDEO | CAPTURE_IMAGE | HAS_BASIC_ZOOM
+        self.mount_check_camera_information(
+            "SkyDroid", "C11",
+            expected_fw_version=1,
+            expected_cap_flags=0x43,
+        )
+        # constrain_sysid_target=True (the default): unlike Topotek/Viewpro,
+        # AP_Mount_SkyDroid::send_target_angles does clamp pitch/yaw to the
+        # configured MNT1_PITCH/YAW_MIN/MAX before sending, so the 68-deg
+        # sysid test (which expects that clamp) is exercised here.
+        # neutral_tol_deg=3.5: confirmed on real hardware that SkyDroid gimbals only
+        # respond to the individual-axis GSY/GSP speed commands (GAM/GSM are silently
+        # ignored); since there's no working absolute-angle command, the driver closes
+        # an angle P-controller loop on top of them, so allow a little settling
+        # tolerance rather than the exact positioning an absolute-angle backend gives.
+        #
+        # rc_targetting_pitch_tolerance=0.3: GSY/GSP's wire value is a quantized 8bit
+        # signed LSB (see AP_MOUNT_SKYDROID_AXIS_DPS_PER_LSB, confirmed on real
+        # hardware via dataflash log analysis to be 0.5deg/s per LSB), which puts a
+        # genuine, measured floor of ~0.25deg of angular error below which the
+        # closed-loop P-controller's commanded rate rounds to 0 LSB and it simply
+        # stops correcting - this is a real actuator resolution limit, not a driver
+        # bug, and the shared test's default 0.1deg tolerance is tighter than this
+        # actuator can physically deliver.  0.3 gives a little margin above the
+        # measured ~0.25deg floor
+        self.mount_test_body(pitch_rc_neutral=pitch_rc_neutral, do_rate_tests=False, neutral_tol_deg=3.5,
+                             rc_targetting_pitch_tolerance=0.3)
+
+    def MountSkyDroidC13(self):
+        '''test SkyDroid C13 gimbal using SIM_SkyDroid simulator
+
+        SkyDroid have confirmed the gimbal-control commands are IDENTICAL across
+        models - the C13's extra features over the C11 are infrared thermal imaging
+        and laser ranging, neither of which this driver uses.  So this test is
+        deliberately the same body as MountSkyDroid(): it is a regression guard
+        that the driver stays model-independent, and that a differently-named model
+        does not take a different control path.  It also asserts roll stays
+        uncontrollable even when MNT1_ROLL_MIN/MAX is configured, since SkyDroid
+        have confirmed roll is self-stabilized with no control command at all'''
+        # pitch_rc_neutral=1818: with RC6 min=1000 max=2000 trim=1500 and
+        # default MNT1_PITCH_MIN=-90 / MNT1_PITCH_MAX=20, norm_input=0.636
+        # maps to exactly 0 deg pitch.
+        pitch_rc_neutral = 1818
+        # centre RC6 *before* the parameter changes below reboot the FC - same fix as
+        # MountSkyDroid() needed, and for the same reason: see the comment there
+        self.set_rc(6, pitch_rc_neutral)
+        self.set_parameters({
+            "MNT1_TYPE": 15,      # SkyDroid
+            "CAM1_TYPE": 4,       # Mount
+            "SERIAL5_PROTOCOL": 8,  # gimbal
+            "RC6_OPTION": 213,    # MOUNT1_PITCH
+            # deliberately configure a roll range the gimbal cannot actually use, to
+            # prove the driver still refuses to drive roll - see the roll check below
+            "MNT1_ROLL_MIN": -45,
+            "MNT1_ROLL_MAX": 45,
+        })
+        self.customise_SITL_commandline(["--serial5=sim:skydroid_c13:"])
+        # version "V1.0.0" from SIM_SkyDroid: major=1 | (minor=0)<<8 | (patch=0)<<16 = 1
+        # cap flags: CAPTURE_VIDEO | CAPTURE_IMAGE | HAS_BASIC_ZOOM
+        # model name "C13" confirms the differently-named variant was actually selected
+        self.mount_check_camera_information(
+            "SkyDroid", "C13",
+            expected_fw_version=1,
+            expected_cap_flags=0x43,
+        )
+        # identical expectations to MountSkyDroid() - that is the point of this test,
+        # rc_targetting_pitch_tolerance included - see MountSkyDroid()'s comment for
+        # why 0.3 rather than the shared default of 0.1
+        self.mount_test_body(pitch_rc_neutral=pitch_rc_neutral, do_rate_tests=False, neutral_tol_deg=3.5,
+                             rc_targetting_pitch_tolerance=0.3)
+
+        # roll must NOT respond: SkyDroid have confirmed roll is self-stabilized by the
+        # gimbal with no control command on any model, so AP_Mount_SkyDroid reports
+        # has_roll_control() == false and never sends a roll command.  This is the
+        # inverse of a test that used to live here, which drove roll via "GAR" and
+        # expected it to move - that command turned out not to be implemented in the
+        # firmware at all despite being in the protocol document
+        self.progress("Testing mount roll stays uncommanded (roll is not controllable)")
+        # mount_test_body() above ends with its own RTL+landing sequence, so the
+        # vehicle's resting attitude here is whatever it happens to land at - NOT
+        # guaranteed level.  So this deliberately checks for CHANGE in response to the
+        # RC command, not an absolute near-zero value - the claim under test is "roll
+        # doesn't respond to input", which holds regardless of the vehicle's own
+        # attitude, unlike a fixed absolute-value check
+        self.context_push()
+        self.set_parameters({
+            'RC11_OPTION': 212,    # MOUNT1_ROLL
+        })
+        self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
+        start_roll_deg, _, _, _ = self.get_mount_roll_pitch_yaw_deg()
+        self.set_rc(11, 1100)   # would demand roll to the MNT1_ROLL_MIN extreme
+        tstart = self.get_sim_time()
+        max_roll_change_deg = 0
+        while self.get_sim_time_cached() - tstart < 10:
+            mount_roll_deg, _, _, _ = self.get_mount_roll_pitch_yaw_deg()
+            self.progress("roll=%f (start was %f)" % (mount_roll_deg, start_roll_deg))
+            max_roll_change_deg = max(max_roll_change_deg, abs(mount_roll_deg - start_roll_deg))
+        self.set_rc(11, 1500)
+        self.context_pop()
+        # 15deg is well clear of the 45deg the RC input demands, while leaving room for
+        # whatever incidental roll change the gimbal's own stabilization shows as the
+        # vehicle moves - we are checking nothing *drives* roll, not that it is pinned
+        # at any particular value
+        if max_roll_change_deg > 15:
+            raise NotAchievedException(
+                "Mount roll changed %.1fdeg in response to RC input - roll should not be commandable" %
+                max_roll_change_deg)
+
+    def MountSkyDroidNetwork(self):
+        '''test SkyDroid gimbal connected via a UDP network port rather than a serial port
+
+        the real C11 hardware is UDP-only (no serial control interface), so this
+        exercises the actual transport used in the field rather than the SITL
+        serial-port path used by MountSkyDroid'''
+        self.set_parameters({
+            "MNT1_TYPE": 15,      # SkyDroid
+            "CAM1_TYPE": 4,       # Mount
+            "NET_ENABLE": 1,
+            "NET_P1_TYPE": 1,     # UDP client
+            "NET_P1_PROTOCOL": 8,  # gimbal
+            "NET_P1_IP0": 127,
+            "NET_P1_IP1": 0,
+            "NET_P1_IP2": 0,
+            "NET_P1_IP3": 1,
+            "NET_P1_PORT": 15006,
+        })
+        # the simulated gimbal listens on a UDP socket rather than
+        # being attached to one of the autopilot's serial ports:
+        self.customise_SITL_commandline(["--net-device=skydroid:15006,udp"])
+        self.mount_check_camera_information(
+            "SkyDroid", "C11",
+            expected_fw_version=1,
+            expected_cap_flags=0x43,
+        )
+        # command an angle and check the gimbal reports reaching it,
+        # which requires traffic in both directions:
+        self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING)
+        self.run_cmd(
+            mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,
+            p1=-30,  # pitch angle in degrees
+            p7=mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING,
+        )
+        self.wait_mount_roll_pitch_yaw_deg(p=-30)
+
     def MountTopotekNetwork(self):
         '''test Topotek gimbal connected via a network port rather than a serial port'''
         self.set_parameters({
@@ -20221,6 +20388,9 @@ def tests2b(self):  # this block currently around 9.5mins here
             self.TakeoffWithLocation,
             self.MountTopotek,
             self.MountTopotekNetwork,
+            self.MountSkyDroid,
+            self.MountSkyDroidC13,
+            self.MountSkyDroidNetwork,
             self.MountViewPro,
             self.MountAVTCM62,
             self.MountAVTCM62Dual,

Tools/scripts/build_options.py

@@ -247,6 +247,7 @@ def config_option(self):
     Feature('Gimbal', 'SOLOGIMBAL', 'HAL_SOLO_GIMBAL_ENABLED', 'Enable Solo gimbal', 0, "MOUNT"),
     Feature('Gimbal', 'STORM32_MAVLINK', 'HAL_MOUNT_STORM32MAVLINK_ENABLED', 'Enable SToRM32 MAVLink gimbal', 0, "MOUNT"),
     Feature('Gimbal', 'STORM32_SERIAL', 'HAL_MOUNT_STORM32SERIAL_ENABLED', 'Enable SToRM32 Serial gimbal', 0, "MOUNT"),
+    Feature('Gimbal', 'SKYDROID', 'HAL_MOUNT_SKYDROID_ENABLED', 'Enable SkyDroid gimbal', 0, "MOUNT"),
     Feature('Gimbal', 'TOPOTEK', 'HAL_MOUNT_TOPOTEK_ENABLED', 'Enable Topotek gimbal', 0, "MOUNT"),
     Feature('Gimbal', 'XACTI', 'HAL_MOUNT_XACTI_ENABLED', 'Enable Xacti gimbal', 0, "MOUNT,DroneCAN"),
     Feature('Gimbal', 'XFROBOT', 'HAL_MOUNT_XFROBOT_ENABLED', 'Enable XFRobot gimbal', 0, "MOUNT"),

libraries/AP_HAL_SITL/SITL_State_common.cpp

@@ -14,6 +14,7 @@
 #include <unistd.h>
 #include <stdlib.h>
 #include <errno.h>
+#include <strings.h>
 #include <sys/select.h>
 
 #include <AP_Param/AP_Param.h>
@@ -243,6 +244,18 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const
         sitl_model->add_gimbal_sim(*topotek);
         return topotek;
 #endif  // AP_SIM_TOPOTEK_ENABLED
+#if AP_SIM_SKYDROID_ENABLED
+    } else if (streq(name, "skydroid")) {
+        const auto skydroid = NEW_NOTHROW SITL::SkyDroid("C11");
+        sitl_model->add_gimbal_sim(*skydroid);
+        return skydroid;
+    } else if (streq(name, "skydroid_c13")) {
+        // the model name is the ONLY difference from "skydroid" above - SkyDroid have
+        // confirmed the gimbal-control commands are identical across models
+        const auto skydroid_c13 = NEW_NOTHROW SITL::SkyDroid("C13");
+        sitl_model->add_gimbal_sim(*skydroid_c13);
+        return skydroid_c13;
+#endif  // AP_SIM_SKYDROID_ENABLED
 #if AP_SIM_VIEWPRO_ENABLED
     } else if (streq(name, "viewpro")) {
         const auto viewpro = NEW_NOTHROW SITL::Viewpro();
@@ -359,10 +372,16 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const
 
 #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED
 /*
-  create a simulated device which the autopilot connects to over TCP
-  rather than over one of its simulated serial ports.  This is used to
-  simulate devices attached to the autopilot's network ports (NET_Pn).
-  spec is of the form NAME:TCPPORT e.g. "topotek:15005"
+  create a simulated device which the autopilot connects to over the
+  network (TCP by default, or UDP) rather than over one of its
+  simulated serial ports.  This is used to simulate devices attached
+  to the autopilot's network ports (NET_Pn).
+  spec is of the form NAME:PORT or NAME:PORT,OPTION,OPTION,..., e.g.
+  "topotek:15005" (TCP, the default) or "skydroid:15005,udp" - any
+  options beyond the port number are comma-separated from each other
+  (and from the port number), rather than each being tacked on with
+  another colon, since they're logically grouped with the port rather
+  than being another NAME-like top-level field
  */
 void SITL_State_Common::create_net_serial_sim(const char *spec)
 {
@@ -376,14 +395,27 @@ void SITL_State_Common::create_net_serial_sim(const char *spec)
     }
     char *saveptr = nullptr;
     const char *name = strtok_r(s, ":", &saveptr);
-    const char *port_str = strtok_r(nullptr, ":", &saveptr);
-    if (name == nullptr || port_str == nullptr) {
-        AP_HAL::panic("Bad network device (%s); expected NAME:TCPPORT", spec);
+    char *port_and_options = strtok_r(nullptr, ":", &saveptr);
+    if (name == nullptr || port_and_options == nullptr) {
+        AP_HAL::panic("Bad network device (%s); expected NAME:PORT[,PROTOCOL]", spec);
+    }
+    char *saveptr2 = nullptr;
+    const char *port_str = strtok_r(port_and_options, ",", &saveptr2);
+    const char *protocol_str = strtok_r(nullptr, ",", &saveptr2);  // optional, defaults to "tcp"
+    if (port_str == nullptr) {
+        AP_HAL::panic("Bad network device (%s); expected NAME:PORT[,PROTOCOL]", spec);
+    }
+    const bool use_udp = (protocol_str != nullptr) && (strcasecmp(protocol_str, "udp") == 0);
+    if (protocol_str != nullptr && !use_udp && strcasecmp(protocol_str, "tcp") != 0) {
+        AP_HAL::panic("Bad network device protocol (%s); expected 'tcp' or 'udp'", protocol_str);
     }
 
     SITL::SerialDevice *device = create_serial_sim(name, nullptr, 0);
-    if (!device->listen_on_tcp_port(atoi(port_str))) {
-        AP_HAL::panic("Failed to attach %s to TCP port %s", name, port_str);
+    const bool ok = use_udp ?
+        device->listen_on_udp_port(atoi(port_str)) :
+        device->listen_on_tcp_port(atoi(port_str));
+    if (!ok) {
+        AP_HAL::panic("Failed to attach %s to %s port %s", name, use_udp ? "UDP" : "TCP", port_str);
     }
     net_serial_sims[num_net_serial_sims++] = device;
 

libraries/AP_HAL_SITL/SITL_State_common.h

@@ -28,6 +28,7 @@
 
 #include <SITL/SIM_Siyi_ZT30.h>
 #include <SITL/SIM_Topotek.h>
+#include <SITL/SIM_SkyDroid.h>
 #include <SITL/SIM_Viewpro.h>
 #include <SITL/SIM_AVT_CM62.h>
 

libraries/AP_HAL_SITL/SITL_cmdline.cpp

@@ -109,7 +109,7 @@ void SITL_State::_usage(void)
            "\t--serial8 device         set device string for SERIAL8\n"
            "\t--serial9 device         set device string for SERIAL9\n"
            "\t--uartA device           alias for --serial0 (do not use)\n"
-           "\t--net-device NAME:PORT   attach simulated device NAME to TCP port PORT rather than to a serial port\n"
+           "\t--net-device NAME:PORT[,udp]   attach simulated device NAME to TCP (or, with ',udp', UDP) port PORT rather than to a serial port\n"
            "\t--base-port PORT         set port num for base port(default 5670) must be before -I option\n"
            "\t--rc-in-port PORT        set port num for rc in\n"
            "\t--sim-address ADDR       set address string for simulator\n"
@@ -267,7 +267,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
     struct AP_Param::defaults_table_struct temp_cmdline_param{};
 
 #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED
-    // NAME:TCPPORT strings from --net-device options:
+    // NAME:PORT[,udp] strings from --net-device options:
     const char *net_device_strings[4];
     uint8_t num_net_device_strings = 0;
 #endif  // AP_SIM_SERIALDEVICE_NETWORK_ENABLED

libraries/AP_Mount/AP_Mount.cpp

@@ -20,6 +20,7 @@
 #include "AP_Mount_Topotek.h"
 #include "AP_Mount_CADDX.h"
 #include "AP_Mount_XFRobot.h"
+#include "AP_Mount_SkyDroid.h"
 #include <stdio.h>
 #include <AP_Math/location.h>
 #include <SRV_Channel/SRV_Channel.h>
@@ -188,6 +189,15 @@ void AP_Mount::init()
             serial_instance++;
             break;
 #endif // HAL_MOUNT_XFROBOT_ENABLED
+
+#if HAL_MOUNT_SKYDROID_ENABLED
+        // check for SkyDroid gimbal
+        case Type::SkyDroid:
+            _backends[instance] = NEW_NOTHROW AP_Mount_SkyDroid(*this, _params[instance], instance, serial_instance);
+            _num_instances++;
+            serial_instance++;
+            break;
+#endif // HAL_MOUNT_SKYDROID_ENABLED
         }
 
         // init new instance

libraries/AP_Mount/AP_Mount.h

@@ -50,6 +50,7 @@ class AP_Mount_Viewpro;
 class AP_Mount_Topotek;
 class AP_Mount_CADDX;
 class AP_Mount_XFRobot;
+class AP_Mount_SkyDroid;
 
 /*
   This is a workaround to allow the MAVLink backend access to the
@@ -129,6 +130,9 @@ class AP_Mount
 #if HAL_MOUNT_XFROBOT_ENABLED
         XFRobot = 14,        /// XFRobot gimbal using a custom serial protocol
 #endif
+#if HAL_MOUNT_SKYDROID_ENABLED
+        SkyDroid = 15,       /// SkyDroid gimbal using a custom serial protocol
+#endif  // HAL_MOUNT_SKYDROID_ENABLED
     };
 
     // init - detect and initialise all mounts

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -1119,6 +1119,16 @@ void AP_Mount_Backend::update_angle_target_from_rate(const MountRateTarget& rate
     }
 }
 
+// simple P-controller converting an angle error to a rate command - see this
+// function's declaration for the full explanation
+float AP_Mount_Backend::angle_error_to_rate(float error, float gain, float rate_max, float deadzone)
+{
+    if (fabsf(error) <= deadzone) {
+        return 0.0f;
+    }
+    return constrain_float(error * gain, -rate_max, rate_max);
+}
+
 // helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message
 uint16_t AP_Mount_Backend::get_gimbal_device_flags() const
 {

libraries/AP_Mount/AP_Mount_Backend.h

@@ -407,6 +407,18 @@ class AP_Mount_Backend
     // assumes a 50hz update rate
     void update_angle_target_from_rate(const MountRateTarget& rate_rad, MountAngleTarget& angle_rad) const;
 
+    // simple P-controller converting an angle error to a rate command: multiply
+    // by gain, then constrain to +/- rate_max.  A non-zero deadzone forces a
+    // clean zero output for small errors instead of tapering to an
+    // ever-smaller command - useful for a rate actuator with a coarse
+    // resolution floor, where a command below that floor is simply rounded
+    // away by the wire encoding anyway, so continuing to send one just adds
+    // dither with no actual effect.  Unit-agnostic: error, deadzone and
+    // rate_max must all share one consistent unit (e.g. all in deg, all in
+    // rad/s, or all as a normalised -100..100 scalar) - gain converts between
+    // error's unit and the returned rate's
+    static float angle_error_to_rate(float error, float gain, float rate_max, float deadzone = 0.0f);
+
     // helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message
     uint16_t get_gimbal_device_flags() const;