Merge branch 'master' into navigator_new_vector

21 files changed, 230 insertions, 180 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 e4561eee3..e2cb8833d 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 6eac00d0c..d647e7d65 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -204,64 +204,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
@@ -270,7 +309,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 +
@@ -279,7 +318,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
@@ -288,7 +327,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 +
@@ -297,7 +336,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
@@ -306,40 +345,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
 
@@ -349,21 +388,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
 	
@@ -412,5 +451,10 @@ then
 	
 	fi
 
+	if [ $EXIT_ON_END == yes ]
+	then
+		exit
+	fi
+
 # End of autostart
 fi