Merge pull request #850 from PX4/mavlink_startup_cleanup

mavlink: Start the same in HIL mode as in normal mode. Requires all HIL ...

2 files changed, 26 insertions, 29 deletions

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 2ab1e2e96..ea5cf8deb 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -20,7 +20,7 @@ echo "[init] Looking for microSD..."
 if mount -t vfat /dev/mmcsd0 /fs/microsd
 then
 	set LOG_FILE /fs/microsd/bootlog.txt
-	echo "[init] microSD card mounted at /fs/microsd"
+	echo "[init] microSD mounted: /fs/microsd"
 	# Start playing the startup tune
 	tone_alarm start
 else
@@ -83,9 +83,9 @@ then
 	param select $PARAM_FILE
 	if param load
 	then
-		echo "[init] Parameters loaded: $PARAM_FILE"
+		echo "[init] Params loaded: $PARAM_FILE"
 	else
-		echo "[init] ERROR: Parameters loading failed: $PARAM_FILE"
+		echo "[init] ERROR: Params loading failed: $PARAM_FILE"
 	fi
 	
 	#
@@ -146,7 +146,7 @@ then
 	#
 	if param compare SYS_AUTOSTART 0
 	then
-		echo "[init] Don't try to find autostart script"
+		echo "[init] No autostart"
 	else
 		sh /etc/init.d/rc.autostart
 	fi
@@ -156,10 +156,10 @@ then
 	#
 	if [ -f $CONFIG_FILE ]
 	then
-		echo "[init] Reading config: $CONFIG_FILE"
+		echo "[init] Config: $CONFIG_FILE"
 		sh $CONFIG_FILE
 	else
-		echo "[init] Config file not found: $CONFIG_FILE"
+		echo "[init] Config not found: $CONFIG_FILE"
 	fi
 
 	#
@@ -264,10 +264,10 @@ then
 		then
 			set FMU_MODE serial
 		fi
-	else
-		# Try to get an USB console if not in HIL mode
-		nshterm /dev/ttyACM0 &
 	fi
+
+	# Try to get an USB console
+	nshterm /dev/ttyACM0 &
 	
 	#
 	# Start the Commander (needs to be this early for in-air-restarts)
@@ -401,23 +401,17 @@ then
 	#
 	if [ $MAVLINK_FLAGS == default ]
 	then
-		if [ $HIL == yes ]
+		# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
+		if [ $TTYS1_BUSY == yes ]
 		then
-			sleep 1
-			set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0"
+			# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
+			set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0"
+
+			# Exit from nsh to free port for mavlink
+			set EXIT_ON_END yes
 		else
-			# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
-			if [ $TTYS1_BUSY == yes ]
-			then
-				# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
-				set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0"
-				
-				# Exit from nsh to free port for mavlink
-				set EXIT_ON_END yes
-			else
-				# Start MAVLink on default port: ttyS1
-				set MAVLINK_FLAGS "-r 1000"
-			fi
+			# Start MAVLink on default port: ttyS1
+			set MAVLINK_FLAGS "-r 1000"
 		fi
 	fi
 
@@ -436,17 +430,15 @@ then
 	#
 	# Sensors, Logging, GPS
 	#
-	echo "[init] Start sensors"
 	sh /etc/init.d/rc.sensors
 
 	if [ $HIL == no ]
 	then
 		echo "[init] Start logging"
 		sh /etc/init.d/rc.logging
-		
-		echo "[init] Start GPS"
-		gps start
 	fi
+
+	gps start
 	
 	#
 	# Start up ARDrone Motor interface
@@ -561,7 +553,7 @@ then
 		echo "[init] Starting addons script: $EXTRAS_FILE"
 		sh $EXTRAS_FILE
 	else
-		echo "[init] Addons script not found: $EXTRAS_FILE"
+		echo "[init] No addons script: $EXTRAS_FILE"
 	fi
 
 	if [ $EXIT_ON_END == yes ]
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index a9f5f4de7..227e99b48 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -574,6 +574,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 	/* open uart */
 	_uart_fd = open(uart_name, O_RDWR | O_NOCTTY);
 
+	if (_uart_fd < 0) {
+		return _uart_fd;
+	}
+
+
 	/* Try to set baud rate */
 	struct termios uart_config;
 	int termios_state;