GCS_MAVLink: correct txspace return value issues

- checking of space in send_to_active_channels was incorrect - did not
take into account locked status of the channel

 - corrected return value on comm_get_txspace - took a uint32_t, cast it
to int16_t, checked it for zero, then cast it to uint16_t on return.
That's just... odd.

libraries/GCS_MAVLink/GCS.cpp

@@ -60,7 +60,7 @@ void GCS::send_to_active_channels(uint32_t msgid, const char *pkt)
         if (!c.is_active()) {
             continue;
         }
-        if (entry->max_msg_len + c.packet_overhead() > c.get_uart()->txspace()) {
+        if (entry->max_msg_len + c.packet_overhead() > c.txspace()) {
             // no room on this channel
             continue;
         }

libraries/GCS_MAVLink/GCS.h

@@ -25,9 +25,9 @@
 #define GCS_DEBUG_SEND_MESSAGE_TIMINGS 0
 
 // check if a message will fit in the payload space available
-#define PAYLOAD_SIZE(chan, id) (GCS_MAVLINK::packet_overhead_chan(chan)+MAVLINK_MSG_ID_ ## id ## _LEN)
+#define PAYLOAD_SIZE(chan, id) (unsigned(GCS_MAVLINK::packet_overhead_chan(chan)+MAVLINK_MSG_ID_ ## id ## _LEN))
 #define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= PAYLOAD_SIZE(chan, id))
-#define CHECK_PAYLOAD_SIZE(id) if (comm_get_txspace(chan) < packet_overhead()+MAVLINK_MSG_ID_ ## id ## _LEN) return false
+#define CHECK_PAYLOAD_SIZE(id) if (txspace() < unsigned(packet_overhead()+MAVLINK_MSG_ID_ ## id ## _LEN)) return false
 #define CHECK_PAYLOAD_SIZE2(id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return false
 
 // convenience macros for defining which ap_message ids are in which streams:
@@ -77,6 +77,17 @@ public:
     // returns true if we are requesting any items from the GCS:
     bool requesting_mission_items() const;
 
+    /// Check for available transmit space
+    uint16_t txspace() const {
+        if (_locked) {
+            return 0;
+        }
+        // there were concerns over return a too-large value for
+        // txspace (in case we tried to do too much with the space in
+        // a single loop):
+        return MIN(_port->txspace(), 8192U);
+    }
+
     void send_mission_ack(const mavlink_message_t &msg,
                           MAV_MISSION_TYPE mission_type,
                           MAV_MISSION_RESULT result) const {
@@ -216,7 +227,14 @@ public:
     void send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc);
     void send_rpm() const;
 
-    bool locked() const;
+    // lock a channel, preventing use by MAVLink
+    void lock(bool _lock) {
+        _locked = _lock;
+    }
+    // returns true if this channel isn't available for MAVLink
+    bool locked() const {
+        return _locked;
+    }
 
     // return a bitmap of active channels. Used by libraries to loop
     // over active channels to send to all active channels    
@@ -775,6 +793,10 @@ private:
 #endif
 
     uint32_t last_mavlink_stats_logged;
+
+    // true if we should NOT do MAVLink on this port (usually because
+    // someone's doing SERIAL_CONTROL over mavlink)
+    bool _locked;
 };
 
 /// @class GCS

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -2329,7 +2329,7 @@ bool GCS_MAVLINK::get_ap_message_interval(ap_message id, uint16_t &interval_ms)
 
 MAV_RESULT GCS_MAVLINK::handle_command_get_message_interval(const mavlink_command_long_t &packet)
 {
-    if (comm_get_txspace(chan) < PAYLOAD_SIZE(chan, MESSAGE_INTERVAL) + PAYLOAD_SIZE(chan, COMMAND_ACK)) {
+    if (txspace() < PAYLOAD_SIZE(chan, MESSAGE_INTERVAL) + PAYLOAD_SIZE(chan, COMMAND_ACK)) {
         return MAV_RESULT_TEMPORARILY_REJECTED;
     }
 

libraries/GCS_MAVLink/GCS_Dummy.h

@@ -43,8 +43,8 @@ protected:
     MAV_MODE base_mode() const override { return (MAV_MODE)MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; }
     MAV_STATE system_status() const override { return MAV_STATE_CALIBRATING; }
 
-    bool set_home_to_current_location(bool lock) override { return false; }
-    bool set_home(const Location& loc, bool lock) override { return false; }
+    bool set_home_to_current_location(bool _lock) override { return false; }
+    bool set_home(const Location& loc, bool _lock) override { return false; }
 
     void send_nav_controller_output() const override {};
     void send_pid_tuning() override {};

libraries/GCS_MAVLink/GCS_MAVLink.cpp

@@ -44,32 +44,9 @@ static HAL_Semaphore chan_locks[MAVLINK_COMM_NUM_BUFFERS];
 
 mavlink_system_t mavlink_system = {7,1};
 
-// mask of serial ports disabled to allow for SERIAL_CONTROL
-static uint8_t mavlink_locked_mask;
-
 // routing table
 MAVLink_routing GCS_MAVLINK::routing;
 
-/*
-  lock a channel, preventing use by MAVLink
- */
-void GCS_MAVLINK::lock_channel(mavlink_channel_t _chan, bool lock)
-{
-    if (!valid_channel(chan)) {
-        return;
-    }
-    if (lock) {
-        mavlink_locked_mask |= (1U<<(unsigned)_chan);
-    } else {
-        mavlink_locked_mask &= ~(1U<<(unsigned)_chan);
-    }
-}
-
-bool GCS_MAVLINK::locked() const
-{
-    return (1U<<chan) & mavlink_locked_mask;
-}
-
 // set a channel as private. Private channels get sent heartbeats, but
 // don't get broadcast packets or forwarded packets
 void GCS_MAVLINK::set_channel_private(mavlink_channel_t _chan)
@@ -102,17 +79,11 @@ MAV_PARAM_TYPE GCS_MAVLINK::mav_param_type(enum ap_var_type t)
 /// @returns		Number of bytes available
 uint16_t comm_get_txspace(mavlink_channel_t chan)
 {
-    if (!valid_channel(chan)) {
+    GCS_MAVLINK *link = gcs().chan(chan);
+    if (link == nullptr) {
         return 0;
     }
-    if ((1U<<chan) & mavlink_locked_mask) {
-        return 0;
-    }
-	int16_t ret = mavlink_comm_port[chan]->txspace();
-	if (ret < 0) {
-		ret = 0;
-	}
-    return (uint16_t)ret;
+    return link->txspace();
 }
 
 /*

libraries/GCS_MAVLink/GCS_Param.cpp

@@ -49,8 +49,8 @@ GCS_MAVLINK::queued_param_send()
     if (bytes_allowed < size_for_one_param_value_msg) {
         bytes_allowed = size_for_one_param_value_msg;
     }
-    if (bytes_allowed > comm_get_txspace(chan)) {
-        bytes_allowed = comm_get_txspace(chan);
+    if (bytes_allowed > txspace()) {
+        bytes_allowed = txspace();
     }
     uint32_t count = bytes_allowed / size_for_one_param_value_msg;
 

libraries/GCS_MAVLink/GCS_serial_control.cpp

@@ -43,14 +43,24 @@ void GCS_MAVLINK::handle_serial_control(const mavlink_message_t &msg)
     bool exclusive = (packet.flags & SERIAL_CONTROL_FLAG_EXCLUSIVE) != 0;
 
     switch (packet.device) {
-    case SERIAL_CONTROL_DEV_TELEM1:
-        stream = port = hal.uartC;
-        lock_channel(MAVLINK_COMM_1, exclusive);
+    case SERIAL_CONTROL_DEV_TELEM1: {
+        GCS_MAVLINK *link = gcs().chan(1);
+        if (link == nullptr) {
+            break;
+        }
+        stream = port = link->get_uart();
+        link->lock(exclusive);
         break;
-    case SERIAL_CONTROL_DEV_TELEM2:
-        stream = port = hal.uartD;
-        lock_channel(MAVLINK_COMM_2, exclusive);
+    }
+    case SERIAL_CONTROL_DEV_TELEM2: {
+        GCS_MAVLINK *link = gcs().chan(2);
+        if (link == nullptr) {
+            break;
+        }
+        stream = port = link->get_uart();
+        link->lock(exclusive);
         break;
+    }
     case SERIAL_CONTROL_DEV_GPS1:
         stream = port = hal.uartB;
         AP::gps().lock_port(0, exclusive);