Merge branch 'master' into frsky_telemetry

25 files changed, 278 insertions, 369 deletions

diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
index c79e9d283..81d4b5d57 100644
--- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery
+++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
@@ -32,8 +32,6 @@ fi
 #
 param set MAV_TYPE 2
 
-set EXIT_ON_END no
-
 #
 # Start and configure PX4IO or FMU interface
 #
@@ -74,8 +72,3 @@ pwm max -c 1234 -p 1900
 # Start common for all multirotors apps
 #
 sh /etc/init.d/rc.multirotor
-
-if [ $EXIT_ON_END == yes ]
-then
-	exit
-fi
diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris
index f6b071cf1..b0f4eda79 100644
--- a/ROMFS/px4fmu_common/init.d/16_3dr_iris
+++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris
@@ -32,8 +32,6 @@ fi
 #
 param set MAV_TYPE 2
 
-set EXIT_ON_END no
-
 #
 # Start and configure PX4IO or FMU interface
 #
@@ -73,8 +71,3 @@ pwm min -c 1234 -p 1050
 # Start common for all multirotors apps
 #
 sh /etc/init.d/rc.multirotor
-
-if [ $EXIT_ON_END == yes ]
-then
-	exit
-fi
diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar
index abe378b22..97c2d7c90 100644
--- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar
+++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar
@@ -42,8 +42,6 @@ fi
 #
 param set MAV_TYPE 1
 
-set EXIT_ON_END no
-
 #
 # Start and configure PX4IO or FMU interface
 #
@@ -80,8 +78,3 @@ fi
 # Start common fixedwing apps
 #
 sh /etc/init.d/rc.fixedwing
-
-if [ $EXIT_ON_END == yes ]
-then
-	exit
-fi
diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_hk_bixler
index c616da988..995d3ba07 100644
--- a/ROMFS/px4fmu_common/init.d/101_hk_bixler
+++ b/ROMFS/px4fmu_common/init.d/2101_hk_bixler
@@ -42,8 +42,6 @@ fi
 #
 param set MAV_TYPE 1
 
-set EXIT_ON_END no
-
 #
 # Start and configure PX4IO or FMU interface
 #
@@ -80,8 +78,3 @@ fi
 # Start common fixedwing apps
 #
 sh /etc/init.d/rc.fixedwing
-
-if [ $EXIT_ON_END == yes ]
-then
-	exit
-fi
diff --git a/ROMFS/px4fmu_common/init.d/102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker
index e5d21c321..a6d2ace96 100644
--- a/ROMFS/px4fmu_common/init.d/102_3dr_skywalker
+++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker
@@ -42,8 +42,6 @@ fi
 #
 param set MAV_TYPE 1
 
-set EXIT_ON_END no
-
 #
 # Start and configure PX4IO or FMU interface
 #
@@ -82,8 +80,3 @@ fi
 # Start common fixedwing apps
 #
 sh /etc/init.d/rc.fixedwing
-
-if [ $EXIT_ON_END == yes ]
-then
-	exit
-fi
diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer
index 8a8bc1590..65f01c974 100644
--- a/ROMFS/px4fmu_common/init.d/30_io_camflyer
+++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer
@@ -20,8 +20,6 @@ fi
 #
 param set MAV_TYPE 1
 
-set EXIT_ON_END no
-
 #
 # Start and configure PX4IO or FMU interface
 #
@@ -58,8 +56,3 @@ fi
 # Start common fixedwing apps
 #
 sh /etc/init.d/rc.fixedwing
-
-if [ $EXIT_ON_END == yes ]
-then
-	exit
-fi
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/3031_io_phantom
index 62cfe1a9c..0cf6ee39a 100644
--- a/ROMFS/px4fmu_common/init.d/31_io_phantom
+++ b/ROMFS/px4fmu_common/init.d/3031_io_phantom
@@ -47,8 +47,6 @@ fi
 #
 param set MAV_TYPE 1
 
-set EXIT_ON_END no
-
 #
 # Start and configure PX4IO or FMU interface
 #
@@ -85,8 +83,3 @@ fi
 # Start common fixedwing apps
 #
 sh /etc/init.d/rc.fixedwing
-
-if [ $EXIT_ON_END == yes ]
-then
-	exit
-fi
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
index 1e752f13a..41e041654 100644
--- a/ROMFS/px4fmu_common/init.d/32_skywalker_x5
+++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
@@ -20,8 +20,6 @@ fi
 #
 param set MAV_TYPE 1
 
