mavlink: Only send event-based messages if there is space in the buffer

3 files changed, 84 insertions, 6 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index ea60bb226..e6b558ca6 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -246,7 +246,8 @@ Mavlink::Mavlink() :
 	   _ftp_on(false),
 	   _uart_fd(-1),
 	   _baudrate(57600),
-	   _datarate(10000),
+	   _datarate(1000),
+	   _datarate_events(500),
 	   _mavlink_param_queue_index(0),
 	   mavlink_link_termination_allowed(false),
 	   _subscribe_to_stream(nullptr),
@@ -352,6 +353,18 @@ Mavlink::count_txerr()
 	perf_count(_txerr_perf);
 }
 
+unsigned
+Mavlink::get_free_tx_buf()
+{
+	unsigned buf_free;
+
+	if (!ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free)) {
+		return buf_free;
+	} else {
+		return 0;
+	}
+}
+
 void
 Mavlink::set_mode(enum MAVLINK_MODE mode)
 {
@@ -1084,6 +1097,31 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 }
 
 void
+Mavlink::adjust_stream_rates(const float multiplier)
+{
+	/* do not allow to push us to zero */
+	if (multiplier < 0.01f) {
+		return;
+	}
+
+	/* search if stream exists */
+	MavlinkStream *stream;
+	LL_FOREACH(_streams, stream) {
+		/* set new interval */
+		unsigned interval = stream->get_interval();
+		interval /= multiplier;
+
+		/* allow max ~600 Hz */
+		if (interval < 1600) {
+			interval = 1600;
+		} 
+
+		/* set new interval */
+		stream->set_interval(interval * multiplier);
+	}
+}
+
+void
 Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
 {
 	/* orb subscription must be done from the main thread,
@@ -1464,7 +1502,7 @@ Mavlink::task_main(int argc, char *argv[])
 	/* don't send parameters on startup without request */
 	_mavlink_param_queue_index = param_count();
 
-	MavlinkRateLimiter fast_rate_limiter(35000.0f / rate_mult);
+	MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult);
 
 	/* set main loop delay depending on data rate to minimize CPU overhead */
 	_main_loop_delay = MAIN_LOOP_DELAY / rate_mult;
@@ -1519,10 +1557,22 @@ Mavlink::task_main(int argc, char *argv[])
 		}
 
 		if (fast_rate_limiter.check(t)) {
-			mavlink_pm_queued_send();
-			_mission_manager->eventloop();
 
-			if (!mavlink_logbuffer_is_empty(&_logbuffer)) {
+			unsigned buf_free = get_free_tx_buf();
+
+			/* only send messages if they fit the buffer */
+			if (buf_free >= (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) {
+				mavlink_pm_queued_send();
+			}
+
+			buf_free = get_free_tx_buf();
+			if (buf_free >= (MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) {
+				_mission_manager->eventloop();
+			}
+
+			buf_free = get_free_tx_buf();
+			if (buf_free >= (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) &&
+				(!mavlink_logbuffer_is_empty(&_logbuffer))) {
 				struct mavlink_logmessage msg;
 				int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);
 
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 70d13acb0..e9d6b9c86 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -248,6 +248,13 @@ public:
 	void			count_rxbytes(unsigned n) { _bytes_rx += n; };
 
 	/**
+	 * Get the free space in the transmit buffer
+	 *
+	 * @return free space in the UART TX buffer
+	 */
+	unsigned		get_free_tx_buf();
+
+	/**
 	 * Get the receive status of this MAVLink link
 	 */
 	struct telemetry_status_s&	get_rx_status() { return _rstatus; }
@@ -292,7 +299,8 @@ private:
 	bool			_ftp_on;
 	int			_uart_fd;
 	int			_baudrate;
-	int			_datarate;
+	int			_datarate;		///< data rate for normal streams (attitude, position, etc.)
+	int			_datarate_events;	///< data rate for params, waypoints, text messages
 
 	/**
 	 * If the queue index is not at 0, the queue sending
@@ -385,6 +393,13 @@ private:
 
 	int configure_stream(const char *stream_name, const float rate);
 
+	/**
+	 * Adjust the stream rates based on the current rate
+	 *
+	 * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease
+	 */
+	void adjust_stream_rates(const float multiplier);
+
 	int message_buffer_init(int size);
 
 	void message_buffer_destroy();
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index 20e1c7c44..f539d225b 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -56,7 +56,20 @@ public:
 
 	MavlinkStream();
 	virtual ~MavlinkStream();
+
+	/**
+	 * Get the interval
+	 *
+	 * @param interval the inveral in microseconds (us) between messages
+	 */
 	void set_interval(const unsigned int interval);
+
+	/**
+	 * Get the interval
+	 *
+	 * @return the inveral in microseconds (us) between messages
+	 */
+	unsigned get_interval() { return _interval; }
 	void set_channel(mavlink_channel_t channel);
 
 	/**