Merge pull request #763 from PX4/beta_mavlink2

Mavlink multi-stream support

2 files changed, 29 insertions, 47 deletions

diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index 0cd8a0e04..2aeea5cbe 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -5,38 +5,11 @@
 
 echo "Starting MAVLink on this USB console"
 
-# Stop tone alarm
-tone_alarm stop
-
-#
-# Check for UORB
-#
-if uorb start
-then
-	echo "uORB started"
-fi
-
-# Tell MAVLink that this link is "fast"
-if mavlink stop
-then
-	echo "stopped other MAVLink instance"
-fi
-mavlink start -b 230400 -d /dev/ttyACM0
-
-# Stop commander
-if commander stop
-then
-	echo "Commander stopped"
-fi
-sleep 1
-
-# Start the commander
-if commander start
-then
-	echo "Commander started"
-fi
-
-echo "MAVLink started, exiting shell.."
+mavlink start -r 10000 -d /dev/ttyACM0
+# Enable a number of interesting streams we want via USB
+mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
+mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10
+mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30
 
 # Exit shell to make it available to MAVLink
 exit 
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 65a66a085..503dbb83e 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -116,6 +116,8 @@ then
 	set PWM_MAX none
 	set MKBLCTRL_MODE none
 	set FMU_MODE pwm
+	set MAVLINK_FLAGS default
+	set EXIT_ON_END no
 	set MAV_TYPE none
 	set LOAD_DEFAULT_APPS yes
 	
@@ -394,28 +396,34 @@ then
 	#
 	# MAVLink
 	#
-	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 -d /dev/ttyS0
+			sleep 1
+			set MAVLINK_FLAGS "-r 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
-			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
@@ -560,6 +568,7 @@ then
 
 	if [ $EXIT_ON_END == yes ]
 	then
+		echo "[init] Exit from nsh"
 		exit
 	fi