Merge branch 'pwm_ioctls' of github.com:PX4/Firmware into pwm_ioctls

39 files changed, 1628 insertions, 178 deletions

diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone
index f6d82a5ec..f6f09ac22 100644
--- a/ROMFS/px4fmu_common/init.d/08_ardrone
+++ b/ROMFS/px4fmu_common/init.d/08_ardrone
@@ -8,7 +8,18 @@ echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board"
 if param compare SYS_AUTOCONFIG 1
 then
 	# Set all params here, then disable autoconfig
-	# TODO
+	param set MC_ATTRATE_D 0
+	param set MC_ATTRATE_I 0
+	param set MC_ATTRATE_P 0.13
+	param set MC_ATT_D 0
+	param set MC_ATT_I 0.3
+	param set MC_ATT_P 5
+	param set MC_YAWPOS_D 0.1
+	param set MC_YAWPOS_I 0.15
+	param set MC_YAWPOS_P 1
+	param set MC_YAWRATE_D 0
+	param set MC_YAWRATE_I 0
+	param set MC_YAWRATE_P 0.15
 
 	param set SYS_AUTOCONFIG 0
 	param save
@@ -19,6 +30,7 @@ fi
 # MAV_TYPE     2 = quadrotor
 #
 param set MAV_TYPE 2
+param set BAT_V_SCALING 0.008381
 
 #
 # Start MAVLink
diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow
index 794342a0b..9b739f245 100644
--- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow
+++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow
@@ -8,7 +8,18 @@ echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW"
 if param compare SYS_AUTOCONFIG 1
 then
 	# Set all params here, then disable autoconfig
-	# TODO
+	param set MC_ATTRATE_D 0
+	param set MC_ATTRATE_I 0
+	param set MC_ATTRATE_P 0.13
+	param set MC_ATT_D 0
+	param set MC_ATT_I 0.3
+	param set MC_ATT_P 5
+	param set MC_YAWPOS_D 0.1
+	param set MC_YAWPOS_I 0.15
+	param set MC_YAWPOS_P 1
+	param set MC_YAWRATE_D 0
+	param set MC_YAWRATE_I 0
+	param set MC_YAWRATE_P 0.15
 
 	param set SYS_AUTOCONFIG 0
 	param save
@@ -19,6 +30,7 @@ fi
 # MAV_TYPE     2 = quadrotor
 #
 param set MAV_TYPE 2
+param set BAT_V_SCALING 0.008381
 
 #
 # Start MAVLink and MAVLink Onboard (PX4FLOW Sensor)
diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
new file mode 100644
index 000000000..5e4028bbb
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
@@ -0,0 +1,92 @@
+#!nsh
+#
+# USB HIL start
+#
+
+echo "[HIL] HILStar starting.."
+
+#
+# Load default params for this platform
+#
+if param compare SYS_AUTOCONFIG 1
+then
+	# Set all params here, then disable autoconfig
+	
+	param set FW_P_D 0
+	param set FW_P_I 0
+	param set FW_P_IMAX 15
+	param set FW_P_LIM_MAX 50
+	param set FW_P_LIM_MIN -50
+	param set FW_P_P 60
+	param set FW_P_RMAX_NEG 0
+	param set FW_P_RMAX_POS 0
+	param set FW_P_ROLLFF 1.1
+	param set FW_R_D 0
+	param set FW_R_I 5
+	param set FW_R_IMAX 20
+	param set FW_R_P 100
+	param set FW_R_RMAX 100
+	param set FW_THR_CRUISE 0.65
+	param set FW_THR_MAX 1
+	param set FW_THR_MIN 0
+	param set FW_T_SINK_MAX 5.0
+	param set FW_T_SINK_MIN 4.0
+	param set FW_Y_ROLLFF 1.1
+	param set FW_L1_PERIOD 16
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
+
+	param set SYS_AUTOCONFIG 0
+	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 1
+
+#
+# Start the commander (depends on orb, mavlink)
+#
+commander start
+
+#
+# Check if we got an IO
+#
+if px4io start
+then
+	echo "IO started"
+else
+	fmu mode_serial
+	echo "FMU started"
+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_AET.mix
+fw_pos_control_l1 start
+fw_att_control start
+
+echo "[HIL] setup done, running"
+
diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil
new file mode 100644
index 000000000..bbe3b7e28
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil
@@ -0,0 +1,110 @@
+#!nsh
+#
+# USB HIL start
+#
+
+echo "[HIL] HILS quadrotor starting.."
+
+#
+# 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.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
+
+#
+# Start the commander (depends on orb, mavlink)
+#
+commander start
+
+#
+# Check if we got an IO
+#
+if px4io start
+then
+	echo "IO started"
+else
+	fmu mode_serial
+	echo "FMU started"
+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"
+
diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil
new file mode 100644
index 000000000..d5782a89b
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil
@@ -0,0 +1,84 @@
+#!nsh
+#
+# USB HIL start
+#
+
+echo "[HIL] HILStar starting in state-HIL mode.."
+
+#
+# Load default params for this platform
+#
+if param compare SYS_AUTOCONFIG 1
+then
+	# Set all params here, then disable autoconfig
+	
+	param set FW_P_D 0
+	param set FW_P_I 0
+	param set FW_P_IMAX 15
+	param set FW_P_LIM_MAX 50
+	param set FW_P_LIM_MIN -50
+	param set FW_P_P 60
+	param set FW_P_RMAX_NEG 0
+	param set FW_P_RMAX_POS 0
+	param set FW_P_ROLLFF 1.1
+	param set FW_R_D 0
+	param set FW_R_I 5
+	param set FW_R_IMAX 20
+	param set FW_R_P 100
+	param set FW_R_RMAX 100
+	param set FW_THR_CRUISE 0.65
+	param set FW_THR_MAX 1
+	param set FW_THR_MIN 0
+	param set FW_T_SINK_MAX 5.0
+	param set FW_T_SINK_MIN 4.0
+	param set FW_Y_ROLLFF 1.1
+
+	param set SYS_AUTOCONFIG 0
+	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 1
+
+#
+# Start the commander (depends on orb, mavlink)
+#
+commander start
+
+#
+# Check if we got an IO
+#
+if px4io start
+then
+	echo "IO started"
+else
+	fmu mode_serial
+	echo "FMU started"
+fi
+
+#
+# Start the sensors (depends on orb, px4io)
+#
+sh /etc/init.d/rc.sensors
+ 
+#
+# Load mixer and start controllers (depends on px4io)
+#
+mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix
+fw_pos_control_l1 start
+fw_att_control start
+
+echo "[HIL] setup done, running"
+
diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
new file mode 100644
index 000000000..1c85e04f2
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
@@ -0,0 +1,110 @@
+#!nsh
+#
+# USB HIL start
+#
+
+echo "[HIL] HILS quadrotor + starting.."
+
+#
+# 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.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
+
+#
+# Start the commander (depends on orb, mavlink)
+#
+commander start
+
+#
+# Check if we got an IO
+#
+if px4io start
+then
+	echo "IO started"
+else
+	fmu mode_serial
+	echo "FMU started"
+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_+.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"
+
diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
new file mode 100644
index 000000000..8c5e4b6a8
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
@@ -0,0 +1,103 @@
+#!nsh
+#
+# USB HIL start
+#
+
+echo "[HIL] HILStar starting.."
+
+#
+# Load default params for this platform
+#
+if param compare SYS_AUTOCONFIG 1
+then
+	# Set all params here, then disable autoconfig
+	
+	param set FW_P_D 0
+	param set FW_P_I 0
+	param set FW_P_IMAX 15
+	param set FW_P_LIM_MAX 50
+	param set FW_P_LIM_MIN -50
+	param set FW_P_P 60
+	param set FW_P_RMAX_NEG 0
+	param set FW_P_RMAX_POS 0
+	param set FW_P_ROLLFF 1.1
+	param set FW_R_D 0
+	param set FW_R_I 5
+	param set FW_R_IMAX 20
+	param set FW_R_P 100
+	param set FW_R_RMAX 100
+	param set FW_THR_CRUISE 0.65
+	param set FW_THR_MAX 1
+	param set FW_THR_MIN 0
+	param set FW_T_SINK_MAX 5.0
+	param set FW_T_SINK_MIN 4.0
+	param set FW_Y_ROLLFF 1.1
+	param set FW_L1_PERIOD 16
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
+
+	param set SYS_AUTOCONFIG 0
+	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 1
+
+#
+# Start the commander (depends on orb, mavlink)
+#
+commander start
+
+#
+# Check if we got an IO
+#
+if px4io start
+then
+	echo "IO started"
+else
+	fmu mode_serial
+	echo "FMU started"
+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)
+#
+set MODE autostart
+mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
+if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ]
+then
+	echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix"
+	mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix
+else
+	echo "Using /etc/mixers/FMU_AERT.mix"
+	mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
+fi
+
+
+fw_pos_control_l1 start
+fw_att_control start
+
+echo "[HIL] setup done, running"
+
diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar
index e1cefdb99..4f843e9aa 100644
--- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar
+++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar
@@ -1,6 +1,6 @@
 #!nsh
 
-echo "[init] Multiplex Easystar"
+echo "[init] PX4FMU v1, v2 with or without IO on EasyStar"
 
 #
 # Load default params for this platform
@@ -8,7 +8,29 @@ echo "[init] Multiplex Easystar"
 if param compare SYS_AUTOCONFIG 1
 then
 	# Set all params here, then disable autoconfig
-	# TODO
+	param set FW_P_D 0
+	param set FW_P_I 0
+	param set FW_P_IMAX 15
+	param set FW_P_LIM_MAX 50
+	param set FW_P_LIM_MIN -50
+	param set FW_P_P 60
+	param set FW_P_RMAX_NEG 0
+	param set FW_P_RMAX_POS 0
+	param set FW_P_ROLLFF 1.1
+	param set FW_R_D 0
+	param set FW_R_I 5
+	param set FW_R_IMAX 20
+	param set FW_R_P 100
+	param set FW_R_RMAX 100
+	param set FW_THR_CRUISE 0.65
+	param set FW_THR_MAX 1
+	param set FW_THR_MIN 0
+	param set FW_T_SINK_MAX 5.0
+	param set FW_T_SINK_MIN 4.0
+	param set FW_Y_ROLLFF 1.1
+	param set FW_L1_PERIOD 16
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
 	
 	param set SYS_AUTOCONFIG 0
 	param save
