WIP: mavlink send hacks

src/modules/mavlink/mavlink_main.cpp

@@ -106,7 +106,7 @@ void mavlink_start_uart_send(mavlink_channel_t chan, int length)
 	Mavlink *m = Mavlink::get_instance(chan);
 
 	if (m != nullptr) {
-		(void)m->begin_send();
+		m->send_start(length);
 #ifdef MAVLINK_PRINT_PACKETS
 		printf("START PACKET (%u): ", (unsigned)chan);
 #endif
@@ -118,7 +118,7 @@ void mavlink_end_uart_send(mavlink_channel_t chan, int length)
 	Mavlink *m = Mavlink::get_instance(chan);
 
 	if (m != nullptr) {
-		(void)m->send_packet();
+		m->send_finish();
 #ifdef MAVLINK_PRINT_PACKETS
 		printf("\n");
 #endif
@@ -187,6 +187,11 @@ Mavlink::Mavlink() :
 
 Mavlink::~Mavlink()
 {
+	perf_free(_loop_perf);
+	perf_free(_loop_interval_perf);
+	perf_free(_send_byte_error_perf);
+	perf_free(_send_start_tx_buf_low);
+
 	if (_task_running) {
 		/* task wakes up every 10ms or so at the longest */
 		_task_should_exit = true;
@@ -749,12 +754,13 @@ Mavlink::get_free_tx_buf()
 	} else
 #endif // MAVLINK_UDP
 	{
-		// No FIONSPACE on Linux todo:use SIOCOUTQ  and queue size to emulate FIONSPACE
+
-#if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN)
+#if defined(__PX4_NUTTX)
-		//Linux cp210x does not support TIOCOUTQ
-		buf_free = 256;
-#else
 		(void) ioctl(_uart_fd, FIONSPACE, (unsigned long)&buf_free);
+#else
+		// No FIONSPACE on Linux todo:use SIOCOUTQ  and queue size to emulate FIONSPACE
+		//Linux cp210x does not support TIOCOUTQ
+		buf_free = MAVLINK_MAX_PACKET_LEN;
 #endif
 
 		if (_flow_control_mode == FLOW_CONTROL_AUTO && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) {
@@ -774,45 +780,64 @@ Mavlink::get_free_tx_buf()
 	return buf_free;
 }
 
-int
+void Mavlink::send_start(int length)
-Mavlink::send_packet()
 {
+	pthread_mutex_lock(&_send_mutex);
+	_last_write_try_time = hrt_absolute_time();
+
+	// check if there is space in the buffer
+	if (length > (int)get_free_tx_buf()) {
+		// not enough space in buffer to send
+		count_txerrbytes(length);
+
+		// prevent writes
+		_tx_buffer_low = true;
+
+		perf_count(_send_start_tx_buf_low);
+
+	} else {
+		_tx_buffer_low = false;
+	}
+}
+
+void Mavlink::send_finish()
+{
+	if (_tx_buffer_low || (_buf_fill == 0)) {
+		pthread_mutex_unlock(&_send_mutex);
+		return;
+	}
+
 	int ret = -1;
 
+	// send message to UART
+	if (get_protocol() == Protocol::SERIAL) {
+		ret = ::write(_uart_fd, _buf, _buf_fill);
+	}
+
 #if defined(MAVLINK_UDP)
 
-	/* Only send packets if there is something in the buffer. */
+	else if (get_protocol() == Protocol::UDP) {
-	if (_network_buf_len == 0) {
-		pthread_mutex_unlock(&_send_mutex);
-		return 0;
-	}
 
-	if (get_protocol() == Protocol::UDP) {
+# if defined(CONFIG_NET)
-
-#ifdef CONFIG_NET
 
 		if (_src_addr_initialized) {
-#endif
+# endif // CONFIG_NET
-			ret = sendto(_socket_fd, _network_buf, _network_buf_len, 0,
+			ret = sendto(_socket_fd, _buf, _buf_fill, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
-				     (struct sockaddr *)&_src_addr, sizeof(_src_addr));
+# if defined(CONFIG_NET)
-#ifdef CONFIG_NET
 		}
 
-#endif
+# endif // CONFIG_NET
 
-		/* resend message via broadcast if no valid connection exists */
 		if ((_mode != MAVLINK_MODE_ONBOARD) && broadcast_enabled() &&
-		    (!get_client_source_initialized()
+		    (!get_client_source_initialized() || (hrt_elapsed_time(&_tstatus.heartbeat_time) > 3_s))) {
-		     || (hrt_elapsed_time(&_tstatus.heartbeat_time) > 3_s))) {
 
 			if (!_broadcast_address_found) {
 				find_broadcast_address();
 			}
 
-			if (_broadcast_address_found && _network_buf_len > 0) {
+			if (_broadcast_address_found && _buf_fill > 0) {
 
-				int bret = sendto(_socket_fd, _network_buf, _network_buf_len, 0,
+				int bret = sendto(_socket_fd, _buf, _buf_fill, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));
-						  (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));
 
 				if (bret <= 0) {
 					if (!_broadcast_failed_warned) {
@@ -825,69 +850,38 @@ Mavlink::send_packet()
 				}
 			}
 		}
-
-	}
-
-	_network_buf_len = 0;
-
-#endif // MAVLINK_UDP
-
-	pthread_mutex_unlock(&_send_mutex);
-	return ret;
-}
-
-void
-Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
-{
-	_last_write_try_time = hrt_absolute_time();
-
-	if (_mavlink_start_time == 0) {
-		_mavlink_start_time = _last_write_try_time;
-	}
-
-	if (get_protocol() == Protocol::SERIAL) {
-		/* check if there is space in the buffer, let it overflow else */
-		unsigned buf_free = get_free_tx_buf();
-
-		if (buf_free < packet_len) {
-			/* not enough space in buffer to send */
-			count_txerrbytes(packet_len);
-			return;
-		}
-	}
-
-	size_t ret = -1;
-
-	/* send message to UART */
-	if (get_protocol() == Protocol::SERIAL) {
-		ret = ::write(_uart_fd, buf, packet_len);
-	}
-
-#if defined(MAVLINK_UDP)
-
-	else {
-		if (_network_buf_len + packet_len < sizeof(_network_buf) / sizeof(_network_buf[0])) {
-			memcpy(&_network_buf[_network_buf_len], buf, packet_len);
-			_network_buf_len += packet_len;
-
-			ret = packet_len;
-		}
 	}
 
 #endif // MAVLINK_UDP
 
-	if (ret != (size_t) packet_len) {
+	if (ret == (int)_buf_fill) {
-		count_txerrbytes(packet_len);
+		count_txbytes(_buf_fill);
+		_last_write_success_time = _last_write_try_time;
 
 	} else {
-		_last_write_success_time = _last_write_try_time;
+		count_txerrbytes(_buf_fill);
-		count_txbytes(packet_len);
+	}
+
+	_buf_fill = 0;
+
+	pthread_mutex_unlock(&_send_mutex);
+}
+
+void Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
+{
+	if (!_tx_buffer_low) {
+		if (_buf_fill + packet_len < sizeof(_buf)) {
+			memcpy(&_buf[_buf_fill], buf, packet_len);
+			_buf_fill += packet_len;
+
+		} else {
+			perf_count(_send_byte_error_perf);
+		}
 	}
 }
 
 #ifdef MAVLINK_UDP
-void
+void Mavlink::find_broadcast_address()
-Mavlink::find_broadcast_address()
 {
 #if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN)
 	struct ifconf ifconf;
@@ -2210,6 +2204,8 @@ Mavlink::task_main(int argc, char *argv[])
 	/* start the MAVLink receiver last to avoid a race */
 	MavlinkReceiver::receive_start(&_receive_thread, this);
 
+	_mavlink_start_time = hrt_absolute_time();
+
 	while (!_task_should_exit) {
 		/* main loop */
 		px4_usleep(_main_loop_delay);

src/modules/mavlink/mavlink_main.h

@@ -318,21 +318,17 @@ public:
 	/**
 	 * This is the beginning of a MAVLINK_START_UART_SEND/MAVLINK_END_UART_SEND transaction
 	 */
-	void 			begin_send() { pthread_mutex_lock(&_send_mutex); }
+	void 			send_start(int length);
 
 	/**
-	 * Send bytes out on the link.
+	 * Buffer bytes to send out on the link.
-	 *
-	 * On a network port these might actually get buffered to form a packet.
 	 */
 	void			send_bytes(const uint8_t *buf, unsigned packet_len);
 
 	/**
 	 * Flush the transmit buffer and send one MAVLink packet
-	 *
-	 * @return the number of bytes sent or -1 in case of error
 	 */
-	int             	send_packet();
+	void             	send_finish();
 
 	/**
 	 * Resend message as is, don't change sequence number and CRC.
@@ -619,13 +615,16 @@ private:
 	bool			_broadcast_address_found{false};
 	bool			_broadcast_address_not_found_warned{false};
 	bool			_broadcast_failed_warned{false};
-	uint8_t			_network_buf[MAVLINK_MAX_PACKET_LEN] {};
-	unsigned		_network_buf_len{0};
 
 	unsigned short		_network_port{14556};
 	unsigned short		_remote_port{DEFAULT_REMOTE_PORT_UDP};
 #endif // MAVLINK_UDP
 
+	uint8_t			_buf[MAVLINK_MAX_PACKET_LEN] {};
+	unsigned		_buf_fill{0};
+
+	bool			_tx_buffer_low{false};
+
 	const char 		*_interface_name{nullptr};
 
 	int			_socket_fd{-1};
@@ -668,6 +667,8 @@ private:
 
 	perf_counter_t		_loop_perf{perf_alloc(PC_ELAPSED, "mavlink_el")};		/**< loop performance counter */
 	perf_counter_t		_loop_interval_perf{perf_alloc(PC_INTERVAL, "mavlink_int")};	/**< loop interval performance counter */
+	perf_counter_t		_send_byte_error_perf{perf_alloc(PC_COUNT, "mavlink_send_bytes_error")};	/**< send bytes error count */
+	perf_counter_t		_send_start_tx_buf_low{perf_alloc(PC_COUNT, "mavlink_tx_buf_low")};	/**< available tx buffer smaller than message */
 
 	void			mavlink_update_parameters();