diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c
index 57f9d3a810..d75dfab1aa 100644
--- a/src/main/drivers/accgyro_mpu6050.c
+++ b/src/main/drivers/accgyro_mpu6050.c
@@ -225,6 +225,13 @@ void configureMPUDataReadyInterruptHandling(void)
     gpioExtiLineConfig(mpu6050Config->exti_port_source, mpu6050Config->exti_pin_source);
 #endif
 
+#ifdef ENSURE_MPU_DATA_READY_IS_LOW
+    uint8_t status = GPIO_ReadInputDataBit(mpu6050Config->gpioPort, mpu6050Config->gpioPin);
+    if (status) {
+        return;
+    }
+#endif
+
     registerExti15_10_CallbackHandler(MPU_DATA_READY_EXTI_Handler);
 
     EXTI_ClearITPendingBit(mpu6050Config->exti_line);
diff --git a/src/main/drivers/barometer_bmp085.c b/src/main/drivers/barometer_bmp085.c
index 6ebd4d8f74..20558e431b 100644
--- a/src/main/drivers/barometer_bmp085.c
+++ b/src/main/drivers/barometer_bmp085.c
@@ -209,6 +209,7 @@ bool bmp085Detect(const bmp085Config_t *config, baro_t *baro)
         return true;
     }
 
+    unregisterExti15_10_CallbackHandler(BMP085_EOC_EXTI_Handler);
     BMP085_OFF;
 
     return false;
diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c
index 7c6daf1128..346cdc0892 100644
--- a/src/main/drivers/system.c
+++ b/src/main/drivers/system.c
@@ -44,7 +44,19 @@ void registerExti15_10_CallbackHandler(extiCallbackHandler *fn)
         extiCallbackHandler *candidate = exti15_10_handlers[index];
         if (!candidate) {
             exti15_10_handlers[index] = fn;
-            break;
+            return;
+        }
+    }
+    failureMode(15); // EXTI15_10_CALLBACK_HANDLER_COUNT is too low for the amount of handlers required.
+}
+
+void unregisterExti15_10_CallbackHandler(extiCallbackHandler *fn)
+{
+    for (int index = 0; index < EXTI15_10_CALLBACK_HANDLER_COUNT; index++) {
+        extiCallbackHandler *candidate = exti15_10_handlers[index];
+        if (candidate == fn) {
+            exti15_10_handlers[index] = 0;
+            return;
         }
     }
 }
diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h
index 6c36b5d1c6..efc30a6fae 100644
--- a/src/main/drivers/system.h
+++ b/src/main/drivers/system.h
@@ -39,3 +39,4 @@ extern uint32_t hse_value;
 typedef void extiCallbackHandler(void);
 
 void registerExti15_10_CallbackHandler(extiCallbackHandler *fn);
+void unregisterExti15_10_CallbackHandler(extiCallbackHandler *fn);
diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index 9e31dd6e88..7eaeab2e49 100644
--- a/src/main/target/NAZE/target.h
+++ b/src/main/target/NAZE/target.h
@@ -29,7 +29,6 @@
 #define LED1_PIN    Pin_4 // PB4 (LED)
 #define LED1_PERIPHERAL RCC_APB2Periph_GPIOB
 
-
 #define BEEP_GPIO   GPIOA
 #define BEEP_PIN    Pin_12 // PA12 (Beeper)
 #define BEEP_PERIPHERAL RCC_APB2Periph_GPIOA
@@ -71,6 +70,7 @@
 
 #define USE_FLASH_M25P16
 
+#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU data ready and BMP085 EOC
 #define USE_MPU_DATA_READY_SIGNAL
 
 #define GYRO