@@ -20,28 +42,34 @@ fi
 #
 param set MAV_TYPE 1
 
-#
-# Start MAVLink (depends on orb)
-#
-mavlink start -d /dev/ttyS1 -b 57600
-usleep 5000
+set EXIT_ON_END no
 
 #
-# Start and configure PX4IO interface
+# Start and configure PX4IO or FMU interface
 #
-sh /etc/init.d/rc.io
+if px4io detect
+then
+	# Start MAVLink (depends on orb)
+	mavlink start
+
+	commander start
+
+	sh /etc/init.d/rc.io
+	# Limit to 100 Hz updates and (implicit) 50 Hz PWM
+	px4io limit 100
+else
+	# Start MAVLink (on UART1 / ttyS0)
+	mavlink start -d /dev/ttyS0
+
+	commander start
+
+	fmu mode_pwm
+	param set BAT_V_SCALING 0.004593
+	set EXIT_ON_END yes
+fi
 
 #
-# Set actuator limit to 100 Hz update (50 Hz PWM)
-px4io limit 100
- 
-#
-# Start the commander
-#
-commander start
- 
-#
-# Start the sensors
+# Start the sensors and test them.
 #
 sh /etc/init.d/rc.sensors
 
@@ -49,9 +77,9 @@ sh /etc/init.d/rc.sensors
 # Start logging (depends on sensors)
 #
 sh /etc/init.d/rc.logging
- 
+
 #
-# Start GPS interface
+# Start GPS interface (depends on orb)
 #
 gps start
  
@@ -63,6 +91,18 @@ att_pos_estimator_ekf start
 #
 # Load mixer and start controllers (depends on px4io)
 #
-mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix
+if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ]
+then
+	echo "Using FMU_RET mixer from sd card"
+	mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix
+else
+	echo "Using standard FMU_RET mixer"
+	mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix
+fi
 fw_att_control start
 fw_pos_control_l1 start
+
+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/101_hk_bixler
new file mode 100644
index 000000000..cef86c34d
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler
@@ -0,0 +1,101 @@
+#!nsh
+
+echo "[init] PX4FMU v1, v2 with or without IO on HK Bixler"
+
+#
+# Load default params for this platform
+#
+if param compare SYS_AUTOCONFIG 1
+then
+	# Set all params here, then disable autoconfig
+	param set FW_P_D 0
+	param set FW_P_I 0
+	param set FW_P_IMAX 15
+	param set FW_P_LIM_MAX 50
+	param set FW_P_LIM_MIN -50
+	param set FW_P_P 60
+	param set FW_P_RMAX_NEG 0
+	param set FW_P_RMAX_POS 0
+	param set FW_P_ROLLFF 1.1
+	param set FW_R_D 0
+	param set FW_R_I 5
+	param set FW_R_IMAX 20
+	param set FW_R_P 100
+	param set FW_R_RMAX 100
+	param set FW_THR_CRUISE 0.65
+	param set FW_THR_MAX 1
+	param set FW_THR_MIN 0
+	param set FW_T_SINK_MAX 5.0
+	param set FW_T_SINK_MIN 4.0
+	param set FW_Y_ROLLFF 1.1
+	param set FW_L1_PERIOD 16
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
+	
+	param set SYS_AUTOCONFIG 0
+	param save
+fi
+ 
+#
+# Force some key parameters to sane values
+# MAV_TYPE     1 = fixed wing
+#
+param set MAV_TYPE 1
+
+set EXIT_ON_END no
+
+#
+# Start and configure PX4IO or FMU interface
+#
+if px4io detect
+then
+	# Start MAVLink (depends on orb)
+	mavlink start
+
+	commander start
+
+	sh /etc/init.d/rc.io
+	# Limit to 100 Hz updates and (implicit) 50 Hz PWM
+	px4io limit 100
+else
+	# Start MAVLink (on UART1 / ttyS0)
+	mavlink start -d /dev/ttyS0
+
+	commander start
+
+	fmu mode_pwm
+	param set BAT_V_SCALING 0.004593
+	set EXIT_ON_END yes
+fi
+ 
+#
+# 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 and position estimator
+#
+att_pos_estimator_ekf start
+ 
+#
+# Load mixer and start controllers (depends on px4io)
+#
+mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
+fw_att_control start
+fw_pos_control_l1 start
+
+if [ $EXIT_ON_END == yes ]
+then
+	exit
+fi
diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330
index b8fe5fc31..81ea292aa 100644
--- a/ROMFS/px4fmu_common/init.d/10_dji_f330
+++ b/ROMFS/px4fmu_common/init.d/10_dji_f330
@@ -59,11 +59,10 @@ then
 	mavlink start
 	usleep 5000
 
+	commander start
+
 	sh /etc/init.d/rc.io
 	# Set PWM values for DJI ESCs
-	px4io idle 900 900 900 900
-	px4io min 1200 1200 1200 1200
-	px4io max 1800 1800 1800 1800
 else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
