Major autostart rewrite

9 files changed, 453 insertions, 485 deletions

diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil
index 25ea25ae8..8732673f7 100644
--- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil
+++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil
@@ -1,108 +1,42 @@
 #!nsh
-#
-# USB HIL start
-#
 
-echo "[HIL] HILS quadrotor starting.."
+echo "[init] PX4FMU v1, v2 with or without IO on HIL Quad"
 
-#
-# Load default params for this platform
-#
-if param compare SYS_AUTOCONFIG 1
+if [ $DO_AUTOCONFIG == yes ]
 then
-        # Set all params here, then disable autoconfig
-        param set SYS_AUTOCONFIG 0
-
-        param set MC_ATTRATE_D 0.0
-        param set MC_ATTRATE_I 0.0
-        param set MC_ATTRATE_P 0.05
-        param set MC_ATT_D 0.0
-        param set MC_ATT_I 0.0
-        param set MC_ATT_P 3.0
-        param set MC_YAWPOS_D 0.0
-        param set MC_YAWPOS_I 0.0
-        param set MC_YAWPOS_P 2.1
-        param set MC_YAWRATE_D 0.0
-        param set MC_YAWRATE_I 0.0
-        param set MC_YAWRATE_P 0.05
-        param set NAV_TAKEOFF_ALT 3.0
-        param set MPC_TILT_MAX 0.5
-        param set MPC_THR_MAX 0.5
-        param set MPC_THR_MIN 0.1
-        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
-
-# Allow USB some time to come up
-sleep 1
-# Tell MAVLink that this link is "fast"
-mavlink start -b 230400 -d /dev/ttyACM0
-
-# Create a fake HIL /dev/pwm_output interface
-hil mode_pwm
-
-#
-# Force some key parameters to sane values
-# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
-#              see https://pixhawk.ethz.ch/mavlink/
-#
-param set MAV_TYPE 2
-
-#
-# Check if we got an IO
-#
-if px4io start
-then
-	echo "IO started"
-else
-	if hw_ver compare PX4FMU_V1
-	then
-		fmu mode_serial
-		echo "FMU started"
-	fi
+	#
+	# Default parameters for this platform
+	#
+	param set MC_ATTRATE_P 0.12
+	param set MC_ATTRATE_I 0.0
+	param set MC_ATTRATE_D 0.004
+	param set MC_ATT_P 7.0
+	param set MC_ATT_I 0.0
+	param set MC_ATT_D 0.0
+	param set MC_YAWPOS_P 2.0
+	param set MC_YAWPOS_I 0.0
+	param set MC_YAWPOS_D 0.0
+	param set MC_YAWRATE_P 0.3
+	param set MC_YAWRATE_I 0.2
+	param set MC_YAWRATE_D 0.005
+	
+	param set MPC_TILT_MAX 0.5
+	param set MPC_THR_MAX 0.8
+	param set MPC_THR_MIN 0.2
+	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
 fi
 
-#
-# Start the sensors (depends on orb, px4io)
-#
-sh /etc/init.d/rc.sensors
-
-#
-# Start the attitude estimator (depends on orb)
-#
-att_pos_estimator_ekf start
- 
-#
-# Load mixer and start controllers (depends on px4io)
-#
-mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
-
-#
-# Start position estimator
-#
-position_estimator_inav start
-
-#
-# Start attitude control
-#
-multirotor_att_control start
-
-#
-# Start position control
-#
-multirotor_pos_control start
-
-echo "[HIL] setup done, running"
-
+set FRAME_TYPE mc
+set FRAME_GEOMETRY x
+set FRAME_COUNT 4
diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330
index 7054210e2..58c6d99bb 100644
--- a/ROMFS/px4fmu_common/init.d/4010_dji_f330
+++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330
@@ -2,30 +2,27 @@
 
 echo "[init] PX4FMU v1, v2 with or without IO on DJI F330"
 
