SPRACING32/NAZE32 - Add support for MPU6050 data ready interrupt.

Currently the interrupt handler is unused.  Later it can be used as a
potential source for watchdog checking or to syncronize the system
around new acc/gyro data availability.

Verified on Naze32 rev 3/4/5 and SPRacingF3 targets.

src/main/drivers/accgyro_mpu6050.c

@@ -23,6 +23,8 @@
 
 #include "common/maths.h"
 
+#include "nvic.h"
+
 #include "system.h"
 #include "gpio.h"
 #include "bus_i2c.h"
@@ -31,13 +33,17 @@
 #include "accgyro.h"
 #include "accgyro_mpu6050.h"
 
+//#define DEBUG_MPU_DATA_READY_INTERRUPT
+
 // MPU6050, Standard address 0x68
-// MPU_INT on PB13 on rev4 hardware
+// MPU_INT on PB13 on rev4 Naze32 hardware
 #define MPU6050_ADDRESS         0x68
 
 #define DMP_MEM_START_ADDR 0x6E
 #define DMP_MEM_R_W 0x6F
 
+// RA = Register Address
+
 #define MPU_RA_XG_OFFS_TC       0x00    //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
 #define MPU_RA_YG_OFFS_TC       0x01    //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
 #define MPU_RA_ZG_OFFS_TC       0x02    //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
@@ -127,6 +133,9 @@
 #define MPU_RA_FIFO_R_W         0x74
 #define MPU_RA_WHO_AM_I         0x75
 
