From 6d5b44df7af32d194448c57a99f2d0af02d89f27 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Thu, 16 Apr 2015 20:26:20 +0100
Subject: [PATCH] Fix boot failure on Flip32+ Deluxe Acro.

The problem was the MPU6050 EXTI handler was not registered due to baro
detection taking the only callback handler slot.  When the MPU6050 EXTI
was configured the interrupt flag was never cleared which results in the
CPU being starved.
---
 src/main/drivers/accgyro_mpu6050.c  |  7 +++++++
 src/main/drivers/barometer_bmp085.c |  1 +
 src/main/drivers/system.c           | 14 +++++++++++++-
 src/main/drivers/system.h           |  1 +
 src/main/target/NAZE/target.h       |  2 +-
 5 files changed, 23 insertions(+), 2 deletions(-)

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