-#
-# Load default params for this platform
-#
-if param compare SYS_AUTOCONFIG 1
+if [ $DO_AUTOCONFIG == yes ]
 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
+	#
+	# Default parameters for this platform
+	#
 	param set MC_ATTRATE_P 0.12
-	param set MC_ATT_D 0.0
-	param set MC_ATT_I 0.0
+	param set MC_ATTRATE_I 0.0
+	param set MC_ATTRATE_D 0.004
 	param set MC_ATT_P 7.0
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWPOS_I 0.0
+	param set MC_ATT_I 0.0
+	param set MC_ATT_D 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_YAWPOS_I 0.0
+	param set MC_YAWPOS_D 0.0
 	param set MC_YAWRATE_P 0.3
-	param set NAV_TAKEOFF_ALT 3.0
+	param set MC_YAWRATE_I 0.2
+	param set MC_YAWRATE_D 0.005
+	
 	param set MPC_TILT_MAX 0.5
-	param set MPC_THR_MAX 0.7
-	param set MPC_THR_MIN 0.3
+	param set MPC_THR_MAX 0.8
+	param set MPC_THR_MIN 0.2
 	param set MPC_XY_D 0
 	param set MPC_XY_P 0.5
 	param set MPC_XY_VEL_D 0
@@ -38,32 +35,12 @@ then
 	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
 
-#
-# Start and configure PX4IO or FMU interface
-#
-if px4io detect
-then
-	# Start MAVLink (depends on orb)
-	mavlink start
-	usleep 5000
-
-	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
-fi
-
-sh /etc/init.d/rc.mc_interface
-
-#
-# Start common for all multirotors apps
-#
-sh /etc/init.d/rc.multirotor
+set FRAME_TYPE mc
+set FRAME_GEOMETRY x
+set FRAME_COUNT 4
+set PWM_RATE 400
+set PWM_MIN 1200
+set PWM_MAX 1900
+set PWM_DISARMED 900
diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450
index a1d253191..ca055dfcb 100644
--- a/ROMFS/px4fmu_common/init.d/4011_dji_f450
+++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450
@@ -2,14 +2,11 @@
 
 echo "[init] PX4FMU v1, v2 with or without IO on DJI F450"
 
-#
-# Load default params for this platform
-#
-if param compare SYS_AUTOCONFIG 1
+if [ $DO_AUTOCONFIG == yes ]
 then
-	# Set all params here, then disable autoconfig
-	param set SYS_AUTOCONFIG 0
-
+	#
+	# Default parameters for this platform
+	#
 	param set MC_ATTRATE_D 0.004
 	param set MC_ATTRATE_I 0.0
 	param set MC_ATTRATE_P 0.12
@@ -22,53 +19,12 @@ then
 	param set MC_YAWRATE_D 0.005
 	param set MC_YAWRATE_I 0.2
 	param set MC_YAWRATE_P 0.3
-
-	param save
 fi
  
