From 494f35d5f420e434842f6684a83f323bce2ce420 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Wed, 18 Sep 2019 09:07:55 -0400
Subject: [PATCH] Mavlink: wait for boot complete before sending or receiving
 anything

---
 src/modules/mavlink/mavlink_main.cpp       | 10 ++++------
 src/modules/mavlink/mavlink_parameters.cpp |  7 +------
 src/modules/mavlink/mavlink_receiver.cpp   | 19 ++++++++++++-------
 3 files changed, 17 insertions(+), 19 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index a78fc243a2..0c3a7d76f7 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -846,12 +846,6 @@ Mavlink::send_packet()
 void
 Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 {
-	/* If the wait until transmit flag is on, only transmit after we've received messages.
-	   Otherwise, transmit all the time. */
-	if (!should_transmit()) {
-		return;
-	}
-
 	_last_write_try_time = hrt_absolute_time();
 
 	if (_mavlink_start_time == 0) {
@@ -2208,6 +2202,10 @@ Mavlink::task_main(int argc, char *argv[])
 		/* main loop */
 		px4_usleep(_main_loop_delay);
 
+		if (!should_transmit()) {
+			continue;
+		}
+
 		perf_count(_loop_interval_perf);
 		perf_begin(_loop_perf);
 
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
index d20c84ee58..b7a03f4ad6 100644
--- a/src/modules/mavlink/mavlink_parameters.cpp
+++ b/src/modules/mavlink/mavlink_parameters.cpp
@@ -415,7 +415,7 @@ MavlinkParametersManager::send_uavcan()
 bool
 MavlinkParametersManager::send_one()
 {
-	if (_send_all_index >= 0 && _mavlink->boot_complete()) {
+	if (_send_all_index >= 0) {
 		/* send all parameters if requested, but only after the system has booted */
 
 		/* The first thing we send is a hash of all values for the ground
@@ -461,11 +461,6 @@ MavlinkParametersManager::send_one()
 		} else {
 			return true;
 		}
-
-	} else if (_send_all_index == PARAM_HASH && hrt_absolute_time() > 20 * 1000 * 1000) {
-		/* the boot did not seem to ever complete, warn user and set boot complete */
-		_mavlink->send_statustext_critical("WARNING: SYSTEM BOOT INCOMPLETE. CHECK CONFIG.");
-		_mavlink->set_boot_complete();
 	}
 
 	return false;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7a8b46823c..941072c487 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -2517,11 +2517,21 @@ MavlinkReceiver::Run()
 {
 	/* set thread name */
 	{
-		char thread_name[24];
-		sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
+		char thread_name[17];
+		snprintf(thread_name, sizeof(thread_name), "mavlink_rcv_if%d", _mavlink->get_instance_id());
 		px4_prctl(PR_SET_NAME, thread_name, px4_getpid());
 	}
 
+	// make sure mavlink app has booted before we start processing anything (parameter sync, etc)
+	while (!_mavlink->boot_complete()) {
+		if (hrt_absolute_time() > 20_s) {
+			PX4_ERR("system boot did not complete in 20 seconds");
+			_mavlink->set_boot_complete();
+		}
+
+		px4_usleep(100000);
+	}
+
 	// poll timeout in ms. Also defines the max update frequency of the mission & param manager, etc.
 	const int timeout = 10;
 
@@ -2549,11 +2559,6 @@ MavlinkReceiver::Run()
 	socklen_t addrlen = sizeof(srcaddr);
 
 	if (_mavlink->get_protocol() == UDP || _mavlink->get_protocol() == TCP) {
-		// make sure mavlink app has booted before we start using the socket
-		while (!_mavlink->boot_complete()) {
-			px4_usleep(100000);
-		}
-
 		fds[0].fd = _mavlink->get_socket_fd();
 		fds[0].events = POLLIN;
 	}