From 1b8004cd8ecf7824584aac9e7fed447714feb716 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Sat, 1 Mar 2014 16:43:04 +0400
Subject: mavlink: add new streams in main loop, minor cleanup and fixes

---
 ROMFS/px4fmu_common/init.d/rcS | 36 ++++++++++++++++++++++--------------
 1 file changed, 22 insertions(+), 14 deletions(-)

(limited to 'ROMFS/px4fmu_common/init.d/rcS')

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index c3065b6fc..17f7dd077 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -117,6 +117,7 @@ then
 	set PWM_MAX none
 	set MKBLCTRL_MODE none
 	set FMU_MODE pwm
+	set MAVLINK_FLAGS default
 	set MAV_TYPE none
 	
 	#
@@ -381,26 +382,33 @@ then
 	#
 	set EXIT_ON_END no
 	
-	if [ $HIL == yes ]
+	if [ $MAVLINK_FLAGS == default ]
 	then
-		sleep 1
-		mavlink start -b 230400 -d /dev/ttyACM0 
-		usleep 5000
-	else
-		if [ $TTYS1_BUSY == yes ]
+		if [ $HIL == yes ]
 		then
-			# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
-			mavlink start -r 1000 -d /dev/ttyS0
+			sleep 1
+			set MAVLINK_FLAGS "-d 10000 -d /dev/ttyACM0"
 			usleep 5000
-			
-			# Exit from nsh to free port for mavlink
-			set EXIT_ON_END yes
 		else
-			# Start MAVLink on default port: ttyS1
-			mavlink start -r 1000
-			usleep 5000
+			# 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"
+				usleep 5000
+				
+				# 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"
+				usleep 5000
+			fi
 		fi
 	fi
+
+	mavlink start $MAVLINK_FLAGS
+	usleep 5000
 	
 	#
 	# Start the datamanager
-- 
cgit v1.2.3