-#
-# Force some key parameters to sane values
-# MAV_TYPE     2 = quadrotor
-#
-param set MAV_TYPE 2
-
-#
-# Start and configure PX4IO or FMU interface
-#
-if px4io detect
-then
-	# Start MAVLink (depends on orb)
-	mavlink start
-	usleep 5000
-
-	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
-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
-
-#
-# Start common multirotor apps
-#
-sh /etc/init.d/rc.multirotor
+set FRAME_TYPE mc
+set FRAME_GEOMETRY x
+set FRAME_COUNT 4
+set PWM_RATE 400
+set PWM_MIN 1200
+set PWM_MAX 1900
+set PWM_DISARMED 900
diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart
new file mode 100644
index 000000000..10b7bc424
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.autostart
@@ -0,0 +1,180 @@
+#
+# 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	..	10999		Wide arm / H frame
+# 11000	..	11999		Hexa Cox
+# 12000	..	12999		Octo Cox
+
+if param compare SYS_AUTOSTART 4008 8
+then
+	#sh /etc/init.d/4008_ardrone
+fi
+
+if param compare SYS_AUTOSTART 4009 9
+then
+	#sh /etc/init.d/4009_ardrone_flow
+fi
+
+if param compare SYS_AUTOSTART 4010 10
+then
+	sh /etc/init.d/4010_dji_f330
+fi
+
+if param compare SYS_AUTOSTART 4011 11
+then
+	sh /etc/init.d/4011_dji_f450
+fi
+
+if param compare SYS_AUTOSTART 4012
+then
+	#sh /etc/init.d/666_fmu_q_x550
+fi
+
+if param compare SYS_AUTOSTART 6012 12
+then
+	#set MIXER /etc/mixers/FMU_hex_x.mix
+	#sh /etc/init.d/rc.hexa
+fi
+
+if param compare SYS_AUTOSTART 7013 13
+then
+	#set MIXER /etc/mixers/FMU_hex_+.mix
+	#sh /etc/init.d/rc.hexa
+fi
+
+if param compare SYS_AUTOSTART 8001
+then
+	#set MIXER /etc/mixers/FMU_octo_x.mix
+	#sh /etc/init.d/rc.octo
+fi
+
+if param compare SYS_AUTOSTART 9001
+then
+	#set MIXER /etc/mixers/FMU_octo_+.mix
+	#sh /etc/init.d/rc.octo
+fi
+
+if param compare SYS_AUTOSTART 12001
+then
+	#set MIXER /etc/mixers/FMU_octo_cox.mix
+	#sh /etc/init.d/rc.octo
+fi
+
+if param compare SYS_AUTOSTART 10015 15
+then
+	#sh /etc/init.d/10015_tbs_discovery
+fi
+
+if param compare SYS_AUTOSTART 10016 16
+then
+	#sh /etc/init.d/10016_3dr_iris
+fi
+
+if param compare SYS_AUTOSTART 4017 17
+then
+	#set MKBLCTRL_MODE no
+	#set MKBLCTRL_FRAME x
+	#sh /etc/init.d/rc.custom_dji_f330_mkblctrl
+fi
+
+if param compare SYS_AUTOSTART 5018 18
+then
+	#set MKBLCTRL_MODE no
+	#set MKBLCTRL_FRAME +
+	#sh /etc/init.d/rc.custom_dji_f330_mkblctrl
+fi
+
+if param compare SYS_AUTOSTART 4019 19
+then
+	#set MKBLCTRL_MODE yes
+	#set MKBLCTRL_FRAME x
+	#sh /etc/init.d/rc.custom_dji_f330_mkblctrl
+fi
+
+if param compare SYS_AUTOSTART 5020 20
+then
+	#set MKBLCTRL_MODE yes
+	#set MKBLCTRL_FRAME +
+	#sh /etc/init.d/rc.custom_dji_f330_mkblctrl
+fi
+
+if param compare SYS_AUTOSTART 4021 21
+then
+	#set FRAME_GEOMETRY x
+	#set ESC_MAKER afro
+	#sh /etc/init.d/rc.custom_io_esc
+fi
+
+if param compare SYS_AUTOSTART 10022 22
+then
+	#set FRAME_GEOMETRY w
+	#sh /etc/init.d/rc.custom_io_esc
+fi
+
+if param compare SYS_AUTOSTART 3030 30
+then
+	#sh /etc/init.d/3030_io_camflyer
+fi
+
+if param compare SYS_AUTOSTART 3031 31
+then
+	#sh /etc/init.d/3031_io_phantom
+fi
+
+if param compare SYS_AUTOSTART 3032 32
+then
+	#sh /etc/init.d/3032_skywalker_x5
+fi
+
+if param compare SYS_AUTOSTART 3033 33
+then
+	#sh /etc/init.d/3033_io_wingwing
+fi
+
+if param compare SYS_AUTOSTART 3034 34
+then
+	#sh /etc/init.d/3034_io_fx79
+	#set MODE custom
+fi
+
+if param compare SYS_AUTOSTART 40
+then
+	#sh /etc/init.d/40_io_segway
+	#set MODE custom
+fi
+
+if param compare SYS_AUTOSTART 2100 100
+then
+	#sh /etc/init.d/2100_mpx_easystar
+	#set MODE custom
+fi
+
+if param compare SYS_AUTOSTART 2101 101
+then
+	#sh /etc/init.d/2101_hk_bixler
+	#set MODE custom
+fi
+
+if param compare SYS_AUTOSTART 2102 102
+then
+	#sh /etc/init.d/2102_3dr_skywalker
+	#set MODE custom
+fi
+
+if param compare SYS_AUTOSTART 800
+then
+	#sh /etc/init.d/800_sdlogger
+	#set MODE custom
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart_hil b/ROMFS/px4fmu_common/init.d/rc.autostart_hil
new file mode 100644
index 000000000..d5fc5eb08
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.autostart_hil
@@ -0,0 +1,34 @@
+#
+# Check if auto-setup from one of the standard scripts for HIL is wanted
+#
+
+if param compare SYS_AUTOSTART 1000
+then
+	#sh /etc/init.d/1000_rc_fw_easystar.hil		
+	set MODE hil
+fi
+
+if param compare SYS_AUTOSTART 1001
+then
+	sh /etc/init.d/1001_rc_quad.hil
+	set MODE hil
+fi
+
+if param compare SYS_AUTOSTART 1002
+then
+	#sh /etc/init.d/1002_rc_fw_state.hil
+	set MODE hil
+fi
+
+if param compare SYS_AUTOSTART 1003
+then
+	#sh /etc/init.d/1003_rc_quad_+.hil
+	set MODE hil
+fi
+
+if param compare SYS_AUTOSTART 1004
+then
+	#sh /etc/init.d/1004_rc_fw_Rascal110.hil
+	set MODE hil
+fi
+
diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io
index 24784610c..5a010cc9b 100644
--- a/ROMFS/px4fmu_common/init.d/rc.io
+++ b/ROMFS/px4fmu_common/init.d/rc.io
@@ -1,23 +1,19 @@
 #