-set EXIT_ON_END no
-
 #
 # Start and configure PX4IO or FMU interface
 #
@@ -58,8 +56,3 @@ fi
 # Start common fixedwing apps
 #
 sh /etc/init.d/rc.fixedwing
-
-if [ $EXIT_ON_END == yes ]
-then
-	exit
-fi
diff --git a/ROMFS/px4fmu_common/init.d/33_io_wingwing b/ROMFS/px4fmu_common/init.d/3033_io_wingwing
index 538c69711..82ff425e6 100644
--- a/ROMFS/px4fmu_common/init.d/33_io_wingwing
+++ b/ROMFS/px4fmu_common/init.d/3033_io_wingwing
@@ -46,8 +46,6 @@ fi
 #
 param set MAV_TYPE 1
 
-set EXIT_ON_END no
-
 #
 # Start and configure PX4IO or FMU interface
 #
@@ -84,8 +82,3 @@ fi
 # Start common fixedwing apps
 #
 sh /etc/init.d/rc.fixedwing
-
-if [ $EXIT_ON_END == yes ]
-then
-   exit
-fi
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/34_io_fx79 b/ROMFS/px4fmu_common/init.d/3034_io_fx79
index 989204952..759c17bb4 100644
--- a/ROMFS/px4fmu_common/init.d/34_io_fx79
+++ b/ROMFS/px4fmu_common/init.d/3034_io_fx79
@@ -46,8 +46,6 @@ fi
 #
 param set MAV_TYPE 1
 
-set EXIT_ON_END no
-
 #
 # Start and configure PX4IO or FMU interface
 #
@@ -84,8 +82,3 @@ fi
 # Start common fixedwing apps
 #
 sh /etc/init.d/rc.fixedwing
-
-if [ $EXIT_ON_END == yes ]
-then
-	exit
-fi
diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone
index f6f09ac22..f6f09ac22 100644
--- a/ROMFS/px4fmu_common/init.d/08_ardrone
+++ b/ROMFS/px4fmu_common/init.d/4008_ardrone
diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow
index 9b739f245..2886bcb75 100644
--- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow
+++ b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow
@@ -55,11 +55,6 @@ ardrone_interface start -d /dev/ttyS1
 sh /etc/init.d/rc.sensors
 
 #
-# Start the commander.
-#
-commander start
-
-#
 # Start the attitude estimator
 #
 attitude_estimator_ekf start
diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330
index 467b56bbf..7054210e2 100644
--- a/ROMFS/px4fmu_common/init.d/10_dji_f330
+++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330
@@ -41,14 +41,6 @@ then
 
 	param save
 fi
- 
-#
-# Force some key parameters to sane values
-# MAV_TYPE     2 = quadrotor
-#
-param set MAV_TYPE 2
-
-set EXIT_ON_END no
 
 #
 # Start and configure PX4IO or FMU interface
@@ -69,29 +61,9 @@ else
 	set EXIT_ON_END yes
 fi
 
-#
-# Load mixer
-#
-mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
-
-#
-# Set PWM output frequency
-#
-pwm rate -c 1234 -r 400
-
-#
-# Set disarmed, min and max PWM signals (for DJI ESCs)
-#
-pwm disarmed -c 1234 -p 900
-pwm min -c 1234 -p 1200
-pwm max -c 1234 -p 1800
+sh /etc/init.d/rc.mc_interface
 
 #
 # Start common for all multirotors apps
 #
 sh /etc/init.d/rc.multirotor
-
-if [ $EXIT_ON_END == yes ]
-then
-	exit
-fi
diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450
index 818f9375e..a1d253191 100644
--- a/ROMFS/px4fmu_common/init.d/11_dji_f450
+++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450
@@ -32,8 +32,6 @@ fi
 #
 param set MAV_TYPE 2
 
-set EXIT_ON_END no
-
 #
 # Start and configure PX4IO or FMU interface
 #
@@ -43,8 +41,6 @@ then
 	mavlink start
 	usleep 5000
 
-	commander start
-
 	sh /etc/init.d/rc.io
 else
 	# Start MAVLink (on UART1 / ttyS0)
@@ -76,8 +72,3 @@ pwm max -c 1234 -p 1800
 # Start common multirotor apps
 #
 sh /etc/init.d/rc.multirotor
