GCS_MAVLink: fixed mavlink packet corruption with multiple threads

this ensures we don't try to send more data to a uart than is
available in the tx buffer

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -1051,6 +1051,7 @@ bool GCS_MAVLINK::do_try_send_message(const ap_message id)
     if (telemetry_delayed()) {
         return false;
     }
+    WITH_SEMAPHORE(comm_chan_lock(chan));
 #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
     void *data = hal.scheduler->disable_interrupts_save();
     uint32_t start_send_message_us = AP_HAL::micros();
@@ -2058,6 +2059,8 @@ void GCS_MAVLINK::service_statustext(void)
     // want to move idx.
     const uint16_t payload_size = PAYLOAD_SIZE(chan, STATUSTEXT);
     for (uint8_t idx=0; idx<_statustext_queue.available(); ) {
+        WITH_SEMAPHORE(comm_chan_lock(chan));
+
         if (txspace() < payload_size) {
             break;
         }

libraries/GCS_MAVLink/GCS_MAVLink.cpp

@@ -41,6 +41,7 @@ bool gcs_alternative_active[MAVLINK_COMM_NUM_BUFFERS];
 
 // per-channel lock
 static HAL_Semaphore chan_locks[MAVLINK_COMM_NUM_BUFFERS];
+static bool chan_discard[MAVLINK_COMM_NUM_BUFFERS];
 
 mavlink_system_t mavlink_system = {7,1};
 
@@ -90,7 +91,7 @@ uint16_t comm_get_txspace(mavlink_channel_t chan)
  */
 void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len)
 {
-    if (!valid_channel(chan) || mavlink_comm_port[chan] == nullptr) {
+    if (!valid_channel(chan) || mavlink_comm_port[chan] == nullptr || chan_discard[chan]) {
         return;
     }
     if (gcs_alternative_active[chan]) {
@@ -109,16 +110,34 @@ void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len)
 
 /*
   lock a channel for send
+  if there is insufficient space to send size bytes then all bytes
+  written to the channel by the mavlink library will be discarded
+  while the lock is held.
  */
-void comm_send_lock(mavlink_channel_t chan)
+void comm_send_lock(mavlink_channel_t chan_m, uint16_t size)
 {
-    chan_locks[(uint8_t)chan].take_blocking();
+    const uint8_t chan = uint8_t(chan_m);
+    chan_locks[chan].take_blocking();
+    if (mavlink_comm_port[chan]->txspace() < size) {
+        chan_discard[chan] = true;
+    }
 }
 
 /*
   unlock a channel
  */
-void comm_send_unlock(mavlink_channel_t chan)
+void comm_send_unlock(mavlink_channel_t chan_m)
+{
+    const uint8_t chan = uint8_t(chan_m);
+    chan_discard[chan] = false;
+    chan_locks[chan].give();
+}
+
+/*
+  return reference to GCS channel lock, allowing for
+  HAVE_PAYLOAD_SPACE() to be run with a locked channel
+ */
+HAL_Semaphore &comm_chan_lock(mavlink_channel_t chan)
 {
-    chan_locks[(uint8_t)chan].give();
+    return chan_locks[uint8_t(chan)];
 }

libraries/GCS_MAVLink/GCS_MAVLink.h

@@ -11,7 +11,7 @@
 
 #define MAVLINK_SEND_UART_BYTES(chan, buf, len) comm_send_buffer(chan, buf, len)
 
-#define MAVLINK_START_UART_SEND(chan, size) comm_send_lock(chan)
+#define MAVLINK_START_UART_SEND(chan, size) comm_send_lock(chan, size)
 #define MAVLINK_END_UART_SEND(chan, size) comm_send_unlock(chan)
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@@ -70,7 +70,8 @@ uint16_t comm_get_txspace(mavlink_channel_t chan);
 #include "include/mavlink/v2.0/all/mavlink.h"
 
 // lock and unlock a channel, for multi-threaded mavlink send
-void comm_send_lock(mavlink_channel_t chan);
+void comm_send_lock(mavlink_channel_t chan, uint16_t size);
 void comm_send_unlock(mavlink_channel_t chan);
+HAL_Semaphore &comm_chan_lock(mavlink_channel_t chan);
 
 #pragma GCC diagnostic pop