@@ -81,7 +80,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
 #
 # Set PWM output frequency
 #
-pwm -u 400 -m 0xff
+pwm rate -c 1234 -r 400
+
+#
+# Set disarmed, min and max PWM signals
+#
+pwm disarmed -c 1234 -p 900
+pwm min -c 1234 -p 1200
+pwm max -c 1234 -p 1800
 
 #
 # Start common for all multirotors apps
diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450
index 388f40a47..4dbf76cee 100644
--- a/ROMFS/px4fmu_common/init.d/11_dji_f450
+++ b/ROMFS/px4fmu_common/init.d/11_dji_f450
@@ -43,11 +43,9 @@ then
 	mavlink start
 	usleep 5000
 
+	commander start
+
 	sh /etc/init.d/rc.io
-	# Set PWM values for DJI ESCs
-	px4io idle 900 900 900 900
-	px4io min 1200 1200 1200 1200
-	px4io max 1800 1800 1800 1800
 else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
@@ -65,7 +63,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
 #
 # Set PWM output frequency
 #
-pwm -u 400 -m 0xff
+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 for all multirotors apps
diff --git a/ROMFS/px4fmu_common/init.d/12-13_hex b/ROMFS/px4fmu_common/init.d/12-13_hex
new file mode 100644
index 000000000..0f0bb05ce
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/12-13_hex
@@ -0,0 +1,95 @@
+#!nsh
+
+echo "[init] PX4FMU v1, v2 with or without IO on Hex"
+
+#
+# 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/
+# 13 = hexarotor
+#
+param set MAV_TYPE 13
+
+set EXIT_ON_END no
+
+#
+# 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
+	# Set PWM values for DJI ESCs
+	px4io idle 900 900 900 900 900 900
+	px4io min 1200 1200 1200 1200 1200 1200
+	px4io max 1900 1900 1900 1900 1900 1900
+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 $MIXER
+
+#
+# Set PWM output frequency to 400 Hz
+#
+pwm -u 400 -m 0xff
+
+#
+# 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/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery
index cbfde6d3c..bd6189a6d 100644
--- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery
+++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery
@@ -43,11 +43,9 @@ then
 	mavlink start
 	usleep 5000
 
+	commander start
+
 	sh /etc/init.d/rc.io
-	# Set PWM values for DJI ESCs
-	px4io idle 900 900 900 900
-	px4io min 1200 1200 1200 1200
-	px4io max 1800 1800 1800 1800
 else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
@@ -65,7 +63,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
 #
 # Set PWM output frequency
 #
-pwm -u 400 -m 0xff
+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 for all multirotors apps
diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris
index b6c5fbdea..d8cc0e913 100644
--- a/ROMFS/px4fmu_common/init.d/16_3dr_iris
+++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris
@@ -10,12 +10,12 @@ then
 	# Set all params here, then disable autoconfig
 	param set SYS_AUTOCONFIG 0
 
-	param set MC_ATTRATE_D 0.006
+	param set MC_ATTRATE_D 0.004
 	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_P 0.17
+	param set MC_ATTRATE_P 0.13
 	param set MC_ATT_D 0.0
 	param set MC_ATT_I 0.0
-	param set MC_ATT_P 5.0
+	param set MC_ATT_P 9.0
 	param set MC_YAWPOS_D 0.0
 	param set MC_YAWPOS_I 0.15
 	param set MC_YAWPOS_P 0.5
@@ -43,17 +43,15 @@ then
 	mavlink start
 	usleep 5000
 
+	commander start
+
 	sh /etc/init.d/rc.io
-	# Set PWM values for DJI ESCs
-	px4io idle 900 900 900 900
-	px4io min 1200 1200 1200 1200
-	px4io max 1800 1800 1800 1800
 else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
 	usleep 5000
 	fmu mode_pwm
-	param set BAT_V_SCALING 0.004593
+	param set BAT_V_SCALING 0.0098
 	set EXIT_ON_END yes
 fi
 
@@ -65,7 +63,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
 #
 # Set PWM output frequency
 #
-pwm -u 400 -m 0xff
+pwm rate -c 1234 -r 400
+
+#
+# Set disarmed, min and max PWM signals
+#
+pwm disarmed -c 1234 -p 900
+pwm min -c 1234 -p 1200
+pwm max -c 1234 -p 1800
 
 #
 # Start common for all multirotors apps
diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer
index f7e653362..191d8cd95 100644
--- a/ROMFS/px4fmu_common/init.d/30_io_camflyer
+++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer
@@ -1,6 +1,6 @@
 #!nsh
 
-echo "[init] 30_io_camflyer: PX4FMU+PX4IO on Camflyer"
+echo "[init] PX4FMU v1, v2 with or without IO on Camflyer"
 
 #
 # Load default params for this platform
@@ -19,34 +19,35 @@ fi
 # MAV_TYPE     1 = fixed wing
 #
 param set MAV_TYPE 1
- 
-#
-# Start MAVLink (depends on orb)
-#
-mavlink start -d /dev/ttyS1 -b 57600
-usleep 5000
- 
-#
-# Start the commander (depends on orb, mavlink)
-#
-commander start
- 
-#
-# Start PX4IO interface (depends on orb, commander)
+
+set EXIT_ON_END no
+
 #
