Fix multithreading bug in mavlink over serial port.

src/modules/mavlink/mavlink_bridge_header.h

@@ -47,12 +47,12 @@
 /* use efficient approach, see mavlink_helpers.h */
 #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
 
+#define MAVLINK_START_UART_SEND mavlink_start_uart_send
 #define MAVLINK_END_UART_SEND mavlink_end_uart_send
 
 #define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer
 #define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status
 
-#include <stdbool.h>			// Needs to be included before v2.0/mavlink_types.h until https://github.com/ArduPilot/pymavlink/pull/22 makes it through.
 #include <v2.0/mavlink_types.h>
 #include <unistd.h>
 
@@ -78,6 +78,7 @@ extern mavlink_system_t mavlink_system;
  */
 void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
 
+void mavlink_start_uart_send(mavlink_channel_t chan, int length);
 void mavlink_end_uart_send(mavlink_channel_t chan, int length);
 
 extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);

src/modules/mavlink/mavlink_main.cpp

@@ -118,6 +118,15 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int leng
 	}
 }
 
+void mavlink_start_uart_send(mavlink_channel_t chan, int length)
+{
+	Mavlink *m = Mavlink::get_instance((unsigned)chan);
+
+	if (m != nullptr) {
+		(void)m->begin_send();
+	}
+}
+
 void mavlink_end_uart_send(mavlink_channel_t chan, int length)
 {
 	Mavlink *m = Mavlink::get_instance((unsigned)chan);
@@ -901,6 +910,14 @@ Mavlink::get_free_tx_buf()
 	return buf_free;
 }
 
+void
+Mavlink::begin_send()
+{
+	// must protect the network buffer so other calls from receive_thread do not
+	// mangle the message.
+	pthread_mutex_lock(&_send_mutex);
+}
+
 int
 Mavlink::send_packet()
 {
@@ -910,6 +927,7 @@ Mavlink::send_packet()
 
 	/* Only send packets if there is something in the buffer. */
 	if (_network_buf_len == 0) {
+		pthread_mutex_unlock(&_send_mutex);
 		return 0;
 	}
 
@@ -955,6 +973,7 @@ Mavlink::send_packet()
 	_network_buf_len = 0;
 #endif
 
+	pthread_mutex_unlock(&_send_mutex);
 	return ret;
 }
 
@@ -967,8 +986,6 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 		return;
 	}
 
-	pthread_mutex_lock(&_send_mutex);
-
 	_last_write_try_time = hrt_absolute_time();
 
 	if (_mavlink_start_time == 0) {
@@ -983,7 +1000,6 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 			/* not enough space in buffer to send */
 			count_txerr();
 			count_txerrbytes(packet_len);
-			pthread_mutex_unlock(&_send_mutex);
 			return;
 		}
 	}
@@ -1017,7 +1033,6 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 		count_txbytes(packet_len);
 	}
 
-	pthread_mutex_unlock(&_send_mutex);
 }
 
 void

src/modules/mavlink/mavlink_main.h

@@ -171,11 +171,10 @@ public:
 		BROADCAST_MODE_ON
 	};
 
-	static const char *mavlink_mode_str(enum MAVLINK_MODE mode)
-	{
+	static const char *mavlink_mode_str(enum MAVLINK_MODE mode) {
 		switch (mode) {
 		case MAVLINK_MODE_NORMAL:
-			return "Normal";
+				return "Normal";
 
 		case MAVLINK_MODE_CUSTOM:
 			return "Custom";
@@ -273,6 +272,12 @@ public:
 	 */
 	bool			get_manual_input_mode_generation() { return _generate_rc; }
 
+
+	/**
+	 * This is the beginning of a MAVLINK_START_UART_SEND/MAVLINK_END_UART_SEND transaction
+	 */
+	void 			begin_send();
+
 	/**
 	 * Send bytes out on the link.
 	 *
@@ -285,7 +290,7 @@ public:
 	 *
 	 * @return the number of bytes sent or -1 in case of error
 	 */
-	int			send_packet();
+	int             send_packet();
 
 	/**
 	 * Resend message as is, don't change sequence number and CRC.
@@ -440,6 +445,7 @@ public:
 	MavlinkULog		*get_ulog_streaming() { return _mavlink_ulog; }
 	void			try_start_ulog_streaming(uint8_t target_system, uint8_t target_component) {
 		if (_mavlink_ulog) { return; }
+
 		_mavlink_ulog = MavlinkULog::try_start(_datarate, 0.7f, target_system, target_component);
 	}
 	void			request_stop_ulog_streaming() {