-
-if [ $EXIT_ON_END == yes ]
-then
-	exit
-fi
diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway
index fb9dec043..ad455b440 100644
--- a/ROMFS/px4fmu_common/init.d/40_io_segway
+++ b/ROMFS/px4fmu_common/init.d/40_io_segway
@@ -28,11 +28,6 @@ usleep 5000
 # Start and configure PX4IO interface
 #
 sh /etc/init.d/rc.io
- 
-#
-# Start the commander (depends on orb, mavlink)
-#
-commander start
 
 #
 # Start the sensors (depends on orb, px4io)
diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550
index 1ee84b9b0..acd8027fb 100644
--- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550
+++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550
@@ -34,8 +34,6 @@ fi
 #
 param set MAV_TYPE 2
 
-set EXIT_ON_END no
-
 #
 # Start and configure PX4IO or FMU interface
 #
@@ -76,8 +74,3 @@ pwm max -c 1234 -p 1900
 # Start common for all multirotors apps
 #
 sh /etc/init.d/rc.multirotor
-
-if [ $EXIT_ON_END == yes ]
-then
-	exit
-fi
diff --git a/ROMFS/px4fmu_common/init.d/800_sdlogger b/ROMFS/px4fmu_common/init.d/800_sdlogger
index 955fe0e2a..9b90cbdd0 100644
--- a/ROMFS/px4fmu_common/init.d/800_sdlogger
+++ b/ROMFS/px4fmu_common/init.d/800_sdlogger
@@ -13,8 +13,6 @@ then
 	param save
 fi
 
-set EXIT_ON_END no
-
 #
 # Start and configure PX4IO or FMU interface
 #
@@ -24,8 +22,6 @@ then
 	mavlink start
 	usleep 5000
 
-	commander start
-
 	sh /etc/init.d/rc.io
 	# Set PWM values for DJI ESCs
 else
@@ -53,8 +49,3 @@ then
 		sdlog2 start -r 200 -e -b 16
 	fi
 fi
-
-if [ $EXIT_ON_END == yes ]
-then
-	exit
-fi
diff --git a/ROMFS/px4fmu_common/init.d/12-13_hex b/ROMFS/px4fmu_common/init.d/rc.hexa
index a7578bcaf..097db28e4 100644
--- a/ROMFS/px4fmu_common/init.d/12-13_hex
+++ b/ROMFS/px4fmu_common/init.d/rc.hexa
@@ -52,7 +52,7 @@ param set MAV_TYPE 13
 set EXIT_ON_END no
 
 #
-# Start and configure PX4IO or FMU interface
+# Start and configure PX4IO interface
 #
 if px4io detect
 then
@@ -62,18 +62,14 @@ then
 
 	sh /etc/init.d/rc.io
 else
-	# Start MAVLink (on UART1 / ttyS0)
-	mavlink start -d /dev/ttyS0
-	usleep 5000
-	fmu mode_pwm
-	param set BAT_V_SCALING 0.004593
-	set EXIT_ON_END yes
+	# This is not possible on a hexa
+	tone_alarm error
 fi
 
 #
 # Load mixer
 #
