diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface
index 4f9bfdeef3..2c3cdb0330 100644
--- a/ROMFS/px4fmu_common/init.d/rc.interface
+++ b/ROMFS/px4fmu_common/init.d/rc.interface
@@ -36,6 +36,109 @@ then
 	set MIXER_AUX none
 fi
 
+#
+# Set the default output mode if none was set.
+#
+if [ $OUTPUT_MODE == none ]
+then
+	if [ $USE_IO == yes ]
+	then
+		# Enable IO output only if IO is present.
+		if [ $IO_PRESENT == yes ]
+		then
+			set OUTPUT_MODE io
+		fi
+	else
+		set OUTPUT_MODE fmu
+	fi
+fi
+
+#
+# If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output.
+#
+if [ $OUTPUT_MODE != none ]
+then
+	if [ $OUTPUT_MODE == mkblctrl ]
+	then
+		if [ $MKBLCTRL_MODE == x ]
+		then
+			set MKBLCTRL_ARG "-mkmode x"
+		fi
+		if [ $MKBLCTRL_MODE == + ]
+		then
+			set MKBLCTRL_ARG "-mkmode +"
+		fi
+
+		if mkblctrl $MKBLCTRL_ARG
+		then
+		else
+			# Error tune.
+			tune_control play -t 2
+		fi
+	fi
+
+	if [ $OUTPUT_MODE == hil ]
+	then
+		if pwm_out_sim start
+		then
+		else
+			# Error tune.
+			tune_control play -t 2
+		fi
+	fi
+
+	if [ $OUTPUT_MODE == tap_esc ]
+	then
+		set FMU_MODE rcin
+	fi
+
+	if [ $OUTPUT_MODE == fmu ]
+	then
+		if fmu mode_$FMU_MODE $FMU_ARGS
+		then
+		else
+			echo "FMU start failed" >> $LOG_FILE
+			# Error tune.
+			tune_control play -t 2
+		fi
+	fi
+
+	if [ $OUTPUT_MODE == uavcan_esc ]
+	then
+		if param compare UAVCAN_ENABLE 0
+		then
+			param set UAVCAN_ENABLE 3
+		fi
+	fi
+
+	if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ]
+	then
+		sh /etc/init.d/rc.io
+	fi
+
+	#
+	# Start IO or FMU for RC PPM input if needed.
+	#
+	if [ $IO_PRESENT == yes ]
+	then
+		if [ $OUTPUT_MODE != io ]
+		then
+			sh /etc/init.d/rc.io
+		fi
+	else
+		if [ $OUTPUT_MODE != fmu ]
+		then
+			if fmu mode_${FMU_MODE} $FMU_ARGS
+			then
+			else
+				echo "FMU mode_${FMU_MODE} start failed" >> $LOG_FILE
+				# Error tune.
+				tune_control play -t 2
+			fi
+		fi
+	fi
+fi
+
 if [ $MIXER != none -a $MIXER != skip ]
 then
 	#
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 50c5247dd6..aed84aef79 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -468,107 +468,6 @@ else
 		fi
 	fi
 
-	#
-	# Set default output if not set.
-	#
-	if [ $OUTPUT_MODE == none ]
-	then
-		if [ $USE_IO == yes ]
-		then
-			# Enable IO output only if IO is present.
-			if [ $IO_PRESENT == yes ]
-			then
-				set OUTPUT_MODE io
-			fi
-		else
-			set OUTPUT_MODE fmu
-		fi
-	fi
-
-	# If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output.
-	if [ $OUTPUT_MODE != none ]
-	then
-		if [ $OUTPUT_MODE == uavcan_esc ]
-		then
-			if param compare UAVCAN_ENABLE 0
-			then
-				param set UAVCAN_ENABLE 3
-			fi
-		fi
-
-		if [ $OUTPUT_MODE == mkblctrl ]
-		then
-			if [ $MKBLCTRL_MODE == x ]
-			then
-				set MKBLCTRL_ARG "-mkmode x"
-			fi
-			if [ $MKBLCTRL_MODE == + ]
-			then
-				set MKBLCTRL_ARG "-mkmode +"
-			fi
-
-			if mkblctrl $MKBLCTRL_ARG
-			then
-			else
-				# Error tune.
-				tune_control play -t 2
-			fi
-		fi
-
-		if [ $OUTPUT_MODE == hil ]
-		then
-			if pwm_out_sim start
-			then
-			else
-				# Error tune.
-				tune_control play -t 2
-			fi
-		fi
-
-		if [ $OUTPUT_MODE == tap_esc ]
-		then
-			set FMU_MODE rcin
-		fi
-
-		if [ $OUTPUT_MODE == fmu ]
-		then
-			if fmu mode_$FMU_MODE $FMU_ARGS
-			then
-			else
-				echo "FMU start failed" >> $LOG_FILE
-				# Error tune.
-				tune_control play -t 2
-			fi
-		fi
-
-		if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ]
-		then
-			sh /etc/init.d/rc.io
-		fi
-
-		#
-		# Start IO or FMU for RC PPM input if needed.
-		#
-		if [ $IO_PRESENT == yes ]
-		then
-			if [ $OUTPUT_MODE != io ]
-			then
-				sh /etc/init.d/rc.io
-			fi
-		else
-			if [ $OUTPUT_MODE != fmu ]
-			then
-				if fmu mode_${FMU_MODE} $FMU_ARGS
-				then
-				else
-					echo "FMU mode_${FMU_MODE} start failed" >> $LOG_FILE
-					# Error tune.
-					tune_control play -t 2
-				fi
-			fi
-		fi
-	fi
-
 	#
 	# Start mavlink streams.
 	#