diff --git a/mk/PX4/ROMFS/init.d/rc.APM b/mk/PX4/ROMFS/init.d/rc.APM
index c8500228db..7e29e66b4c 100644
--- a/mk/PX4/ROMFS/init.d/rc.APM
+++ b/mk/PX4/ROMFS/init.d/rc.APM
@@ -4,6 +4,8 @@
 
 # To disable APM startup add a /fs/microsd/APM/nostart file
 # To enable mkblctrl startup add a /fs/microsd/APM/mkblctrl file
+# To enable mkblctrl_+ startup add a /fs/microsd/APM/mkblctrl_+ file
+# To enable mkblctrl_x startup add a /fs/microsd/APM/mkblctrl_x file
 # To enable PWM on FMUv1 on ttyS1 add a /fs/microsd/APM/AUXPWM.en file
 
 set deviceA /dev/ttyACM0
@@ -84,6 +86,28 @@ else
     sh /etc/init.d/rc.error
 fi
 
+# start mkblctrl driver if configured
+if [ -f /fs/microsd/APM/mkblctrl ]
+then
+    echo "Setting up mkblctrl driver"
+    echo "Setting up mkblctrl driver" >> $logfile
+    mkblctrl -d /dev/pwm_output
+fi
+
+if [ -f /fs/microsd/APM/mkblctrl_+ ]
+then
+    echo "Setting up mkblctrl driver +"
+    echo "Setting up mkblctrl driver +" >> $logfile
+    mkblctrl -mkmode + -d /dev/pwm_output
+fi
+
+if [ -f /fs/microsd/APM/mkblctrl_x ]
+then
+    echo "Setting up mkblctrl driver x"
+    echo "Setting up mkblctrl driver x" >> $logfile
+    mkblctrl -mkmode x -d /dev/pwm_output
+fi
+
 
 # try the px4io start twice. Some FMUv2 board don't
 # come up the first time
@@ -150,15 +174,6 @@ else
     then
        sh /etc/init.d/rc.error
     fi
-
-    if [ -f /fs/microsd/APM/mkblctrl ]
-    then
-   	echo "APM/mkblctrl found - switch to MK I2C ESCs"
-   	echo "APM/mkblctrl found - switch to MK I2C ESCs" >> $logfile
-	echo "Setting up mkblctrl driver"
-	echo "Setting up mkblctrl driver" >> $logfile
-	mkblctrl -mkmode x
-    fi
 fi
 
 if [ $BOARD == FMUv1 -a $deviceD == /dev/ttyS1 ]
diff --git a/mk/PX4/config_px4fmu-v2_APM.mk b/mk/PX4/config_px4fmu-v2_APM.mk
index 649dd7a27c..b186035b66 100644
--- a/mk/PX4/config_px4fmu-v2_APM.mk
+++ b/mk/PX4/config_px4fmu-v2_APM.mk
@@ -35,9 +35,7 @@ MODULES		+= drivers/blinkm
 MODULES		+= drivers/airspeed
 MODULES		+= drivers/ets_airspeed
 MODULES		+= drivers/meas_airspeed
-
-# mkblctrl does not compile on FMUv2 yet
-# MODULES		+= drivers/mkblctrl
+MODULES		+= drivers/mkblctrl
 
 #
 # System commands