+// RF = Register Flag
+#define MPU_RF_DATA_RDY_EN (1 << 0)
+
 #define MPU6050_SMPLRT_DIV      0       // 8000Hz
 
 enum lpf_e {
@@ -175,9 +184,77 @@ static mpu6050Resolution_e mpuAccelTrim;
 
 static const mpu6050Config_t *mpu6050Config = NULL;
 
+void MPU_DATA_READY_EXTI_Handler(void)
+{
+    EXTI_ClearITPendingBit(mpu6050Config->exti_line);
+
+#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
+    // Measure the delta in micro seconds between calls to the interrupt handler
+    static uint32_t lastCalledAt = 0;
+    static int32_t callDelta = 0;
+
+    uint32_t now = micros();
+    callDelta = now - lastCalledAt;
+
+    UNUSED(callDelta);
+
+    lastCalledAt = now;
+#endif
+
+}
+
+void configureMPUDataReadyInterruptHandling(void)
+{
+#ifdef USE_MPU_DATA_READY_SIGNAL
+
+#ifdef STM32F10X
+    // enable AFIO for EXTI support
+    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
+#endif
+
+#ifdef STM32F303xC
+    /* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
+    RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
+#endif
+
+#ifdef STM32F10X
+    gpioExtiLineConfig(mpu6050Config->exti_port_source, mpu6050Config->exti_pin_source);
+#endif
+
+#ifdef STM32F303xC
+    gpioExtiLineConfig(mpu6050Config->exti_port_source, mpu6050Config->exti_pin_source);
+#endif
+
+    registerExti15_10_CallbackHandler(MPU_DATA_READY_EXTI_Handler);
+
+    EXTI_ClearITPendingBit(mpu6050Config->exti_line);
+
+    EXTI_InitTypeDef EXTIInit;
+    EXTIInit.EXTI_Line = mpu6050Config->exti_line;
+    EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
+    EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising;
+    EXTIInit.EXTI_LineCmd = ENABLE;
+    EXTI_Init(&EXTIInit);
+
+    NVIC_InitTypeDef NVIC_InitStructure;
+
+    NVIC_InitStructure.NVIC_IRQChannel = mpu6050Config->exti_irqn;
+    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_MPU_DATA_READY);
+    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_MPU_DATA_READY);
+    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+    NVIC_Init(&NVIC_InitStructure);
+#endif
+}
+
 void mpu6050GpioInit(void) {
     gpio_config_t gpio;
 
+    static bool mpu6050GpioInitDone = false;
+
+    if (mpu6050GpioInitDone || !mpu6050Config) {
+        return;
+    }
+
 #ifdef STM32F303
         if (mpu6050Config->gpioAHBPeripherals) {
             RCC_AHBPeriphClockCmd(mpu6050Config->gpioAHBPeripherals, ENABLE);
@@ -189,11 +266,14 @@ void mpu6050GpioInit(void) {
         }
 #endif
 
-
     gpio.pin = mpu6050Config->gpioPin;
     gpio.speed = Speed_2MHz;
     gpio.mode = Mode_IN_FLOATING;
     gpioInit(mpu6050Config->gpioPort, &gpio);
+
+    configureMPUDataReadyInterruptHandling();
+
+    mpu6050GpioInitDone = true;
 }
 
 static bool mpu6050Detect(void)
@@ -297,10 +377,7 @@ bool mpu6050GyroDetect(const mpu6050Config_t *configToUse, gyro_t *gyro, uint16_
 
 static void mpu6050AccInit(void)
 {
-    if (mpu6050Config) {
+    mpu6050GpioInit();
-        mpu6050GpioInit();
-        mpu6050Config = NULL; // avoid re-initialisation of GPIO;
-    }
 
     switch (mpuAccelTrim) {
         case MPU_6050_HALF_RESOLUTION:
@@ -327,10 +404,7 @@ static void mpu6050AccRead(int16_t *accData)
 
 static void mpu6050GyroInit(void)
 {
-    if (mpu6050Config) {
+    mpu6050GpioInit();
-        mpu6050GpioInit();
-        mpu6050Config = NULL; // avoid re-initialisation of GPIO;
-    }
 
     i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80);      //PWR_MGMT_1    -- DEVICE_RESET 1
     delay(100);
@@ -345,6 +419,10 @@ static void mpu6050GyroInit(void)
 
     i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG,
             0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG   -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
+
+#ifdef USE_MPU_DATA_READY_SIGNAL
+    i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
+#endif
 }
 
 static void mpu6050GyroRead(int16_t *gyroData)

src/main/drivers/accgyro_mpu6050.h

@@ -26,6 +26,11 @@ typedef struct mpu6050Config_s {
 #endif
     uint16_t gpioPin;
     GPIO_TypeDef *gpioPort;
+
+    uint8_t exti_port_source;
+    uint32_t exti_line;
+    uint8_t exti_pin_source;
+    IRQn_Type exti_irqn;
 } mpu6050Config_t;
 
 bool mpu6050AccDetect(const mpu6050Config_t *config,acc_t *acc);

src/main/drivers/barometer_bmp085.c

@@ -39,13 +39,13 @@ static bool isConversionComplete = false;
 static uint16_t bmp085ConversionOverrun = 0;
 
 // EXTI14 for BMP085 End of Conversion Interrupt
-void EXTI15_10_IRQHandler(void)
+void BMP085_EOC_EXTI_Handler(void) {
-{
     if (EXTI_GetITStatus(EXTI_Line14) == SET) {
         EXTI_ClearITPendingBit(EXTI_Line14);
         isConversionComplete = true;
     }
 }
+
 #endif
 
 typedef struct {
@@ -166,6 +166,8 @@ bool bmp085Detect(const bmp085Config_t *config, baro_t *baro)
         gpioInit(config->eocGpioPort, &gpio);
         BMP085_ON;
 
+        registerExti15_10_CallbackHandler(BMP085_EOC_EXTI_Handler);
+
         // EXTI interrupt for barometer EOC
         gpioExtiLineConfig(GPIO_PortSourceGPIOC, GPIO_PinSource14);
         EXTI_InitStructure.EXTI_Line = EXTI_Line14;

src/main/drivers/nvic.h

@@ -22,6 +22,7 @@
 #define NVIC_PRIO_USB                      NVIC_BUILD_PRIORITY(2, 0)
 #define NVIC_PRIO_USB_WUP                  NVIC_BUILD_PRIORITY(1, 0)
 #define NVIC_PRIO_SONAR_ECHO               NVIC_BUILD_PRIORITY(0x0f, 0x0f)
+#define NVIC_PRIO_MPU_DATA_READY           NVIC_BUILD_PRIORITY(0x0f, 0x0f)
 #define NVIC_PRIO_CALLBACK                 NVIC_BUILD_PRIORITY(0x0f, 0x0f)
 
 // utility macros to join/split priority

src/main/drivers/sonar_hcsr04.c

@@ -41,7 +41,7 @@ static uint32_t lastMeasurementAt;
 static volatile int32_t measurement = -1;
 static sonarHardware_t const *sonarHardware;
 
-void ECHO_EXTI_IRQHandler(void)
+static void ECHO_EXTI_IRQHandler(void)
 {
     static uint32_t timing_start;
     uint32_t timing_stop;

src/main/drivers/system.c

@@ -15,6 +15,7 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <string.h>
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdlib.h>
@@ -30,6 +31,35 @@
 
 #include "system.h"
 
+
+#ifndef EXTI15_10_CALLBACK_HANDLER_COUNT
+#define EXTI15_10_CALLBACK_HANDLER_COUNT 1
+#endif
+
+static extiCallbackHandler* exti15_10_handlers[EXTI15_10_CALLBACK_HANDLER_COUNT];
+
+void registerExti15_10_CallbackHandler(extiCallbackHandler *fn)
+{
+    for (int index = 0; index < EXTI15_10_CALLBACK_HANDLER_COUNT; index++) {
+        extiCallbackHandler *candidate = exti15_10_handlers[index];
+        if (!candidate) {
+            exti15_10_handlers[index] = fn;
+            break;
+        }
+    }
+}
+
+void EXTI15_10_IRQHandler(void)
+{
+    for (int index = 0; index < EXTI15_10_CALLBACK_HANDLER_COUNT; index++) {
+        extiCallbackHandler *fn = exti15_10_handlers[index];
+        if (!fn) {
+            continue;
+        }
+        fn();
+    }
+}
+
 // cycles per microsecond
 static uint32_t usTicks = 0;
 // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
@@ -96,6 +126,8 @@ void systemInit(void)
     // Init cycle counter
     cycleCounterInit();
 
+
+    memset(exti15_10_handlers, 0, sizeof(exti15_10_handlers));
     // SysTick
     SysTick_Config(SystemCoreClock / 1000);
 }
@@ -158,5 +190,3 @@ void failureMode(uint8_t mode)
 
     systemResetToBootloader();
 }
-
-

src/main/drivers/system.h

@@ -35,3 +35,7 @@ bool isMPUSoftReset(void);
 void enableGPIOPowerUsageAndNoiseReductions(void);
 // current crystal frequency - 8 or 12MHz
 extern uint32_t hse_value;
+
+typedef void extiCallbackHandler(void);
+
+void registerExti15_10_CallbackHandler(extiCallbackHandler *fn);

src/main/sensors/initialisation.c

@@ -76,19 +76,27 @@ uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NON
 const mpu6050Config_t *selectMPU6050Config(void)
 {
 #ifdef NAZE
-    // MPU_INT output on rev4/5 hardware (PB13, PC13)
+    // MPU_INT output on rev4 PB13
     static const mpu6050Config_t nazeRev4MPU6050Config = {
             .gpioAPB2Peripherals = RCC_APB2Periph_GPIOB,
             .gpioPort = GPIOB,
-            .gpioPin = Pin_13
+            .gpioPin = Pin_13,
+            .exti_port_source = GPIO_PortSourceGPIOB,
+            .exti_pin_source = GPIO_PinSource13,
+            .exti_line = EXTI_Line13,
+            .exti_irqn = EXTI15_10_IRQn
     };
+    // MPU_INT output on rev5 hardware PC13
     static const mpu6050Config_t nazeRev5MPU6050Config = {
             .gpioAPB2Peripherals = RCC_APB2Periph_GPIOC,
             .gpioPort = GPIOC,
-            .gpioPin = Pin_13
+            .gpioPin = Pin_13,
+            .exti_port_source = GPIO_PortSourceGPIOC,
+            .exti_pin_source = GPIO_PinSource13,
+            .exti_line = EXTI_Line13,
+            .exti_irqn = EXTI15_10_IRQn
     };
 
-
     if (hardwareRevision < NAZE32_REV5) {
         return &nazeRev4MPU6050Config;
     } else {
@@ -100,10 +108,15 @@ const mpu6050Config_t *selectMPU6050Config(void)
     static const mpu6050Config_t spRacingF3MPU6050Config = {
             .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
             .gpioPort = GPIOC,
-            .gpioPin = Pin_13
+            .gpioPin = Pin_13,
+            .exti_port_source = EXTI_PortSourceGPIOC,
+            .exti_pin_source = EXTI_PinSource13,
+            .exti_line = EXTI_Line13,
+            .exti_irqn = EXTI15_10_IRQn
     };
     return &spRacingF3MPU6050Config;
 #endif
+
     return NULL;
 }
 

src/main/target/NAZE/target.h

@@ -69,6 +69,8 @@
 
 #define USE_FLASH_M25P16
 
+#define USE_MPU_DATA_READY_SIGNAL
+
 #define GYRO
 #define USE_GYRO_MPU3050
 #define USE_GYRO_MPU6050

src/main/target/SPRACINGF3/target.h

@@ -30,6 +30,8 @@
 
 #define USABLE_TIMER_CHANNEL_COUNT 17
 
+#define USE_MPU_DATA_READY_SIGNAL
+
 #define GYRO
 #define USE_GYRO_MPU6050
 #define GYRO_MPU6050_ALIGN CW270_DEG