-mixer load /dev/pwm_output /etc/mixers/FMU_hex_x.mix
+mixer load /dev/pwm_output $MIXER
 
 #
 # Set PWM output frequency to 400 Hz
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface
new file mode 100644
index 000000000..6bb2e84ec
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface
@@ -0,0 +1,49 @@
+#!nsh
+#
+# Script to set PWM min / max limits and mixer
+#
+
+#
+# Load mixer
+#
+if [ $FRAME_GEOMETRY == x ]
+then
+    echo "Frame geometry X"
+    mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
+else
+    if [ $FRAME_GEOMETRY == w ]
+    then
+	echo "Frame geometry W"
+	mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
+    else
+	echo "Frame geometry +"
+    	mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
+    fi
+fi
+
+if [ $FRAME_COUNT == 4 ]
+then
+    set OUTPUTS 1234
+    param set MAV_TYPE 2
+else
+    if [ $FRAME_COUNT == 6 ]
+    then
+	set OUTPUTS 123456
+	param set MAV_TYPE 13
+    else
+	set OUTPUTS 12345678
+    fi
+fi
+
+
+#
+# Set PWM output frequency
+#
+pwm rate -c $OUTPUTS -r $PWM_RATE
+
+#
+# Set disarmed, min and max PWM signals (for DJI ESCs)
+#
+pwm disarmed -c $OUTPUTS -p $PWM_DISARMED
+pwm min -c $OUTPUTS -p $PWM_MIN
+pwm max -c $OUTPUTS -p $PWM_MAX
diff --git a/ROMFS/px4fmu_common/init.d/rc.octo b/ROMFS/px4fmu_common/init.d/rc.octo
new file mode 100644
index 000000000..ecb12e96e
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.octo
@@ -0,0 +1,94 @@
+#!nsh
+
+echo "[init] Octorotor startup"
+
+#
+# Load default params for this platform
+#
+if param compare SYS_AUTOCONFIG 1
+then
+	# Set all params here, then disable autoconfig
+	param set SYS_AUTOCONFIG 0
+
+	param set MC_ATTRATE_D 0.004
+	param set MC_ATTRATE_I 0.0
+	param set MC_ATTRATE_P 0.12
+	param set MC_ATT_D 0.0
+	param set MC_ATT_I 0.0
+	param set MC_ATT_P 7.0
+	param set MC_YAWPOS_D 0.0
+	param set MC_YAWPOS_I 0.0
+	param set MC_YAWPOS_P 2.0
+	param set MC_YAWRATE_D 0.005
+	param set MC_YAWRATE_I 0.2
+	param set MC_YAWRATE_P 0.3
+	param set NAV_TAKEOFF_ALT 3.0
+	param set MPC_TILT_MAX 0.5
+	param set MPC_THR_MAX 0.7
+	param set MPC_THR_MIN 0.3
+	param set MPC_XY_D 0
+	param set MPC_XY_P 0.5
+	param set MPC_XY_VEL_D 0
+	param set MPC_XY_VEL_I 0
+	param set MPC_XY_VEL_MAX 3
+	param set MPC_XY_VEL_P 0.2
+	param set MPC_Z_D 0
+	param set MPC_Z_P 1
+	param set MPC_Z_VEL_D 0
+	param set MPC_Z_VEL_I 0.1
+	param set MPC_Z_VEL_MAX 2
+	param set MPC_Z_VEL_P 0.20
+
+	param save
+fi
+ 
+#
+# Force some key parameters to sane values
+# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/
+# 14 = octorotor
+#
+param set MAV_TYPE 14
+
+set EXIT_ON_END no
+
+#
+# Start and configure PX4IO interface
+#
+if px4io detect
+then
+	# Start MAVLink (depends on orb)
+	mavlink start
+	usleep 5000
+
+	sh /etc/init.d/rc.io
+else
+	# This is not possible on an octo
+	tone_alarm error
+fi
+
+#
+# Load mixer
+#
+mixer load /dev/pwm_output $MIXER
+
+#
+# Set PWM output frequency to 400 Hz
+#
+pwm rate -a -r 400
+
+#
+# Set disarmed, min and max PWM signals
+#
+pwm disarmed -c 12345678 -p 900
+pwm min -c 12345678 -p 1100
+pwm max -c 12345678 -p 1900
+
+#
+# Start common for all multirotors apps
+#
+sh /etc/init.d/rc.multirotor
+
+if [ $EXIT_ON_END == yes ]
+then
+	exit
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index e6a748f8f..f122921c5 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -176,7 +176,7 @@ then
 		tone_alarm MBABGP
 		if px4io forceupdate 14662 $io_file
 		then
-			usleep 200000
+			usleep 500000
 			if px4io start
 			then
 				echo "PX4IO restart OK"
@@ -186,7 +186,8 @@ then
 				echo "PX4IO restart failed"
 				echo "PX4IO restart failed" >> $logfile
 				tone_alarm MNGGG
-				sh /etc/init.d/rc.error
+				sleep 10
+				reboot
 			fi
 		else
 			echo "PX4IO update failed"
@@ -194,64 +195,103 @@ then
 			tone_alarm MNGGG
 		fi
 	fi
+
+	set EXIT_ON_END no
 	
 	#
 	# Check if auto-setup from one of the standard scripts is wanted
 	# SYS_AUTOSTART = 0 means no autostart (default)
 	#
+	# AUTOSTART PARTITION:
+	#  0   	..	 999		Reserved (historical)
+	#  1000	..	 1999		Simulation setups
+	#  2000	..	 2999		Standard planes
+	#  3000	..	 3999		Flying wing
+	#  4000	..	 4999		Quad X
+	#  5000	..	 5999		Quad +
+	#  6000	..	 6999		Hexa X
+	#  7000	..	 7999		Hexa +
+	#  8000	..	 8999		Octo X
+	#  9000	..	 9999		Octo +
+	# 10000	..	19999		Wide arm / H frame
 	
