From 2e3926b577f0f2fb0b07d19e5041579604f1f562 Mon Sep 17 00:00:00 2001
From: mcsauder <mcsauder@gmail.com>
Date: Sat, 6 Jul 2019 12:53:32 -0600
Subject: [PATCH] Fix a corner case that prevented mavlink config streams from
 being indexed appropriately when USB is not connected at boot. Break out
 Mavlink class constructor logic into set_instance() and set_channel()
 methods. and add const specifiers and default arguments to
 Mavlink::mavlink_open_uart().

---
 src/modules/mavlink/mavlink_main.cpp | 106 +++++++++++++++------------
 src/modules/mavlink/mavlink_main.h   |  11 ++-
 2 files changed, 69 insertions(+), 48 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index a018c5251142..54bb2b9db981 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -164,50 +164,6 @@ bool Mavlink::_boot_complete = false;
 Mavlink::Mavlink() :
 	ModuleParams(nullptr)
 {
-	_instance_id = Mavlink::instance_count();
-
-	/* set channel according to instance id */
-	switch (_instance_id) {
-	case 0:
-		_channel = MAVLINK_COMM_0;
-		break;
-
-	case 1:
-		_channel = MAVLINK_COMM_1;
-		break;
-
-	case 2:
-		_channel = MAVLINK_COMM_2;
-		break;
-
-	case 3:
-		_channel = MAVLINK_COMM_3;
-		break;
-#ifdef MAVLINK_COMM_4
-
-	case 4:
-		_channel = MAVLINK_COMM_4;
-		break;
-#endif
-#ifdef MAVLINK_COMM_5
-
-	case 5:
-		_channel = MAVLINK_COMM_5;
-		break;
-#endif
-#ifdef MAVLINK_COMM_6
-
-	case 6:
-		_channel = MAVLINK_COMM_6;
-		break;
-#endif
-
-	default:
-		PX4_WARN("instance ID is out of range");
-		px4_task_exit(1);
-		break;
-	}
-
 	// initialise parameter cache
 	mavlink_update_parameters();
 
@@ -270,6 +226,58 @@ Mavlink::mavlink_update_parameters()
 	}
 }
 
+void
+Mavlink::set_channel()
+{
+	/* set channel according to instance id */
+	switch (_instance_id) {
+	case 0:
+		_channel = MAVLINK_COMM_0;
+		break;
+
+	case 1:
+		_channel = MAVLINK_COMM_1;
+		break;
+
+	case 2:
+		_channel = MAVLINK_COMM_2;
+		break;
+
+	case 3:
+		_channel = MAVLINK_COMM_3;
+		break;
+#ifdef MAVLINK_COMM_4
+
+	case 4:
+		_channel = MAVLINK_COMM_4;
+		break;
+#endif
+#ifdef MAVLINK_COMM_5
+
+	case 5:
+		_channel = MAVLINK_COMM_5;
+		break;
+#endif
+#ifdef MAVLINK_COMM_6
+
+	case 6:
+		_channel = MAVLINK_COMM_6;
+		break;
+#endif
+
+	default:
+		PX4_WARN("instance ID is out of range");
+		px4_task_exit(1);
+		break;
+	}
+}
+
+void
+Mavlink::set_instance_id()
+{
+	_instance_id = Mavlink::instance_count();
+}
+
 void
 Mavlink::set_proto_version(unsigned version)
 {
@@ -486,7 +494,7 @@ Mavlink::get_uart_fd(unsigned index)
 }
 
 int
-Mavlink::mavlink_open_uart(int baud, const char *uart_name, bool force_flow_control)
+Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const bool force_flow_control)
 {
 #ifndef B460800
 #define B460800 460800
@@ -604,7 +612,9 @@ Mavlink::mavlink_open_uart(int baud, const char *uart_name, bool force_flow_cont
 				return -1;
 			}
 
-			px4_usleep(100000);
+			int errcode = errno;
+			/* ENOTCONN means that the USB device is not yet connected */
+			px4_usleep(errcode == ENOTCONN ? 1000000 :  100000);
 			_uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
 		}
 	}
@@ -2173,6 +2183,10 @@ Mavlink::task_main(int argc, char *argv[])
 		_main_loop_delay = MAVLINK_MAX_INTERVAL;
 	}
 
+	set_instance_id();
+
+	set_channel();
+
 	/* now the instance is fully initialized and we can bump the instance count */
 	LL_APPEND(_mavlink_instances, this);
 
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 2aab99e98e7f..1910283a96c5 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -83,6 +83,7 @@
 #include "mavlink_shell.h"
 #include "mavlink_ulog.h"
 
+#define DEFAULT_BAUD_RATE       57600
 #define DEFAULT_REMOTE_PORT_UDP 14550 ///< GCS port per MAVLink spec
 #define DEFAULT_DEVICE_NAME     "/dev/ttyS1"
 #define HASH_PARAM              "_HASH_CHECK"
@@ -676,7 +677,9 @@ class Mavlink : public ModuleParams
 
 	void			mavlink_update_parameters();
 
-	int			mavlink_open_uart(int baudrate, const char *uart_name, bool force_flow_control);
+	int mavlink_open_uart(const int baudrate = DEFAULT_BAUD_RATE,
+			      const char *uart_name = DEFAULT_DEVICE_NAME,
+			      const bool force_flow_control = false);
 
 	static constexpr unsigned RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE = 25;
 	static constexpr unsigned RADIO_BUFFER_LOW_PERCENTAGE = 35;
@@ -731,10 +734,14 @@ class Mavlink : public ModuleParams
 
 	void init_udp();
 
+	void set_channel();
+
+	void set_instance_id();
+
 	/**
 	 * Main mavlink task.
 	 */
-	int		task_main(int argc, char *argv[]);
+	int task_main(int argc, char *argv[]);
 
 	// Disallow copy construction and move assignment.
 	Mavlink(const Mavlink &) = delete;