AP_Mount: correct double-mapping of port to channel number

set_message_interval takes a port number, not a channel number (it was originally written as an internal function, after all).  Before this patch we were double-mapping from one to the other.  That works so long as the port you are mapping also corresponds to the chanel number - which it will, for example, if you are using serial2 with both serial0 and serial1 also set to mavlink.  If you set serial5_protocol to 2 an attempt to use it for controlling a gremsy it will *not* work because we map into backwards twice.

libraries/AP_Mount/AP_Mount_Gremsy.cpp

@@ -97,7 +97,7 @@ void AP_Mount_Gremsy::update()
 bool AP_Mount_Gremsy::healthy() const
 {
     // unhealthy until gimbal has been found and replied with device info
-    if (!_found_gimbal || !_got_device_info) {
+    if (_link == nullptr || !_got_device_info) {
         return false;
     }
 
@@ -149,17 +149,16 @@ void AP_Mount_Gremsy::find_gimbal()
     }
 
     // search for a mavlink enabled gimbal
-    if (!_found_gimbal) {
+    if (_link == nullptr) {
         // we expect that instance 0 has compid = MAV_COMP_ID_GIMBAL, instance 1 has compid = MAV_COMP_ID_GIMBAL2, etc
         uint8_t compid = (_instance == 0) ? MAV_COMP_ID_GIMBAL : MAV_COMP_ID_GIMBAL2 + (_instance - 1);
-        if (GCS_MAVLINK::find_by_mavtype_and_compid(MAV_TYPE_GIMBAL, compid, _sysid, _chan)) {
-            _compid = compid;
-            _found_gimbal = true;
-            return;
-        } else {
+        _link = GCS_MAVLINK::find_by_mavtype_and_compid(MAV_TYPE_GIMBAL, compid, _sysid);
+        if (_link == nullptr) {
             // have not yet found a gimbal so return
             return;
         }
+
+        _compid = compid;
     }
 
     // request GIMBAL_DEVICE_INFORMATION
@@ -229,6 +228,11 @@ void AP_Mount_Gremsy::handle_gimbal_device_attitude_status(const mavlink_message
 // request GIMBAL_DEVICE_INFORMATION message
 void AP_Mount_Gremsy::request_gimbal_device_information() const
 {
+    if (_link == nullptr) {
+        return;
+    }
+    const mavlink_channel_t _chan = _link->get_chan();
+
     // check we have space for the message
     if (!HAVE_PAYLOAD_SPACE(_chan, COMMAND_LONG)) {
         return;
@@ -245,8 +249,12 @@ void AP_Mount_Gremsy::request_gimbal_device_information() const
 // start sending ATTITUDE and AUTOPILOT_STATE_FOR_GIMBAL_DEVICE to gimbal
 bool AP_Mount_Gremsy::start_sending_attitude_to_gimbal()
 {
+    // better safe than sorry:
+    if (_link == nullptr) {
+        return false;
+    }
     // send AUTOPILOT_STATE_FOR_GIMBAL_DEVICE
-    const MAV_RESULT res = gcs().set_message_interval(_chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, AP_MOUNT_GREMSY_ATTITUDE_INTERVAL_US);
+    const MAV_RESULT res = _link->set_message_interval(MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, AP_MOUNT_GREMSY_ATTITUDE_INTERVAL_US);
 
     // return true on success
     return (res == MAV_RESULT_ACCEPTED);
@@ -255,6 +263,11 @@ bool AP_Mount_Gremsy::start_sending_attitude_to_gimbal()
 // send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to command gimbal to retract (aka relax)
 void AP_Mount_Gremsy::send_gimbal_device_retract() const
 {
+    if (_link == nullptr) {
+        return;
+    }
+    const mavlink_channel_t _chan = _link->get_chan();
+
     // check we have space for the message
     if (!HAVE_PAYLOAD_SPACE(_chan, GIMBAL_DEVICE_SET_ATTITUDE)) {
         return;
@@ -274,6 +287,11 @@ void AP_Mount_Gremsy::send_gimbal_device_retract() const
 // earth_frame should be true if yaw_rads target is an earth frame rate, false if body_frame
 void AP_Mount_Gremsy::send_gimbal_device_set_rate(float roll_rads, float pitch_rads, float yaw_rads, bool earth_frame) const
 {
+    if (_link == nullptr) {
+        return;
+    }
+    const mavlink_channel_t _chan = _link->get_chan();
+
     // check we have space for the message
     if (!HAVE_PAYLOAD_SPACE(_chan, GIMBAL_DEVICE_SET_ATTITUDE)) {
         return;
@@ -301,6 +319,11 @@ void AP_Mount_Gremsy::send_gimbal_device_set_attitude(float roll_rad, float pitc
         return;
     }
 
+    if (_link == nullptr) {
+        return;
+    }
+    const mavlink_channel_t _chan = _link->get_chan();
+
     // check we have space for the message
     if (!HAVE_PAYLOAD_SPACE(_chan, GIMBAL_DEVICE_SET_ATTITUDE)) {
         return;

libraries/AP_Mount/AP_Mount_Gremsy.h

@@ -71,11 +71,10 @@ private:
     void send_gimbal_device_set_attitude(float roll_rad, float pitch_rad, float yaw_rad, bool earth_frame) const;
 
     // internal variables
-    bool _found_gimbal;             // true once a MAVLink enabled gimbal has been found
     bool _got_device_info;          // true once gimbal has provided device info
     bool _initialised;              // true once the gimbal has provided a GIMBAL_DEVICE_INFORMATION
     uint32_t _last_devinfo_req_ms;  // system time that GIMBAL_DEVICE_INFORMATION was last requested (used to throttle requests)
-    mavlink_channel_t _chan;        // mavlink channel used to communicate with gimbal
+    class GCS_MAVLINK *_link;       // link we have found gimbal on; nullptr if not seen yet
     uint8_t _sysid;                 // sysid of gimbal
     uint8_t _compid;                // component id of gimbal
     mavlink_gimbal_device_attitude_status_t _gimbal_device_attitude_status;  // copy of most recently received gimbal status