From 4502c285eb8b284b7c08666b6d0e3e81035bace3 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 5 Nov 2013 19:56:33 +0100
Subject: [PATCH] Startup scripts: Start the commander early and let it try to
 open the mavlink_fd with 20Hz

---
 ROMFS/px4fmu_common/init.d/rc.fixedwing  |  5 -----
 ROMFS/px4fmu_common/init.d/rc.multirotor |  5 -----
 ROMFS/px4fmu_common/init.d/rcS           |  5 +++++
 src/modules/commander/commander.cpp      | 17 +++++++----------
 4 files changed, 12 insertions(+), 20 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.fixedwing b/ROMFS/px4fmu_common/init.d/rc.fixedwing
index 79e34a3b97..f028510064 100644
--- a/ROMFS/px4fmu_common/init.d/rc.fixedwing
+++ b/ROMFS/px4fmu_common/init.d/rc.fixedwing
@@ -3,11 +3,6 @@
 # Standard everything needed for fixedwing except mixer, actuator output and mavlink
 #
 
-#
-# Start the Commander
-#
-commander start
-
 #
 # Start the sensors and test them.
 #
diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor
index 6bae991754..bc550ac5a1 100644
--- a/ROMFS/px4fmu_common/init.d/rc.multirotor
+++ b/ROMFS/px4fmu_common/init.d/rc.multirotor
@@ -3,11 +3,6 @@
 # Standard everything needed for multirotors except mixer, actuator output and mavlink
 #
 
-#
-# Start the Commander
-#
-commander start
-
 #
 # Start the sensors and test them.
 #
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index cff8446a6b..d8b5cb6087 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -105,6 +105,11 @@ then
 			blinkm systemstate
 		fi
 	fi
+	
+	#
+	# Start the Commander (needs to be this early for in-air-restarts)
+	#
+	commander start
 
 	if param compare SYS_AUTOSTART 1000
 	then
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index ed090271ca..ace13bb784 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -116,6 +116,8 @@ extern struct system_load_s system_load;
 #define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 #define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
+#define MAVLINK_OPEN_INTERVAL 50000
+
 #define STICK_ON_OFF_LIMIT 0.75f
 #define STICK_THRUST_RANGE 1.0f
 #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
@@ -582,16 +584,6 @@ int commander_thread_main(int argc, char *argv[])
 
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
-	if (mavlink_fd < 0) {
-		/* try again later */
-		usleep(20000);
-		mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-
-		if (mavlink_fd < 0) {
-			warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first.");
-		}
-	}
-
 	/* Main state machine */
 	/* make sure we are in preflight state */
 	memset(&status, 0, sizeof(status));
@@ -770,6 +762,11 @@ int commander_thread_main(int argc, char *argv[])
 
 	while (!thread_should_exit) {
 
+		if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) {
+			/* try to open the mavlink log device every once in a while */
+			mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+		}
+
 		/* update parameters */
 		orb_check(param_changed_sub, &updated);