diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io
index a729fdc313..54948b697d 100644
--- a/ROMFS/px4fmu_common/init.d/rc.io
+++ b/ROMFS/px4fmu_common/init.d/rc.io
@@ -1,17 +1,16 @@
+#!nsh
 #
-# Init PX4IO interface
+# PX4IO interface init script.
 #
 
-#
-# Allow PX4IO to recover from midair restarts.
-#
-px4io recovery
-
-#
-# Adjust PX4IO update rate limit
-#
-set PX4IO_LIMIT 400
-
-if px4io limit ${PX4IO_LIMIT}
+if px4io start
 then
+	# Allow PX4IO to recover from midair restarts.
+	px4io recovery
+
+	# Adjust PX4IO update rate limit.
+	px4io limit 400
+else
+	echo "PX4IO start failed" >> $LOG_FILE
+	tune_control play -m ${TUNE_ERR}
 fi
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 2e8bf71148..fe4f17d849 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -470,27 +470,6 @@ else
 			fi
 		fi
 
-		if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ]
-		then
-			if px4io start
-			then
-				sh /etc/init.d/rc.io
-			else
-				echo "PX4IO start failed" >> $LOG_FILE
-				tune_control play -m  ${TUNE_ERR}
-			fi
-		fi
-
-		if [ $OUTPUT_MODE == fmu ]
-		then
-			if fmu mode_$FMU_MODE $FMU_ARGS
-			then
-			else
-				echo "FMU start failed" >> $LOG_FILE
-				tune_control play -m ${TUNE_ERR}
-			fi
-		fi
-
 		if [ $OUTPUT_MODE == mkblctrl ]
 		then
 			if [ $MKBLCTRL_MODE == x ]
@@ -523,6 +502,21 @@ else
 			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
+				tune_control play -m ${TUNE_ERR}
+			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.
 		#
@@ -530,13 +524,7 @@ else
 		then
 			if [ $OUTPUT_MODE != io ]
 			then
-				if px4io start
-				then
-					sh /etc/init.d/rc.io
-				else
-					echo "PX4IO start failed" >> $LOG_FILE
-					tune_control play -m ${TUNE_ERR}
-				fi
+				sh /etc/init.d/rc.io
 			fi
 		else
 			if [ $OUTPUT_MODE != fmu ]