From 45af77a5431a35444afd4d80e32aabc8feb36c66 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 14 Mar 2017 14:11:58 +0100
Subject: [PATCH] mavlink parameters: try to send 5 params at once on USB & UDP
 connections

This further speeds up param loading.

On Pixracer via USB, it's possible to send up to 11 at once before the TX
buffer gets full, so there is still enough free bandwidth left.
---
 src/modules/mavlink/mavlink_parameters.cpp | 25 ++++++++++++++++++++--
 src/modules/mavlink/mavlink_parameters.h   |  4 ++++
 2 files changed, 27 insertions(+), 2 deletions(-)

diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
index 671d2a36d6..def16cf297 100644
--- a/src/modules/mavlink/mavlink_parameters.cpp
+++ b/src/modules/mavlink/mavlink_parameters.cpp
@@ -318,6 +318,21 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 
 void
 MavlinkParametersManager::send(const hrt_abstime t)
+{
+	if (_mavlink->get_protocol() == SERIAL && !_mavlink->is_usb_uart()) {
+		send_one();
+
+	} else {
+		// speed up parameter loading via UDP, TCP or USB: try to send 5 at once
+		int i = 0;
+
+		while (i++ < 5 && send_one());
+	}
+}
+
+
+bool
+MavlinkParametersManager::send_one()
 {
 	bool space_available = _mavlink->get_free_tx_buf() >= get_size();
 
@@ -374,7 +389,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
 
 		/* skip if no space is available */
 		if (!space_available) {
-			return;
+			return false;
 		}
 
 		/* The first thing we send is a hash of all values for the ground
@@ -397,7 +412,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
 			_send_all_index = 0;
 
 			/* No further action, return now */
-			return;
+			return true;
 		}
 
 		/* look for the first parameter which is used */
@@ -415,6 +430,10 @@ MavlinkParametersManager::send(const hrt_abstime t)
 
 		if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) {
 			_send_all_index = -1;
+			return false;
+
+		} else {
+			return true;
 		}
 
 	} else if (_send_all_index == PARAM_HASH && hrt_absolute_time() > 20 * 1000 * 1000) {
@@ -422,6 +441,8 @@ MavlinkParametersManager::send(const hrt_abstime t)
 		_mavlink->send_statustext_critical("WARNING: SYSTEM BOOT INCOMPLETE. CHECK CONFIG.");
 		_mavlink->set_boot_complete();
 	}
+
+	return false;
 }
 
 int
diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h
index 9e0e875787..3f6bbc0f73 100644
--- a/src/modules/mavlink/mavlink_parameters.h
+++ b/src/modules/mavlink/mavlink_parameters.h
@@ -91,6 +91,10 @@ protected:
 
 	void send(const hrt_abstime t);
 
+	/// send a single param if a PARAM_REQUEST_LIST is in progress
+	/// @return true if a parameter was sent
+	bool send_one();
+
 	int send_param(param_t param, int component_id = -1);
 
 	// Item of a single-linked list to store requested uavcan parameters