-px4io start
- 
+# Start and configure PX4IO or FMU interface
 #
-# Allow PX4IO to recover from midair restarts.
-# this is very unlikely, but quite safe and robust.
-px4io recovery
+if px4io detect
+then
+	# Start MAVLink (depends on orb)
+	mavlink start
 
-#
-# Set actuator limit to 100 Hz update (50 Hz PWM)
-px4io limit 100
+	commander start
+
+	sh /etc/init.d/rc.io
+	# Limit to 100 Hz updates and (implicit) 50 Hz PWM
+	px4io limit 100
+else
+	# Start MAVLink (on UART1 / ttyS0)
+	mavlink start -d /dev/ttyS0
+
+	commander start
+
+	fmu mode_pwm
+	param set BAT_V_SCALING 0.004593
+	set EXIT_ON_END yes
+fi
  
 #
-# Start the sensors (depends on orb, px4io)
+# Start the sensors and test them.
 #
 sh /etc/init.d/rc.sensors
 
@@ -54,7 +55,7 @@ sh /etc/init.d/rc.sensors
 # Start logging (depends on sensors)
 #
 sh /etc/init.d/rc.logging
- 
+
 #
 # Start GPS interface (depends on orb)
 #
@@ -71,3 +72,8 @@ att_pos_estimator_ekf start
 mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
 fw_att_control start
 fw_pos_control_l1 start
+
+if [ $EXIT_ON_END == yes ]
+then
+	exit
+fi
diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom
index e1e609927..652833745 100644
--- a/ROMFS/px4fmu_common/init.d/31_io_phantom
+++ b/ROMFS/px4fmu_common/init.d/31_io_phantom
@@ -1,6 +1,6 @@
 #!nsh
 
-echo "[init] 31_io_phantom: PX4FMU+PX4IO on Phantom"
+echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV"
 
 #
 # Load default params for this platform
@@ -8,7 +8,29 @@ echo "[init] 31_io_phantom: PX4FMU+PX4IO on Phantom"
 if param compare SYS_AUTOCONFIG 1
 then
 	# Set all params here, then disable autoconfig
-	# TODO
+	param set FW_P_D 0
+	param set FW_P_I 0
+	param set FW_P_IMAX 15
+	param set FW_P_LIM_MAX 50
+	param set FW_P_LIM_MIN -50
+	param set FW_P_P 60
+	param set FW_P_RMAX_NEG 0
+	param set FW_P_RMAX_POS 0
+	param set FW_P_ROLLFF 1.1
+	param set FW_R_D 0
+	param set FW_R_I 5
+	param set FW_R_IMAX 20
+	param set FW_R_P 100
+	param set FW_R_RMAX 100
+	param set FW_THR_CRUISE 0.65
+	param set FW_THR_MAX 1
+	param set FW_THR_MIN 0
+	param set FW_T_SINK_MAX 5.0
+	param set FW_T_SINK_MIN 4.0
+	param set FW_Y_ROLLFF 1.1
+	param set FW_L1_PERIOD 17
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
 	
 	param set SYS_AUTOCONFIG 0
 	param save
@@ -20,28 +42,34 @@ fi
 #
 param set MAV_TYPE 1
 
-#
-# Start MAVLink (depends on orb)
-#
-mavlink start -d /dev/ttyS1 -b 57600
-usleep 5000
+set EXIT_ON_END no
 
 #
-# Start and configure PX4IO interface
+# Start and configure PX4IO or FMU interface
 #
-sh /etc/init.d/rc.io
+if px4io detect
+then
+	# Start MAVLink (depends on orb)
+	mavlink start
 
-#
-# Set actuator limit to 100 Hz update (50 Hz PWM)
-px4io limit 100
- 
-#
-# Start the commander
-#
-commander start
+	commander start
+
+	sh /etc/init.d/rc.io
+	# Limit to 100 Hz updates and (implicit) 50 Hz PWM
+	px4io limit 100
+else
+	# Start MAVLink (on UART1 / ttyS0)
+	mavlink start -d /dev/ttyS0
+
+	commander start
+
+	fmu mode_pwm
+	param set BAT_V_SCALING 0.004593
+	set EXIT_ON_END yes
+fi
  
 #
-# Start the sensors
+# Start the sensors and test them.
 #
 sh /etc/init.d/rc.sensors
 
@@ -49,9 +77,9 @@ sh /etc/init.d/rc.sensors
 # Start logging (depends on sensors)
 #
 sh /etc/init.d/rc.logging
- 
+
 #
-# Start GPS interface
+# Start GPS interface (depends on orb)
 #
 gps start
  
@@ -66,3 +94,8 @@ att_pos_estimator_ekf start
 mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
 fw_att_control start
 fw_pos_control_l1 start