-# Start PX4IO interface (depends on orb, commander)
+# Init PX4IO interface
 #
-if px4io start
-then
-	#
-	# Allow PX4IO to recover from midair restarts.
-	# this is very unlikely, but quite safe and robust.
-	px4io recovery
 
-	#
-	# Disable px4io topic limiting
-	#
-	if hw_ver compare PX4FMU_V1
-	then
-		px4io limit 200
-	else
-		px4io limit 400
-	fi
+#
+# Allow PX4IO to recover from midair restarts.
+# this is very unlikely, but quite safe and robust.
+#
+px4io recovery
+
+#
+# Adjust px4io topic limiting
+#
+if hw_ver compare PX4FMU_V1
+then
+	px4io limit 200
 else
-	# SOS
-	tone_alarm error
+	px4io limit 400
 fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.mc_apps
index bc550ac5a..16a7a33c6 100644
--- a/ROMFS/px4fmu_common/init.d/rc.multirotor
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps
@@ -4,21 +4,6 @@
 #
 
 #
-# Start the sensors and test them.
-#
-sh /etc/init.d/rc.sensors
-
-#
-# Start logging (depends on sensors)
-#
-sh /etc/init.d/rc.logging
-
-#
-# Start GPS interface (depends on orb)
-#
-gps start
-
-#
 # Start the attitude estimator
 #
 attitude_estimator_ekf start
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface
index 6bb2e84ec..2a05012a6 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_interface
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface
@@ -35,15 +35,26 @@ else
     fi
 fi
 
-
 #
 # Set PWM output frequency
 #
