MAVLink app: 1) only transmit active params, 2) send params faster, 3) ensure no overflow occurs on buffer when sending at higher rate.

3 files changed, 35 insertions, 20 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index f8e819ce7..632c8c709 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1361,7 +1361,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 	/* PARAM_VALUE stream */
 	_parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this);
-	_parameters_manager->set_interval(interval_from_rate(30.0f));
+	_parameters_manager->set_interval(interval_from_rate(80.0f));
 	LL_APPEND(_streams, _parameters_manager);
 
 	/* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index baaa7bc13..6eef594f5 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -143,6 +143,13 @@ public:
 
 	bool			get_forwarding_on() { return _forwarding_on; }
 
+	/**
+	 * Get the free space in the transmit buffer
+	 *
+	 * @return free space in the UART TX buffer
+	 */
+	unsigned		get_free_tx_buf();
+
 	static int		start_helper(int argc, char *argv[]);
 
 	/**
@@ -162,12 +169,12 @@ public:
 	 */
 	int			set_hil_enabled(bool hil_enabled);
 
-	void		send_message(const uint8_t msgid, const void *msg);
+	void			send_message(const uint8_t msgid, const void *msg);
 
 	/**
 	 * Resend message as is, don't change sequence number and CRC.
 	 */
-	void		resend_message(mavlink_message_t *msg);
+	void			resend_message(mavlink_message_t *msg);
 
 	void			handle_message(const mavlink_message_t *msg);
 
@@ -309,7 +316,7 @@ private:
 	int			_baudrate;
 	int			_datarate;		///< data rate for normal streams (attitude, position, etc.)
 	int			_datarate_events;	///< data rate for params, waypoints, text messages
-	float		_rate_mult;
+	float			_rate_mult;
 
 	/**
 	 * If the queue index is not at 0, the queue sending
@@ -331,9 +338,9 @@ private:
 	unsigned		_bytes_txerr;
 	unsigned		_bytes_rx;
 	uint64_t		_bytes_timestamp;
-	float		_rate_tx;
-	float		_rate_txerr;
-	float		_rate_rx;
+	float			_rate_tx;
+	float			_rate_txerr;
+	float			_rate_rx;
 
 	struct telemetry_status_s	_rstatus;			///< receive status
 
@@ -363,16 +370,9 @@ private:
 
 	void			mavlink_update_system();
 
-	int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
-
-	/**
-	 * Get the free space in the transmit buffer
-	 *
-	 * @return free space in the UART TX buffer
-	 */
-	unsigned			get_free_tx_buf();
+	int			mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
 
-	static unsigned int interval_from_rate(float rate);
+	static unsigned int	interval_from_rate(float rate);
 
 	int configure_stream(const char *stream_name, const float rate);
 
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
index e910f674a..e7e0c11df 100644
--- a/src/modules/mavlink/mavlink_parameters.cpp
+++ b/src/modules/mavlink/mavlink_parameters.cpp
@@ -184,8 +184,23 @@ MavlinkParametersManager::send(const hrt_abstime t)
 {
 	/* send all parameters if requested */
 	if (_send_all_index >= 0) {
-		send_param(param_for_index(_send_all_index));
-		_send_all_index++;
+
+		/* skip if no space is available */
+		if (_mavlink->get_free_tx_buf() < get_size()) {
+			return;
+		}
+
+		/* look for the first parameter which is used */
+		param_t p;
+		do {
+			p = param_for_index(_send_all_index);
+			_send_all_index++;
+		} while (p != PARAM_INVALID && !param_used(p));
+
+		if (p != PARAM_INVALID) {
+			send_param(p);
+		}
+
 		if (_send_all_index >= (int) param_count()) {
 			_send_all_index = -1;
 		}
@@ -209,8 +224,8 @@ MavlinkParametersManager::send_param(param_t param)
 		return;
 	}
 
-	msg.param_count = param_count();
-	msg.param_index = param_get_index(param);
+	msg.param_count = param_count_used();
+	msg.param_index = param_get_used_index(param);
 
 	/* copy parameter name */
 	strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);