+
+if [ $EXIT_ON_END == yes ]
+then
+	exit
+fi
diff --git a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 b/ROMFS/px4fmu_common/init.d/32_skywalker_x5
new file mode 100644
index 000000000..cd7677112
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/32_skywalker_x5
@@ -0,0 +1,86 @@
+#!nsh
+
+echo "[init] PX4FMU v1, v2 with or without IO on Skywalker X5"
+
+#
+# Load default params for this platform
+#
+if param compare SYS_AUTOCONFIG 1
+then
+	# Set all params here, then disable autoconfig
+	# TODO
+	
+	param set SYS_AUTOCONFIG 0
+	param save
+fi
+ 
+#
+# Force some key parameters to sane values
+# MAV_TYPE     1 = fixed wing
+#
+param set MAV_TYPE 1
+
+set EXIT_ON_END no
+
+#
+# Start and configure PX4IO or FMU interface
+#
+if px4io detect
+then
+	# Start MAVLink (depends on orb)
+	mavlink start
+
+	commander start
+
+	sh /etc/init.d/rc.io
+	# Limit to 100 Hz updates and (implicit) 50 Hz PWM
+	px4io limit 100
+else
+	# Start MAVLink (on UART1 / ttyS0)
+	mavlink start -d /dev/ttyS0
+
+	commander start
+
+	fmu mode_pwm
+	param set BAT_V_SCALING 0.004593
+	set EXIT_ON_END yes
+fi
+ 
+#
+# 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 and position estimator
+#
+att_pos_estimator_ekf start
+
+#
+# Load mixer and start controllers (depends on px4io)
+#
+if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
+then
+	echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
+	mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
+else
+	echo "Using /etc/mixers/FMU_Q.mix"
+	mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
+fi
+fw_att_control start
+fw_pos_control_l1 start
+
+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 4b7ed5286..fb9dec043 100644
--- a/ROMFS/px4fmu_common/init.d/40_io_segway
+++ b/ROMFS/px4fmu_common/init.d/40_io_segway
@@ -52,5 +52,5 @@ attitude_estimator_ekf start
 #
 # Load mixer and start controllers (depends on px4io)
 #
-md25 start 3 0x58
+roboclaw start /dev/ttyS2 128 1200
 segway start
diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550
index ae41f2a01..eae37098b 100644
--- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550
+++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550
@@ -53,7 +53,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
 #
 # Set PWM output frequency
 #
-pwm -u 400 -m 0xff
+pwm rate -c 1234 -r 400
 
 #
 # Start common for all multirotors apps
diff --git a/ROMFS/px4fmu_common/init.d/cmp_test b/ROMFS/px4fmu_common/init.d/cmp_test
new file mode 100644
index 000000000..f86f4f85b
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/cmp_test
@@ -0,0 +1,9 @@
+#!nsh
+
+cp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded
+if cmp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded
+then
+	echo "CMP returned true"
+else
+	echo "CMP returned false"
+fi
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl
new file mode 100644
index 000000000..a63d0e5f1
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl
@@ -0,0 +1,120 @@
+#!nsh
+
+#
+# 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.002
+	param set MC_ATTRATE_I 0.0
+	param set MC_ATTRATE_P 0.09
+	param set MC_ATT_D 0.0
+	param set MC_ATT_I 0.0
+	param set MC_ATT_P 6.8
+	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     2 = quadrotor
+#
+param set MAV_TYPE 2
+
+set EXIT_ON_END no
+
+#
+# Start the Mikrokopter ESC driver
+#
+if [ $MKBLCTRL_MODE == yes ]
+then
+    if [ $MKBLCTRL_FRAME == x ]
+    then
+        echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing"
+        mkblctrl -mkmode x
+    else
+        echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing"
+        mkblctrl -mkmode +
+    fi
+else
+    if [ $MKBLCTRL_FRAME == x ]
+    then
+        echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame"
+    else
+        echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame"
+    fi
+    mkblctrl
+fi
+
+usleep 10000
+
+#
+# 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
+	fmu mode_pwm
+	# Start MAVLink (on UART1 / ttyS0)
+	mavlink start -d /dev/ttyS0
+	usleep 5000
+	param set BAT_V_SCALING 0.004593
+	set EXIT_ON_END yes
+fi
+
+#
+# Load mixer
+#
+if [ $MKBLCTRL_FRAME == x ]
+then
+    mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
+else
+    mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
+fi
+
+#
+# Set disarmed, min and max PWM signals
+#
+pwm disarmed -c 1234 -p 900
+pwm min -c 1234 -p 1200
+pwm max -c 1234 -p 1800
+
+#
+# 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/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc
new file mode 100644
index 000000000..0c0cfa53d
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc
@@ -0,0 +1,122 @@
+#!nsh
+
+#
+# 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.002
+	param set MC_ATTRATE_I 0.0
+	param set MC_ATTRATE_P 0.09
+	param set MC_ATT_D 0.0
+	param set MC_ATT_I 0.0
+	param set MC_ATT_P 6.8
+	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
+
+echo "RC script for PX4FMU + PX4IO + PPM-ESCs running"
+ 
+#
+# Force some key parameters to sane values
+# MAV_TYPE     2 = quadrotor
+#
+param set MAV_TYPE 2
+
+set EXIT_ON_END no
+
+usleep 10000
+
+#
+# Start and configure PX4IO or FMU interface
+#
+if px4io detect
+then
+	# Start MAVLink (depends on orb)
+	mavlink start -d /dev/ttyS1 -b 57600
+	usleep 5000
+
+	commander start
+
+	sh /etc/init.d/rc.io
+else
+	fmu mode_pwm
+	# Start MAVLink (on UART1 / ttyS0)
+	mavlink start -d /dev/ttyS1 -b 57600
+	usleep 5000
+	param set BAT_V_SCALING 0.004593
+	set EXIT_ON_END yes
+fi
+
+if [ $ESC_MAKER = afro ]
+then
+	# Set PWM values for Afro ESCs
+	pwm disarmed -c 1234 -p 1050
+	pwm min -c 1234 -p 1080
+	pwm max -c 1234 -p 1860
+else
+	# Set PWM values for typical ESCs
+	pwm disarmed -c 1234 -p 900
+	pwm min -c 1234 -p 980
+	pwm max -c 1234 -p 1800
+fi
+
+#
+# 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
+
+#
+# Set PWM output frequency
+#
+pwm rate -r 400 -c 1234
+
+#
+# Start common for all multirotors apps
+#
+sh /etc/init.d/rc.multirotor
+
+if [ $EXIT_ON_END == yes ]
+then
+	exit
+fi
+
+echo "Script end"
diff --git a/ROMFS/px4fmu_common/init.d/rc.hil b/ROMFS/px4fmu_common/init.d/rc.hil
deleted file mode 100644
index eccb2b767..000000000
--- a/ROMFS/px4fmu_common/init.d/rc.hil
+++ /dev/null
@@ -1,56 +0,0 @@
-#!nsh
-#
-# USB HIL start
-#
-
-echo "[HIL] starting.."
-
-uorb start
-
-# Tell MAVLink that this link is "fast"
-mavlink start -b 230400 -d /dev/ttyS1
-
-# 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 1
-
-#
-# Start the commander (depends on orb, mavlink)
-#
-commander start
-
-#
-# Check if we got an IO
-#
-if px4io start
-then
-	echo "IO started"
-else
-	fmu mode_serial
-	echo "FMU started"
-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_AET.mix
-fw_pos_control_l1 start
-fw_att_control start
-
-echo "[HIL] setup done, running"
diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor
index e3638e3d1..dfff07198 100644
--- a/ROMFS/px4fmu_common/init.d/rc.multirotor
+++ b/ROMFS/px4fmu_common/init.d/rc.multirotor
@@ -14,11 +14,6 @@ sh /etc/init.d/rc.sensors
 sh /etc/init.d/rc.logging
 
 #