-pwm rate -c $OUTPUTS -r $PWM_RATE
+if [ $PWM_RATE != none ]
+then
+	pwm rate -c $OUTPUTS -r $PWM_RATE
+fi
 
 #
-# Set disarmed, min and max PWM signals (for DJI ESCs)
+# Set disarmed, min and max PWM values
 #
-pwm disarmed -c $OUTPUTS -p $PWM_DISARMED
-pwm min -c $OUTPUTS -p $PWM_MIN
-pwm max -c $OUTPUTS -p $PWM_MAX
+if [ $PWM_DISARMED != none ]
+then
+	pwm disarmed -c $OUTPUTS -p $PWM_DISARMED
+fi
+if [ $PWM_MIN != none ]
+then
+	pwm min -c $OUTPUTS -p $PWM_MIN
+fi
+if [ $PWM_MAX != none ]
+then
+	pwm max -c $OUTPUTS -p $PWM_MAX
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 8801d1126..4dd9af407 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -60,12 +60,9 @@ fi
 if [ $MODE == autostart ]
 then
 	#
-	# Start terminal
+	# Start CDC/ACM serial driver
 	#
-	if sercon
-	then
-		echo "USB connected"
-	fi
+	sercon
 	
 	#
 	# Start the ORB (first app to start)
@@ -107,52 +104,79 @@ then
 			blinkm systemstate
 		fi
 	fi
+
+	set USE_IO no
+	set FRAME_TYPE none
+	set PWM_RATE none
+	set PWM_DISARMED none
+	set PWM_MIN none
+	set PWM_MAX none
+	
+	if param compare SYS_AUTOCONFIG 1
+	then
+		set DO_AUTOCONFIG yes
+	else
+		set DO_AUTOCONFIG no
+	fi
 	
 	#
 	# Start the Commander (needs to be this early for in-air-restarts)
 	#
 	commander start
-
-	if param compare SYS_AUTOSTART 1000
-	then
-		sh /etc/init.d/1000_rc_fw_easystar.hil		
-		set MODE custom
-	fi
-	
-	if param compare SYS_AUTOSTART 1001
-	then
-		sh /etc/init.d/1001_rc_quad.hil
-		set MODE custom
-	fi
 	
-	if param compare SYS_AUTOSTART 1002
-	then
-		sh /etc/init.d/1002_rc_fw_state.hil
-		set MODE custom
-	fi
+	#
+	# Set parameters and env variables for selected AUTOSTART (HIL setups)
+	#
+	sh /etc/init.d/rc.autostart_hil
 	
-	if param compare SYS_AUTOSTART 1003
-	then
-		sh /etc/init.d/1003_rc_quad_+.hil
-		set MODE custom
-	fi
-
-	if param compare SYS_AUTOSTART 1004
-	then
-		sh /etc/init.d/1004_rc_fw_Rascal110.hil
-		set MODE custom
-	fi
+	if [ $MODE == hil ]
+	then
+		#
+		# Do common HIL setup depending on env variables
+		#
+		# Allow USB some time to come up
+		sleep 1
+
+		# Start MAVLink on USB port
+		mavlink start -b 230400 -d /dev/ttyACM0
+		usleep 5000
+		
+		# Create a fake HIL /dev/pwm_output interface
+		hil mode_pwm
+		
+		# Sensors
+		echo "Start sensors"
+		sh /etc/init.d/rc.sensors
+		
+		#
+		# Fixed wing setup
+		#
+		if [ $FRAME_TYPE == fw ]
+		then
+			echo "Setup FIXED WING"
+		fi
 	
-	if [ $MODE != custom ]
-	then
-		# Try to get an USB console
+		#
+		# Multicopters setup
+		#
+		if [ $FRAME_TYPE == mc ]
+		then
+			echo "Setup MULTICOPTER"
+			
+			# Load mixer and configure outputs
+			sh /etc/init.d/rc.mc_interface
+			
+			# Start common multicopter apps
+			sh /etc/init.d/rc.mc_apps
+		fi
+	else
+		# Try to get an USB console if not in HIL mode
 		nshterm /dev/ttyACM0 &
 	fi
