diff --git a/flight/PiOS/STM32F4xx/pios_mpu6050.c b/flight/PiOS/STM32F4xx/pios_mpu6050.c
index bb73e5c16..935e57f74 100644
--- a/flight/PiOS/STM32F4xx/pios_mpu6050.c
+++ b/flight/PiOS/STM32F4xx/pios_mpu6050.c
@@ -82,6 +82,8 @@ void PIOS_MPU6050_Init(const struct pios_mpu6050_cfg * new_cfg)
  * \param[in] PIOS_MPU6050_ConfigTypeDef struct to be used to configure sensor.
 *
 */
+uint8_t reg_val;
+
 static void PIOS_MPU6050_Config(struct pios_mpu6050_cfg const * cfg)
 {
 	mpu6050_first_read = true;
@@ -110,8 +112,13 @@ static void PIOS_MPU6050_Config(struct pios_mpu6050_cfg const * cfg)
 	while (PIOS_MPU6050_Write(PIOS_MPU6050_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0) ;
 	
 	// Interrupt configuration
-	while (PIOS_MPU6050_Write(PIOS_MPU6050_INT_CFG_REG, cfg->Interrupt_cfg) != 0) ;
+	while (PIOS_MPU6050_Write(PIOS_MPU6050_INT_CFG_REG, cfg->interrupt_cfg) != 0) ;
+
+	// Interrupt configuration
+	while (PIOS_MPU6050_Write(PIOS_MPU6050_INT_EN_REG, cfg->interrupt_en) != 0) ;
+
 	
+	PIOS_MPU6050_Read(PIOS_MPU6050_INT_CFG_REG, &reg_val, 1);
 	mpu6050_configured = true;
 }
 
@@ -337,6 +344,7 @@ void PIOS_MPU6050_IRQHandler(void)
 	if(!mpu6050_configured)
 		return;
 		
+	return;
 	//PIOS_Assert(MPU6050_cb_ready);
 	if(!mpu6050_cb_ready) {
 		PIOS_LED_Toggle(LED2);
diff --git a/flight/PiOS/inc/pios_mpu6050.h b/flight/PiOS/inc/pios_mpu6050.h
index 1f5700e49..7df340017 100644
--- a/flight/PiOS/inc/pios_mpu6050.h
+++ b/flight/PiOS/inc/pios_mpu6050.h
@@ -43,6 +43,7 @@
 #define PIOS_MPU6050_ACCEL_CFG_REG        0X1C
 #define PIOS_MPU6050_FIFO_EN_REG          0x23
 #define PIOS_MPU6050_INT_CFG_REG          0x37
+#define PIOS_MPU6050_INT_EN_REG           0x38
 #define PIOS_MPU6050_INT_STATUS_REG       0x3A
 #define PIOS_MPU6050_ACCEL_X_OUT_MSB      0x3B
 #define PIOS_MPU6050_ACCEL_X_OUT_LSB      0x3C
@@ -77,8 +78,9 @@
 #define PIOS_MPU6050_INT_OPEN             0x40
 #define PIOS_MPU6050_INT_LATCH_EN         0x20
 #define PIOS_MPU6050_INT_CLR_ANYRD        0x10
-#define PIOS_MPU6050_INT_IMU_RDY          0x04
-#define PIOS_MPU6050_INT_DATA_RDY         0x01
+
+#define PIOS_MPU6050_INTEN_OVERFLOW       0x10
+#define PIOS_MPU6050_INTEN_DATA_RDY       0x01
 
 /* Interrupt status */
 #define PIOS_MPU6050_INT_STATUS_FIFO_FULL 0x80
@@ -129,7 +131,8 @@ struct pios_mpu6050_cfg {
 	
 	uint8_t Fifo_store;		/* FIFO storage of different readings (See datasheet page 31 for more details) */
 	uint8_t Smpl_rate_div;	/* Sample rate divider to use (See datasheet page 32 for more details) */
-	uint8_t Interrupt_cfg;	/* Interrupt configuration (See datasheet page 35 for more details) */
+	uint8_t interrupt_cfg;	/* Interrupt configuration (See datasheet page 35 for more details) */
+	uint8_t interrupt_en;	/* Interrupt configuration (See datasheet page 35 for more details) */
 	uint8_t User_ctl;		/* User control settings (See datasheet page 41 for more details)  */
 	uint8_t Pwr_mgmt_clk;	/* Power management and clock selection (See datasheet page 32 for more details) */
 	enum pios_mpu6050_range gyro_range;
diff --git a/flight/Revolution/System/pios_board.c b/flight/Revolution/System/pios_board.c
index 00401920d..07b3f70a1 100644
--- a/flight/Revolution/System/pios_board.c
+++ b/flight/Revolution/System/pios_board.c
@@ -726,7 +726,8 @@ static const struct pios_mpu6050_cfg pios_mpu6050_cfg = {
 	.Fifo_store = PIOS_MPU6050_FIFO_TEMP_OUT | PIOS_MPU6050_FIFO_GYRO_X_OUT | PIOS_MPU6050_FIFO_GYRO_Y_OUT | PIOS_MPU6050_FIFO_GYRO_Z_OUT,
 	// Clock at 8 khz, downsampled by 8 for 1khz
 	.Smpl_rate_div = 7, 
-	.Interrupt_cfg = PIOS_MPU6050_INT_DATA_RDY | PIOS_MPU6050_INT_CLR_ANYRD,
+	.interrupt_cfg = PIOS_MPU6050_INT_CLR_ANYRD,
+	.interrupt_en = PIOS_MPU6050_INTEN_DATA_RDY,
 	.User_ctl = PIOS_MPU6050_USERCTL_FIFO_EN,
 	.Pwr_mgmt_clk = PIOS_MPU6050_PWRMGMT_PLL_X_CLK,
 	.gyro_range = PIOS_MPU6050_SCALE_500_DEG,