-	if param compare SYS_AUTOSTART 8
+	if param compare SYS_AUTOSTART 4008 8
 	then
-		sh /etc/init.d/08_ardrone
+		sh /etc/init.d/4008_ardrone
 		set MODE custom
 	fi
 	
-	if param compare SYS_AUTOSTART 9
+	if param compare SYS_AUTOSTART 4009 9
 	then
-		sh /etc/init.d/09_ardrone_flow
+		sh /etc/init.d/4009_ardrone_flow
 		set MODE custom
 	fi
 	
-	if param compare SYS_AUTOSTART 10
+	if param compare SYS_AUTOSTART 4010 10
 	then
-		sh /etc/init.d/10_dji_f330
+		set FRAME_GEOMETRY x
+		set FRAME_COUNT 4
+		set PWM_MIN 1200
+		set PWM_MAX 1900
+		set PWM_DISARMED 900
+		sh /etc/init.d/4010_dji_f330
 		set MODE custom
 	fi
 
-	if param compare SYS_AUTOSTART 11
+	if param compare SYS_AUTOSTART 4011 11
 	then
-		sh /etc/init.d/11_dji_f450
+		sh /etc/init.d/4011_dji_f450
 		set MODE custom
 	fi
 
-	if param compare SYS_AUTOSTART 12
+	if param compare SYS_AUTOSTART 4012
+	then
+		sh /etc/init.d/666_fmu_q_x550
+		set MODE custom
+	fi
+
+	if param compare SYS_AUTOSTART 6012 12
 	then
 		set MIXER /etc/mixers/FMU_hex_x.mix
-		sh /etc/init.d/12-13_hex
+		sh /etc/init.d/rc.hexa
 		set MODE custom
 	fi
 
-	if param compare SYS_AUTOSTART 13
+	if param compare SYS_AUTOSTART 7013 13
 	then
 		set MIXER /etc/mixers/FMU_hex_+.mix
-		sh /etc/init.d/12-13_hex
+		sh /etc/init.d/rc.hexa
+		set MODE custom
+	fi
+
+	if param compare SYS_AUTOSTART 8001
+	then
+		set MIXER /etc/mixers/FMU_octo_x.mix
+		sh /etc/init.d/rc.octo
+		set MODE custom
+	fi
+
+	if param compare SYS_AUTOSTART 9001
+	then
+		set MIXER /etc/mixers/FMU_octo_+.mix
+		sh /etc/init.d/rc.octo
 		set MODE custom
 	fi
 	
-	if param compare SYS_AUTOSTART 15
+	if param compare SYS_AUTOSTART 10015 15
 	then
-		sh /etc/init.d/15_tbs_discovery
+		sh /etc/init.d/10015_tbs_discovery
 		set MODE custom
 	fi
 
-	if param compare SYS_AUTOSTART 16
+	if param compare SYS_AUTOSTART 10016 16
 	then
-		sh /etc/init.d/16_3dr_iris
+		sh /etc/init.d/10016_3dr_iris
 		set MODE custom
 	fi
 
 	# PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame
-	if param compare SYS_AUTOSTART 17
+	if param compare SYS_AUTOSTART 4017 17
 	then
 		set MKBLCTRL_MODE no
 		set MKBLCTRL_FRAME x
@@ -260,7 +300,7 @@ then
 	fi
 
 	# PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame
-	if param compare SYS_AUTOSTART 18
+	if param compare SYS_AUTOSTART 5018 18
 	then
 		set MKBLCTRL_MODE no
 		set MKBLCTRL_FRAME +
@@ -269,7 +309,7 @@ then
 	fi
 
 	# PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing
-	if param compare SYS_AUTOSTART 19
+	if param compare SYS_AUTOSTART 4019 19
 	then
 		set MKBLCTRL_MODE yes
 		set MKBLCTRL_FRAME x
@@ -278,7 +318,7 @@ then
 	fi
 
 	# PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing
-	if param compare SYS_AUTOSTART 20
+	if param compare SYS_AUTOSTART 5020 20
 	then
 		set MKBLCTRL_MODE yes
 		set MKBLCTRL_FRAME +
@@ -287,7 +327,7 @@ then
 	fi
 
 	# PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame
-	if param compare SYS_AUTOSTART 21
+	if param compare SYS_AUTOSTART 4021 21
 	then
 		set FRAME_GEOMETRY x
 		set ESC_MAKER afro
@@ -296,40 +336,40 @@ then
 	fi
 
 	# PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame
-	if param compare SYS_AUTOSTART 22
+	if param compare SYS_AUTOSTART 10022 22
 	then
 		set FRAME_GEOMETRY w
 		sh /etc/init.d/rc.custom_io_esc
 		set MODE custom
 	fi
 	
-	if param compare SYS_AUTOSTART 30
+	if param compare SYS_AUTOSTART 3030 30
 	then
-		sh /etc/init.d/30_io_camflyer
+		sh /etc/init.d/3030_io_camflyer
 		set MODE custom
 	fi
 
-	if param compare SYS_AUTOSTART 31
+	if param compare SYS_AUTOSTART 3031 31
 	then
-		sh /etc/init.d/31_io_phantom
+		sh /etc/init.d/3031_io_phantom
 		set MODE custom
 	fi
 	
-	if param compare SYS_AUTOSTART 32
+	if param compare SYS_AUTOSTART 3032 32
 	then
-		sh /etc/init.d/32_skywalker_x5
+		sh /etc/init.d/3032_skywalker_x5
 		set MODE custom
 	fi
 
-	if param compare SYS_AUTOSTART 33
+	if param compare SYS_AUTOSTART 3033 33
 	then
-		sh /etc/init.d/33_io_wingwing
+		sh /etc/init.d/3033_io_wingwing
 		set MODE custom
 	fi
 
-        if param compare SYS_AUTOSTART 34
+        if param compare SYS_AUTOSTART 3034 34
         then
-                sh /etc/init.d/34_io_fx79
+                sh /etc/init.d/3034_io_fx79
                 set MODE custom
         fi
 
@@ -339,21 +379,21 @@ then
 		set MODE custom
 	fi
 	
-	if param compare SYS_AUTOSTART 100
+	if param compare SYS_AUTOSTART 2100 100
 	then
-		sh /etc/init.d/100_mpx_easystar
+		sh /etc/init.d/2100_mpx_easystar
 		set MODE custom
 	fi
 
-	if param compare SYS_AUTOSTART 101
+	if param compare SYS_AUTOSTART 2101 101
 	then
-		sh /etc/init.d/101_hk_bixler
+		sh /etc/init.d/2101_hk_bixler
 		set MODE custom
 	fi
 
-	if param compare SYS_AUTOSTART 102
+	if param compare SYS_AUTOSTART 2102 102
 	then
-		sh /etc/init.d/102_3dr_skywalker
+		sh /etc/init.d/2102_3dr_skywalker
 		set MODE custom
 	fi
 	
@@ -402,5 +442,10 @@ then
 	
 	fi
 
+	if [ $EXIT_ON_END == yes ]
+	then
+		exit
+	fi
+
 # End of autostart
 fi