-
+		
 	#
 	# Upgrade PX4IO firmware
 	#
-
 	if [ -f /etc/extras/px4io-v2_default.bin ]
 	then
 		set io_file /etc/extras/px4io-v2_default.bin
@@ -170,6 +194,7 @@ then
 	then
 		echo "PX4IO CRC OK"
 		echo "PX4IO CRC OK" >> $logfile
+		set USE_IO yes
 	else
 		echo "PX4IO CRC failure"
 		echo "PX4IO CRC failure" >> $logfile
@@ -182,6 +207,7 @@ then
 				echo "PX4IO restart OK"
 				echo "PX4IO restart OK" >> $logfile
 				tone_alarm MSPAA
+				set USE_IO yes
 			else
 				echo "PX4IO restart failed"
 				echo "PX4IO restart failed" >> $logfile
@@ -200,222 +226,91 @@ then
 	fi
 
 	set EXIT_ON_END no
+	set USE_LOGGING yes
+	set USE_GPS yes
 	
 	#
-	# Check if auto-setup from one of the standard scripts is wanted
-	# SYS_AUTOSTART = 0 means no autostart (default)
+	# Set parameters and env variables for selected AUTOSTART
 	#
-	# 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	..	10999		Wide arm / H frame
-	# 11000	..	11999		Hexa Cox
-	# 12000	..	12999		Octo Cox
-	
-	if param compare SYS_AUTOSTART 4008 8
-	then
-		sh /etc/init.d/4008_ardrone
-		set MODE custom
-	fi
-	
-	if param compare SYS_AUTOSTART 4009 9
-	then
-		sh /etc/init.d/4009_ardrone_flow
-		set MODE custom
-	fi
-	
-	if param compare SYS_AUTOSTART 4010 10
-	then
-		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 4011 11
-	then
-		sh /etc/init.d/4011_dji_f450
-		set MODE custom
-	fi
-
-	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/rc.hexa
-		set MODE custom
-	fi
-
-	if param compare SYS_AUTOSTART 7013 13
-	then
-		set MIXER /etc/mixers/FMU_hex_+.mix
-		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 12001
-	then
-		set MIXER /etc/mixers/FMU_octo_cox.mix
-		sh /etc/init.d/rc.octo
-		set MODE custom
-	fi
+	sh /etc/init.d/rc.autostart
 	
-	if param compare SYS_AUTOSTART 10015 15
-	then
-		sh /etc/init.d/10015_tbs_discovery
-		set MODE custom
-	fi
-
-	if param compare SYS_AUTOSTART 10016 16
-	then
-		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 4017 17
-	then
-		set MKBLCTRL_MODE no
-		set MKBLCTRL_FRAME x
-		sh /etc/init.d/rc.custom_dji_f330_mkblctrl
-		set MODE custom
-	fi
-
-	# PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame
-	if param compare SYS_AUTOSTART 5018 18
-	then
-		set MKBLCTRL_MODE no
-		set MKBLCTRL_FRAME +
-		sh /etc/init.d/rc.custom_dji_f330_mkblctrl
-		set MODE custom
-	fi
-
-	# PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing
-	if param compare SYS_AUTOSTART 4019 19
-	then
-		set MKBLCTRL_MODE yes
-		set MKBLCTRL_FRAME x
-		sh /etc/init.d/rc.custom_dji_f330_mkblctrl
-		set MODE custom
-	fi
-
-	# PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing
-	if param compare SYS_AUTOSTART 5020 20
-	then
-		set MKBLCTRL_MODE yes
-		set MKBLCTRL_FRAME +
-		sh /etc/init.d/rc.custom_dji_f330_mkblctrl
-		set MODE custom
-	fi
-
-	# PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame
-	if param compare SYS_AUTOSTART 4021 21
-	then
-		set FRAME_GEOMETRY x
-		set ESC_MAKER afro
-		sh /etc/init.d/rc.custom_io_esc
-		set MODE custom
-	fi
-
-	# PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame
-	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 3030 30
-	then
-		sh /etc/init.d/3030_io_camflyer
-		set MODE custom
-	fi
-
-	if param compare SYS_AUTOSTART 3031 31
+	#
+	# If autoconfig parameter was set, reset it and save parameters
+	#
+	if [ $DO_AUTOCONFIG == yes ]
 	then
