diff --git a/flight/PiOS/Common/pios_mpu6000.c b/flight/PiOS/Common/pios_mpu6000.c
index 56e8cccbd..f1268e83f 100644
--- a/flight/PiOS/Common/pios_mpu6000.c
+++ b/flight/PiOS/Common/pios_mpu6000.c
@@ -134,12 +134,16 @@ int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios
 */
 static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg)
 {
+
+	PIOS_MPU6000_Test();
+	
 	// Reset chip
 	while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, 0x80) != 0);
-	PIOS_DELAY_WaitmS(100);
+	PIOS_DELAY_WaitmS(300);
 	
 	// Reset chip and fifo
-	while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, 0x01 | 0x02 | 0x04) != 0);
+	while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, 0x80 | 0x01 | 0x02 | 0x04) != 0);
+	
 	// Wait for reset to finish
 	while (PIOS_MPU6000_GetReg(PIOS_MPU6000_USER_CTRL_REG) & 0x07);
 	
@@ -179,7 +183,7 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg)
 	
 	// Interrupt configuration
 	while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_CFG_REG, cfg->interrupt_cfg) != 0) ;
-	
+
 	// Interrupt configuration
 	while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0) ;
 	if((PIOS_MPU6000_GetReg(PIOS_MPU6000_INT_EN_REG)) != cfg->interrupt_en)
diff --git a/flight/PiOS/inc/pios_mpu6000.h b/flight/PiOS/inc/pios_mpu6000.h
index 2a301bc4c..191b69ed5 100644
--- a/flight/PiOS/inc/pios_mpu6000.h
+++ b/flight/PiOS/inc/pios_mpu6000.h
@@ -87,6 +87,7 @@
 
 /* User control functionality */
 #define PIOS_MPU6000_USERCTL_FIFO_EN      0X40
+#define PIOS_MPU6000_USERCTL_DIS_I2C      0X10
 #define PIOS_MPU6000_USERCTL_FIFO_RST     0X02
 #define PIOS_MPU6000_USERCTL_GYRO_RST     0X01
 
diff --git a/flight/Revolution/System/pios_board.c b/flight/Revolution/System/pios_board.c
index 0d57da254..2eaf5bbb8 100644
--- a/flight/Revolution/System/pios_board.c
+++ b/flight/Revolution/System/pios_board.c
@@ -177,7 +177,7 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
 	.Smpl_rate_div = 7,
 	.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
 	.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
-	.User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN,
+	.User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C,
 	.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
 	.accel_range = PIOS_MPU6000_ACCEL_8G,
 	.gyro_range = PIOS_MPU6000_SCALE_500_DEG,