Mount_SToRM32: find gimbal channel on startup

libraries/AP_Mount/AP_Mount_SToRM32.cpp

@@ -9,25 +9,18 @@ extern const AP_HAL::HAL& hal;
 AP_Mount_SToRM32::AP_Mount_SToRM32(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) :
     AP_Mount_Backend(frontend, state, instance),
     _initialised(false),
+    _sysid(0),
+    _compid(0),
     _chan(MAVLINK_COMM_0),
     _last_send(0)
 {}
 
-// init - performs any required initialisation for this instance
-void AP_Mount_SToRM32::init(const AP_SerialManager& serial_manager)
-{
-    // get_mavlink_channel for MAVLink2
-    if (serial_manager.get_mavlink_channel(AP_SerialManager::SerialProtocol_MAVLink, 1, _chan)) {
-        _initialised = true;
-        set_mode((enum MAV_MOUNT_MODE)_state._default_mode.get());
-    }
-}
-
 // update mount position - should be called periodically
 void AP_Mount_SToRM32::update()
 {
     // exit immediately if not initialised
     if (!_initialised) {
+        find_gimbal();
         return;
     }
 
@@ -113,6 +106,24 @@ void AP_Mount_SToRM32::status_msg(mavlink_channel_t chan)
     mavlink_msg_mount_status_send(chan, 0, 0, ToDeg(_angle_ef_target_rad.y)*100, ToDeg(_angle_ef_target_rad.x)*100, ToDeg(_angle_ef_target_rad.z)*100);
 }
 
+// search for gimbal in GCS_MAVLink routing table
+void AP_Mount_SToRM32::find_gimbal()
+{
+    // return immediately if initialised
+    if (_initialised) {
+        return;
+    }
+
+    // return if search time has has passed
+    if (hal.scheduler->millis() > AP_MOUNT_STORM32_SEARCH_MS) {
+        return;
+    }
+
+    if (GCS_MAVLINK::find_by_mavtype(MAV_TYPE_GIMBAL, _sysid, _compid, _chan)) {
+        _initialised = true;
+    }
+}
+
 // send_do_mount_control - send a COMMAND_LONG containing a do_mount_control message
 void AP_Mount_SToRM32::send_do_mount_control(float pitch_deg, float roll_deg, float yaw_deg, enum MAV_MOUNT_MODE mount_mode)
 {
@@ -121,14 +132,19 @@ void AP_Mount_SToRM32::send_do_mount_control(float pitch_deg, float roll_deg, fl
         return;
     }
 
+    // check we have space for the message
+    if (comm_get_txspace(_chan) < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_COMMAND_LONG_LEN) {
+        return;
+    }
+
     // reverse pitch and yaw control
     pitch_deg = -pitch_deg;
     yaw_deg = -yaw_deg;
 
     // send command_long command containing a do_mount_control command
     mavlink_msg_command_long_send(_chan,
-                                  AP_MOUNT_STORM32_SYSID,
+                                  _sysid,
-                                  AP_MOUNT_STORM32_COMPID,
+                                  _compid,
                                   MAV_CMD_DO_MOUNT_CONTROL,
                                   0,        // confirmation of zero means this is the first time this message has been sent
                                   pitch_deg,

libraries/AP_Mount/AP_Mount_SToRM32.h

@@ -13,14 +13,13 @@
 #include <AP_Math.h>
 #include <AP_Common.h>
 #include <AP_GPS.h>
+#include <GCS.h>
 #include <GCS_MAVLink.h>
 #include <RC_Channel.h>
 #include <AP_Mount_Backend.h>
 
-#define AP_MOUNT_STORM32_SYSID      71      // hardcoded system id
-#define AP_MOUNT_STORM32_COMPID     67      // hard coded component id for communicating with the gimbal
-
 #define AP_MOUNT_STORM32_RESEND_MS  1000    // resend angle targets to gimbal once per second
+#define AP_MOUNT_STORM32_SEARCH_MS  60000   // search for gimbal for 1 minute after startup
 
 class AP_Mount_SToRM32 : public AP_Mount_Backend
 {
@@ -30,7 +29,7 @@ public:
     AP_Mount_SToRM32(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance);
 
     // init - performs any required initialisation for this instance
-    virtual void init(const AP_SerialManager& serial_manager);
+    virtual void init(const AP_SerialManager& serial_manager) {}
 
     // update mount position - should be called periodically
     virtual void update();
@@ -46,11 +45,16 @@ public:
 
 private:
 
+    // search for gimbal in GCS_MAVLink routing table
+    void find_gimbal();
+
     // send_do_mount_control - send a COMMAND_LONG containing a do_mount_control message
     void send_do_mount_control(float pitch_deg, float roll_deg, float yaw_deg, enum MAV_MOUNT_MODE mount_mode);
 
     // internal variables
     bool _initialised;              // true once the driver has been initialised
+    uint8_t _sysid;                 // sysid of gimbal
+    uint8_t _compid;                // component id of gimbal
     mavlink_channel_t _chan;        // mavlink channel used to communicate with gimbal.  Currently hard-coded to Telem2
     uint32_t _last_send;            // system time of last do_mount_control sent to gimbal
 };