diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index f17b650bc2..7f73acf859 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -19,12 +19,18 @@ fi
 if mpu6000 start
 then
 	echo "using MPU6000"
-	set BOARD fmuv1
-else
-	echo "using L3GD20 and LSM303D"
-	l3gd20 start
-	lsm303d start
+fi
+
+if l3gd20 start
+then
+	echo "using L3GD20(H)"		
+fi
+
+if lsm303d start
+then
 	set BOARD fmuv2
+else
+	set BOARD fmuv1
 fi
 
 # Start airspeed sensors
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index 015a7387a6..68673c0552 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -21,6 +21,7 @@ MODULES		+= drivers/px4fmu
 MODULES		+= drivers/px4io
 MODULES		+= drivers/boards/px4fmu-v2
 MODULES		+= drivers/rgbled
+MODULES		+= drivers/mpu6000
 MODULES		+= drivers/lsm303d
 MODULES		+= drivers/l3gd20
 MODULES		+= drivers/hmc5883