-# Start the commander.
-#
-commander start
-
-#
 # Start GPS interface (depends on orb)
 #
 gps start
diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 2828a108b..f17b650bc 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -27,6 +27,22 @@ else
 	set BOARD fmuv2
 fi
 
+# Start airspeed sensors
+if meas_airspeed start
+then
+	echo "using MEAS airspeed sensor"
+else
+	if ets_airspeed start
+	then
+		echo "using ETS airspeed sensor (bus 3)"
+	else
+		if ets_airspeed start -b 1
+		then
+			echo "Using ETS airspeed sensor (bus 1)"
+		fi
+	fi
+fi
+
 #
 # Start the sensor collection task.
 # IMPORTANT: this also loads param offsets
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index 9264985d6..ccf2cd47e 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -21,7 +21,6 @@ if mavlink stop
 then
 	echo "stopped other MAVLink instance"
 fi
-sleep 2
 mavlink start -b 230400 -d /dev/ttyACM0
 
 # Stop commander
@@ -37,13 +36,6 @@ then
 	echo "Commander started"
 fi
 
-# Stop px4io
-if px4io stop
-then
-	echo "PX4IO stopped"
-fi
-sleep 1
-
 # Start px4io if present
 if px4io start
 then
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 32fb67a45..cff8446a6 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -73,20 +73,25 @@ then
 	#
 	# Load microSD params
 	#
-	if ramtron start
-	then
-		param select /ramtron/params
-		if [ -f /ramtron/params ]
-		then
-			param load /ramtron/params
-		fi
-	else
+	#if ramtron start
+	#then
+	#	param select /ramtron/params
+	#	if [ -f /ramtron/params ]
+	#	then
+	#		param load /ramtron/params
+	#	fi
+	#else
 		param select /fs/microsd/params
 		if [ -f /fs/microsd/params ]
 		then
-			param load /fs/microsd/params
+			if param load /fs/microsd/params
+			then
+				echo "Parameters loaded"
+			else
+				echo "Parameter file corrupt - ignoring"
+			fi
 		fi
-	fi
+	#fi
 	
 	#
 	# Start system state indicator
@@ -101,8 +106,42 @@ then
 		fi
 	fi
 
-	# Try to get an USB console
-	nshterm /dev/ttyACM0 &
+	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
+	
+	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 != custom ]
+	then
+		# Try to get an USB console
+		nshterm /dev/ttyACM0 &
+	fi
+
 	
 	#
 	# Upgrade PX4IO firmware
@@ -154,6 +193,20 @@ then
 		sh /etc/init.d/11_dji_f450
 		set MODE custom
 	fi