diff --git a/makefiles/config_px4fmu-v2_logging.mk b/makefiles/config_px4fmu-v2_logging.mk
deleted file mode 100644
index ed90f6464..000000000
--- a/makefiles/config_px4fmu-v2_logging.mk
+++ /dev/null
@@ -1,157 +0,0 @@
-#
-# Makefile for the px4fmu_default configuration
-#
-
-#
-# Use the configuration's ROMFS, copy the px4iov2 firmware into
-# the ROMFS if it's available
-#
-ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_logging
-ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin
-
-#
-# Board support modules
-#
-MODULES		+= drivers/device
-MODULES		+= drivers/stm32
-MODULES		+= drivers/stm32/adc
-MODULES		+= drivers/stm32/tone_alarm
-MODULES		+= drivers/led
-MODULES		+= drivers/px4fmu
-MODULES		+= drivers/px4io
-MODULES		+= drivers/boards/px4fmu-v2
-MODULES		+= drivers/rgbled
-MODULES		+= drivers/mpu6000
-MODULES		+= drivers/lsm303d
-MODULES		+= drivers/l3gd20
-MODULES		+= drivers/hmc5883
-MODULES		+= drivers/ms5611
-MODULES		+= drivers/mb12xx
-MODULES		+= drivers/gps
-MODULES		+= drivers/hil
-MODULES		+= drivers/hott/hott_telemetry
-MODULES		+= drivers/hott/hott_sensors
-MODULES		+= drivers/blinkm
-MODULES		+= drivers/roboclaw
-MODULES		+= drivers/airspeed
-MODULES		+= drivers/ets_airspeed
-MODULES		+= drivers/meas_airspeed
-MODULES		+= modules/sensors
-
-# Needs to be burned to the ground and re-written; for now,
-# just don't build it.
-#MODULES		+= drivers/mkblctrl
-
-#
-# System commands
-#
-MODULES		+= systemcmds/ramtron
-MODULES		+= systemcmds/bl_update
-MODULES		+= systemcmds/boardinfo
-MODULES		+= systemcmds/mixer
-MODULES		+= systemcmds/param
-MODULES		+= systemcmds/perf
-MODULES		+= systemcmds/preflight_check
-MODULES		+= systemcmds/pwm
-MODULES		+= systemcmds/esc_calib
-MODULES		+= systemcmds/reboot
-MODULES		+= systemcmds/top
-MODULES		+= systemcmds/tests
-MODULES		+= systemcmds/config
-MODULES		+= systemcmds/nshterm
-
-#
-# General system control
-#
-MODULES		+= modules/commander
-MODULES		+= modules/navigator
-MODULES		+= modules/mavlink
-MODULES		+= modules/mavlink_onboard
-
-#
-# Estimation modules (EKF/ SO3 / other filters)
-#
-MODULES		+= modules/attitude_estimator_ekf
-MODULES		+= modules/attitude_estimator_so3
-MODULES		+= modules/att_pos_estimator_ekf
-MODULES		+= modules/position_estimator_inav
-MODULES		+= examples/flow_position_estimator
-
-#
-# Vehicle Control
-#
-#MODULES		+= modules/segway # XXX Needs GCC 4.7 fix
-MODULES		+= modules/fw_pos_control_l1
-MODULES		+= modules/fw_att_control
-MODULES		+= modules/multirotor_att_control
-MODULES		+= modules/multirotor_pos_control
-
-#
-# Logging
-#
-MODULES		+= modules/sdlog2
-
-#
-# Unit tests
-#
-#MODULES 	+= modules/unit_test
-#MODULES 	+= modules/commander/commander_tests
-
-#
-# Library modules
-#
-MODULES		+= modules/systemlib
-MODULES		+= modules/systemlib/mixer
-MODULES		+= modules/controllib
-MODULES		+= modules/uORB
-
-#
-# Libraries
-#
-LIBRARIES	+= lib/mathlib/CMSIS
-MODULES		+= lib/mathlib
-MODULES		+= lib/mathlib/math/filter
-MODULES		+= lib/ecl
-MODULES		+= lib/external_lgpl
-MODULES		+= lib/geo
-MODULES		+= lib/conversion
-
-#
-# Demo apps
-#
-#MODULES		+= examples/math_demo
-# Tutorial code from
-# https://pixhawk.ethz.ch/px4/dev/hello_sky
-MODULES		+= examples/px4_simple_app
-
-# Tutorial code from
-# https://pixhawk.ethz.ch/px4/dev/daemon
-#MODULES		+= examples/px4_daemon_app
-
-# Tutorial code from
-# https://pixhawk.ethz.ch/px4/dev/debug_values
-#MODULES		+= examples/px4_mavlink_debug
-
-# Tutorial code from
-# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
-#MODULES			+= examples/fixedwing_control
-
-# Hardware test
-#MODULES			+= examples/hwtest
-
-#
-# Transitional support - add commands from the NuttX export archive.
-#
-# In general, these should move to modules over time.
-#
-# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
-# to make the table a bit more readable.
-#
-define _B
-	$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
-endef
-
-#                  command                 priority                   stack  entrypoint
-BUILTIN_COMMANDS := \
-	$(call _B, sercon,                 ,                          2048,  sercon_main                ) \
-	$(call _B, serdis,                 ,                          2048,  serdis_main                )
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 00a0dcd61..60c902ce5 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -479,7 +479,8 @@ FixedwingAttitudeControl::task_main()
 
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vcontrol_mode_sub, 200);
-	orb_set_interval(_att_sub, 100);
+	/* rate limit attitude control to 50 Hz (with some margin, so 17 ms) */
+	orb_set_interval(_att_sub, 17);
 
 	parameters_update();
 
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index 9e43467cc..92b1b45be 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -348,20 +348,26 @@ l_input_rc(const struct listener *l)
 	/* copy rc channels into local buffer */
 	orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw);
 