-		sh /etc/init.d/3031_io_phantom
-		set MODE custom
+		param set SYS_AUTOCONFIG 0
+		param save
 	fi
 	
-	if param compare SYS_AUTOSTART 3032 32
-	then
-		sh /etc/init.d/3032_skywalker_x5
-		set MODE custom
-	fi
-
-	if param compare SYS_AUTOSTART 3033 33
-	then
-		sh /etc/init.d/3033_io_wingwing
-		set MODE custom
-	fi
-
-        if param compare SYS_AUTOSTART 3034 34
-        then
-                sh /etc/init.d/3034_io_fx79
-                set MODE custom
-        fi
-
-	if param compare SYS_AUTOSTART 40
-	then
-		sh /etc/init.d/40_io_segway
-		set MODE custom
-	fi
+	if [ $MODE == autostart ]
+	then	
+		#
+		# Do common setup depending on env variables
+		#
+		if [ $USE_IO == yes ]
+		then
+			echo "Use IO"
+			
+			# Start MAVLink on default port: ttyS1
+			mavlink start
+			usleep 5000	
+			
+			sh /etc/init.d/rc.io
+		else
+			echo "Don't use IO"
+			
+			# Start MAVLink on ttyS0
+			mavlink start -d /dev/ttyS0
+			usleep 5000
+			
+			# Configure FMU for PWM outputs
+			fmu mode_pwm
+			
+			# Exit from nsh to free port for mavlink
+			set EXIT_ON_END yes
+		fi
+		
+		# Sensors
+		echo "Start sensors"
+		sh /etc/init.d/rc.sensors
+		
+		# Logging
+		if [ $USE_LOGGING == yes ]
+		then
+			sh /etc/init.d/rc.logging
+		fi
+		
+		# GPS interface
+		if [ $USE_GPS == yes ]
+		then
+			gps start
+		fi
+		
+		#
+		# Fixed wing setup
+		#
+		if [ $FRAME_TYPE == fw ]
+		then
+			echo "Setup FIXED WING"
+		fi
 	
-	if param compare SYS_AUTOSTART 2100 100
-	then
-		sh /etc/init.d/2100_mpx_easystar
-		set MODE custom
-	fi
-
-	if param compare SYS_AUTOSTART 2101 101
-	then
-		sh /etc/init.d/2101_hk_bixler
-		set MODE custom
+		#
+		# Multicopters setup
+		#
+		if [ $FRAME_TYPE == mc ]
+		then
+			echo "Setup MULTICOPTER"
+			
+			# Load mixer and configure outputs
+			sh /etc/init.d/rc.mc_interface
+			
+			# Start common multicopter apps
+			sh /etc/init.d/rc.mc_apps
+		fi
 	fi
 
-	if param compare SYS_AUTOSTART 2102 102
-	then
-		sh /etc/init.d/2102_3dr_skywalker
-		set MODE custom
-	fi
-	
-	if param compare SYS_AUTOSTART 800
-	then
-		sh /etc/init.d/800_sdlogger
-		set MODE custom
-	fi
-	
-	# Start any custom extensions that might be missing
+	# Start any custom extensions
 	if [ -f /fs/microsd/etc/rc.local ]
 	then
 		sh /fs/microsd/etc/rc.local