+
+	if param compare SYS_AUTOSTART 12
+	then
+		set MIXER /etc/mixers/FMU_hex_x.mix
+		sh /etc/init.d/12-13_hex
+		set MODE custom
+	fi
+
+	if param compare SYS_AUTOSTART 13
+	then
+		set MIXER /etc/mixers/FMU_hex_+.mix
+		sh /etc/init.d/12-13_hex
+		set MODE custom
+	fi
 	
 	if param compare SYS_AUTOSTART 15
 	then
@@ -166,19 +219,96 @@ then
 		sh /etc/init.d/16_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
+	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 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 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 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 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 22
+	then
+		set FRAME_GEOMETRY w
+		sh /etc/init.d/rc.custom_io_esc
+		set MODE custom
+	fi
 	
 	if param compare SYS_AUTOSTART 30
 	then
 		sh /etc/init.d/30_io_camflyer
 		set MODE custom
 	fi
-	
+
 	if param compare SYS_AUTOSTART 31
 	then
 		sh /etc/init.d/31_io_phantom
 		set MODE custom
 	fi
 	
+	if param compare SYS_AUTOSTART 32
+	then
+		sh /etc/init.d/32_skywalker_x5
+		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 100
+	then
+		sh /etc/init.d/100_mpx_easystar
+		set MODE custom
+	fi
+
+	if param compare SYS_AUTOSTART 101
+	then
+		sh /etc/init.d/101_hk_bixler
+		set MODE custom
+	fi
+	
 	# Start any custom extensions that might be missing
 	if [ -f /fs/microsd/etc/rc.local ]
 	then
@@ -189,8 +319,9 @@ then
 	if [ $MODE == autostart ]
 	then
 		# Telemetry port is on both FMU boards ttyS1
-		mavlink start -b 57600 -d /dev/ttyS1
-		usleep 5000
+		# but the AR.Drone motors can be get 'flashed'
+		# if starting MAVLink on them - so do not
+		# start it as default (default link: USB)
 	
 		# Start commander
 		commander start
diff --git a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix
index 75e82bb00..0ec663e35 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix
@@ -62,3 +62,23 @@ range.  Inputs below zero are treated as zero.
 M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
+
+
+Gimbal / flaps / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_AET.mix b/ROMFS/px4fmu_common/mixers/FMU_AET.mix
index 20cb88b91..c73cb2a62 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_AET.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_AET.mix
@@ -58,3 +58,23 @@ range.  Inputs below zero are treated as zero.
 M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
+
+
+Gimbal / flaps / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_Q.mix b/ROMFS/px4fmu_common/mixers/FMU_Q.mix
index ebcb66b24..17ff71151 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_Q.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_Q.mix
@@ -50,3 +50,21 @@ M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
 
+Gimbal / flaps / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_RET.mix b/ROMFS/px4fmu_common/mixers/FMU_RET.mix
index 95beb8927..f07c34ac8 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_RET.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_RET.mix
@@ -51,3 +51,23 @@ range.  Inputs below zero are treated as zero.
 M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
+
+
+Gimbal / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_X5.mix b/ROMFS/px4fmu_common/mixers/FMU_X5.mix
index 9f81e1dc3..610868354 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_X5.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_X5.mix
@@ -48,3 +48,22 @@ M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
 
+Gimbal / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
+
diff --git a/ROMFS/px4fmu_common/mixers/FMU_delta.mix b/ROMFS/px4fmu_common/mixers/FMU_delta.mix
index 981466704..f0aa6650d 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_delta.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_delta.mix
@@ -48,3 +48,23 @@ M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
 
+
+Gimbal / flaps / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
+
diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix
index b5e38ce9e..f8f9f0e4d 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix
@@ -5,3 +5,14 @@ This file defines a single mixer for a hexacopter in the + configuration. All co
 are mixed 100%.
 
 R: 6+ 10000 10000 10000 0
+
+Gimbal / payload mixer for last two channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix
index 8e8d122ad..26b40b9e9 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix
@@ -5,3 +5,14 @@ This file defines a single mixer for a hexacopter in the X configuration.  All c
 are mixed 100%.
 
 R: 6x 10000 10000 10000 0
+
+Gimbal / payload mixer for last two channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix
index dfdf1d58e..cd9a9cfab 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix
@@ -5,3 +5,23 @@ This file defines a single mixer for a quadrotor in the + configuration. All con
 are mixed 100%.
 
 R: 4+ 10000 10000 10000 0
+
+
+Gimbal / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix
index 2a4a0f341..520aba635 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix
@@ -5,3 +5,22 @@ This file defines a single mixer for a quadrotor in the V configuration.  All co
 are mixed 100%.
 
 R: 4v 10000 10000 10000 0
+
+Gimbal / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix
index 81b4af30b..58e6af74b 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix
@@ -4,3 +4,22 @@ Multirotor mixer for PX4FMU
 This file defines a single mixer for a quadrotor with a wide configuration.  All controls are mixed 100%.
 
 R: 4w 10000 10000 10000 0
+
+Gimbal / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix
index 12a3bee20..fa21c8012 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix
@@ -5,3 +5,22 @@ This file defines a single mixer for a quadrotor in the X configuration.  All co
 are mixed 100%.
 
 R: 4x 10000 10000 10000 0
+
+Gimbal / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000