-	if (gcs_link)
-		/* Channels are sent in MAVLink main loop at a fixed interval */
-		mavlink_msg_rc_channels_raw_send(chan,
-						 rc_raw.timestamp / 1000,
-						 0,
-						 (rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX,
-						 (rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX,
-						 (rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX,
-						 (rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX,
-						 (rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX,
-						 (rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX,
-						 (rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX,
-						 (rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX,
-						 255);
+	if (gcs_link) {
+
+		const unsigned port_width = 8;
+
+		for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) {
+			/* Channels are sent in MAVLink main loop at a fixed interval */
+			mavlink_msg_rc_channels_raw_send(chan,
+							 rc_raw.timestamp / 1000,
+							 i,
+							 (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
+							 (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
+							 (rc_raw.channel_count > (i * port_width) + 2) ? rc_raw.values[(i * port_width) + 2] : UINT16_MAX,
+							 (rc_raw.channel_count > (i * port_width) + 3) ? rc_raw.values[(i * port_width) + 3] : UINT16_MAX,
+							 (rc_raw.channel_count > (i * port_width) + 4) ? rc_raw.values[(i * port_width) + 4] : UINT16_MAX,
+							 (rc_raw.channel_count > (i * port_width) + 5) ? rc_raw.values[(i * port_width) + 5] : UINT16_MAX,
+							 (rc_raw.channel_count > (i * port_width) + 6) ? rc_raw.values[(i * port_width) + 6] : UINT16_MAX,
+							 (rc_raw.channel_count > (i * port_width) + 7) ? rc_raw.values[(i * port_width) + 7] : UINT16_MAX,
+							 rc_raw.rssi);
+		}
+	}
 }
 
 void
@@ -489,7 +495,8 @@ l_actuator_outputs(const struct listener *l)
 
 	if (gcs_link) {
 		mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
-						  l->arg /* port number */,
+						  l->arg /* port number - needs GCS support */,
+							 /* QGC has port number support already */
 						  act_outputs.output[0],
 						  act_outputs.output[1],
 						  act_outputs.output[2],
diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index 40a9297a7..65f291f40 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -63,7 +62,7 @@ static void	do_import(const char* param_file_name);
 static void	do_show(const char* search_string);
 static void	do_show_print(void *arg, param_t param);
 static void	do_set(const char* name, const char* val);
-static void	do_compare(const char* name, const char* val);
+static void	do_compare(const char* name, const char* vals[], unsigned comparisons);
 
 int
 param_main(int argc, char *argv[])
@@ -121,7 +120,7 @@ param_main(int argc, char *argv[])
 
 		if (!strcmp(argv[1], "compare")) {
 			if (argc >= 4) {
-				do_compare(argv[2], argv[3]);
+				do_compare(argv[2], &argv[3], argc - 3);
 			} else {
 				errx(1, "not enough arguments.\nTry 'param compare PARAM_NAME 3'");
 			}
@@ -306,7 +305,7 @@ do_set(const char* name, const char* val)
 }
 
 static void
-do_compare(const char* name, const char* val)
+do_compare(const char* name, const char* vals[], unsigned comparisons)
 {
 	int32_t i;
 	float f;
@@ -330,12 +329,16 @@ do_compare(const char* name, const char* val)
 
 			/* convert string */
 			char* end;
-			int j = strtol(val,&end,10);
-			if (i == j) {
-				printf(" %d: ", i);
-				ret = 0;
-			}
 
+			for (unsigned k = 0; k < comparisons; k++) {
+
+				int j = strtol(vals[k],&end,10);
+
+				if (i == j) {
+					printf(" %d: ", i);
+					ret = 0;
+				}
+			}
 		}
 
 		break;
@@ -345,10 +348,14 @@ do_compare(const char* name, const char* val)
 
 			/* convert string */
 			char* end;
-			float g = strtod(val, &end);
-			if (fabsf(f - g) < 1e-7f) {
-				printf(" %4.4f: ", (double)f);
-				ret = 0;	
+
+			for (unsigned k = 0; k < comparisons; k++) {
+
+				float g = strtod(vals[k], &end);
+				if (fabsf(f - g) < 1e-7f) {
+					printf(" %4.4f: ", (double)f);
+					ret = 0;	
+				}
 			}
 		}
 
@@ -359,7 +366,7 @@ do_compare(const char* name, const char* val)
 	}
 
 	if (ret == 0) {
-		printf("%c %s: equal\n",
+		printf("%c %s: match\n",
 		param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
 		param_name(param));
 	}