diff --git a/mk/PX4/ROMFS/init.d/rc.APM b/mk/PX4/ROMFS/init.d/rc.APM
index 7f82901141..f3b9d56d31 100644
--- a/mk/PX4/ROMFS/init.d/rc.APM
+++ b/mk/PX4/ROMFS/init.d/rc.APM
@@ -196,17 +196,33 @@ else
     then
         echo "Using external magnetometer"
     fi
-    if l3gd20 start
+    set HAVE_MPU6000 false          
+    if [ ! -f /fs/microsd/APM/LSM303D.en ]
     then
-       echo "l3gd20 started OK"
-    else
-       sh /etc/init.d/rc.error
-    fi
-    if lsm303d start
+        if mpu6000 start
+        then
+          set HAVE_MPU6000 true
+          echo "Found MPU6000"
+          echo "Found MPU6000" >> $logfile
+        else
+          echo "No MPU6000"
+          echo "No MPU6000" >> $logfile
+        fi
+    fi       
+    if [ $HAVE_MPU6000 == false ]
     then
-       echo "lsm303d started OK"
-    else
-       sh /etc/init.d/rc.error
+        if l3gd20 start
+        then
+           echo "l3gd20 started OK"
+        else
+           sh /etc/init.d/rc.error
+        fi
+        if lsm303d start
+        then
+           echo "lsm303d started OK"
+        else
+           sh /etc/init.d/rc.error
+        fi
     fi
 fi
 
diff --git a/mk/PX4/config_px4fmu-v2_APM.mk b/mk/PX4/config_px4fmu-v2_APM.mk
index f0005a87df..3d4b52fd1c 100644
--- a/mk/PX4/config_px4fmu-v2_APM.mk
+++ b/mk/PX4/config_px4fmu-v2_APM.mk
@@ -23,7 +23,7 @@ MODULES		+= drivers/boards/px4fmu-v2
 MODULES		+= drivers/rgbled
 MODULES		+= drivers/lsm303d
 MODULES		+= drivers/l3gd20
-#MODULES		+= drivers/mpu6000
+MODULES		+= drivers/mpu6000
 MODULES		+= drivers/hmc5883
 MODULES		+= drivers/ms5611
 MODULES		+= drivers/mb12xx