GCS_MAVLink: make mavlink send from multiple threads safe

this takes a lock to prevent interleaving of mavlink msgs from
multiple threads

libraries/GCS_MAVLink/GCS.cpp

@@ -67,5 +67,4 @@ bool GCS::install_alternative_protocol(mavlink_channel_t c, GCS_MAVLINK::protoco
     return true;
 }
 
-
 #undef FOR_EACH_ACTIVE_CHANNEL

libraries/GCS_MAVLink/GCS.h

@@ -648,7 +648,7 @@ public:
 
     // get the VFR_HUD throttle
     int16_t get_hud_throttle(void) const { return num_gcs()>0?chan(0).vfr_hud_throttle():0; }
-    
+
 private:
 
     static GCS *_singleton;
@@ -664,6 +664,11 @@ private:
     static const uint8_t _status_capacity = 30;
 #endif
 
+    // a lock for the statustext queue, to make it safe to use send_text()
+    // from multiple threads
+    HAL_Semaphore _statustext_sem;
+
+    // queue of outgoing statustext messages
     ObjectArray<statustext_t> _statustext_queue{_status_capacity};
 
     // true if we are running short on time in our main loop

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -1281,6 +1281,8 @@ void GCS::send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const cha
     statustext.msg.severity = severity;
     strncpy(statustext.msg.text, text, sizeof(statustext.msg.text));
 
+    _statustext_sem.take_blocking();
+    
     // The force push will ensure comm links do not block other comm links forever if they fail.
     // If we push to a full buffer then we overwrite the oldest entry, effectively removing the
     // block but not until the buffer fills up.
@@ -1293,6 +1295,8 @@ void GCS::send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const cha
     if (notify) {
         notify->send_text(text);
     }
+
+    _statustext_sem.give();
 }
 
 /*
@@ -1359,7 +1363,9 @@ void GCS::retry_deferred()
             chan(i).retry_deferred();
         }
     }
+    _statustext_sem.take_blocking();
     service_statustext();
+    _statustext_sem.give();
 }
 
 void GCS::data_stream_send()

libraries/GCS_MAVLink/GCS_MAVLink.cpp

@@ -26,6 +26,7 @@ This provides some support code and variables for MAVLink enabled sketches
 #include <AP_GPS/AP_GPS.h>
 #include <AP_HAL/AP_HAL.h>
 
+extern const AP_HAL::HAL& hal;
 
 #ifdef MAVLINK_SEPARATE_HELPERS
 // Shut up warnings about missing declarations; TODO: should be fixed on
@@ -39,6 +40,9 @@ This provides some support code and variables for MAVLink enabled sketches
 AP_HAL::UARTDriver	*mavlink_comm_port[MAVLINK_COMM_NUM_BUFFERS];
 bool gcs_alternative_active[MAVLINK_COMM_NUM_BUFFERS];
 
+// per-channel lock
+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
@@ -134,3 +138,19 @@ void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len)
     }
     mavlink_comm_port[chan]->write(buf, len);
 }
+
+/*
+  lock a channel for send
+ */
+void comm_send_lock(mavlink_channel_t chan)
+{
+    chan_locks[(uint8_t)chan].take_blocking();
+}
+
+/*
+  unlock a channel
+ */
+void comm_send_unlock(mavlink_channel_t chan)
+{
+    chan_locks[(uint8_t)chan].give();
+}

libraries/GCS_MAVLink/GCS_MAVLink.h

@@ -11,6 +11,9 @@
 
 #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_END_UART_SEND(chan, size) comm_send_unlock(chan)
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 // allow extra mavlink channels in SITL for:
 //    Vicon
@@ -72,4 +75,8 @@ uint16_t comm_get_txspace(mavlink_channel_t chan);
 // return a MAVLink variable type given a AP_Param type
 uint8_t mav_var_type(enum ap_var_type t);
 
+// lock and unlock a channel, for multi-threaded mavlink send
+void comm_send_lock(mavlink_channel_t chan);
+void comm_send_unlock(mavlink_channel_t chan);
+
 #pragma GCC diagnostic pop