SPRacingF3/Naze32 Rev 5 - Add support for MAG data ready EXTI handler.

src/main/drivers/accgyro_mpu6050.c

@@ -188,6 +188,10 @@ static const mpu6050Config_t *mpu6050Config = NULL;
 
 void MPU_DATA_READY_EXTI_Handler(void)
 {
+    if (EXTI_GetITStatus(mpu6050Config->exti_line) == RESET) {
+        return;
+    }
+
     EXTI_ClearITPendingBit(mpu6050Config->exti_line);
 
 #ifdef DEBUG_MPU_DATA_READY_INTERRUPT
@@ -203,7 +207,6 @@ void MPU_DATA_READY_EXTI_Handler(void)
 
     lastCalledAt = now;
 #endif
-
 }
 
 void configureMPUDataReadyInterruptHandling(void)

src/main/drivers/compass_hmc5883l.c

@@ -22,11 +22,13 @@
 #include <math.h>
 
 #include "platform.h"
+#include "debug.h"
 
 #include "common/axis.h"
 #include "common/maths.h"
 
 #include "system.h"
+#include "nvic.h"
 #include "gpio.h"
 #include "bus_i2c.h"
 #include "light_led.h"
@@ -38,7 +40,10 @@
 
 #include "compass_hmc5883l.h"
 
+//#define DEBUG_MAG_DATA_READY_INTERRUPT
+
 // HMC5883L, default address 0x1E
+// NAZE Target connections
 // PB12 connected to MAG_DRDY on rev4 hardware
 // PC14 connected to MAG_DRDY on rev5 hardware
 
@@ -115,6 +120,83 @@ static float magGain[3] = { 1.0f, 1.0f, 1.0f };
 
 static const hmc5883Config_t *hmc5883Config = NULL;
 
+void MAG_DATA_READY_EXTI_Handler(void)
+{
+    if (EXTI_GetITStatus(hmc5883Config->exti_line) == RESET) {
+        return;
+    }
+
+    EXTI_ClearITPendingBit(hmc5883Config->exti_line);
+
+#ifdef DEBUG_MAG_DATA_READY_INTERRUPT
+    // Measure the delta between calls to the interrupt handler
+    // currently should be around 65/66 milli seconds / 15hz output rate
+    static uint32_t lastCalledAt = 0;
+    static int32_t callDelta = 0;
+
+    uint32_t now = millis();
+    callDelta = now - lastCalledAt;
+
+    //UNUSED(callDelta);
+    debug[0] = callDelta;
+
+    lastCalledAt = now;
+#endif
+}
+
+static void hmc5883lConfigureDataReadyInterruptHandling(void)
+{
+#ifdef USE_MAG_DATA_READY_SIGNAL
+
+    if (!(hmc5883Config->exti_port_source && hmc5883Config->exti_pin_source)) {
+        return;
+    }
+#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(hmc5883Config->exti_port_source, hmc5883Config->exti_pin_source);
+#endif
+
+#ifdef STM32F303xC
+    gpioExtiLineConfig(hmc5883Config->exti_port_source, hmc5883Config->exti_pin_source);
+#endif
+
+#ifdef ENSURE_MAG_DATA_READY_IS_HIGH
+    uint8_t status = GPIO_ReadInputDataBit(hmc5883Config->gpioPort, hmc5883Config->gpioPin);
+    if (!status) {
+        return;
+    }
+#endif
+
+    registerExti15_10_CallbackHandler(MAG_DATA_READY_EXTI_Handler);
+
+    EXTI_ClearITPendingBit(hmc5883Config->exti_line);
+
+    EXTI_InitTypeDef EXTIInit;
+    EXTIInit.EXTI_Line = hmc5883Config->exti_line;
+    EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
+    EXTIInit.EXTI_Trigger = EXTI_Trigger_Falling;
+    EXTIInit.EXTI_LineCmd = ENABLE;
+    EXTI_Init(&EXTIInit);
+
+    NVIC_InitTypeDef NVIC_InitStructure;
+
+    NVIC_InitStructure.NVIC_IRQChannel = hmc5883Config->exti_irqn;
+    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_MAG_DATA_READY);
+    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_MAG_DATA_READY);
+    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+    NVIC_Init(&NVIC_InitStructure);
+#endif
+}
+
 bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse)
 {
     bool ack = false;
@@ -219,6 +301,8 @@ void hmc5883lInit(void)
         magGain[Y] = 1.0f;
         magGain[Z] = 1.0f;
     }
+
+    hmc5883lConfigureDataReadyInterruptHandling();
 }
 
 void hmc5883lRead(int16_t *magData)

src/main/drivers/compass_hmc5883l.h

@@ -26,6 +26,11 @@ typedef struct hmc5883Config_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;
 } hmc5883Config_t;
 
 bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse);

src/main/drivers/nvic.h

@@ -23,6 +23,7 @@
 #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_MAG_DATA_READY           NVIC_BUILD_PRIORITY(0x0f, 0x0f)
 #define NVIC_PRIO_CALLBACK                 NVIC_BUILD_PRIORITY(0x0f, 0x0f)
 
 // utility macros to join/split priority

src/main/sensors/initialisation.c

@@ -476,12 +476,26 @@ static void detectMag(magSensor_e magHardwareToUse)
     static const hmc5883Config_t nazeHmc5883Config_v1_v4 = {
             .gpioAPB2Peripherals = RCC_APB2Periph_GPIOB,
             .gpioPin = Pin_12,
-            .gpioPort = GPIOB
+            .gpioPort = GPIOB,
+
+            .exti_port_source = 0,
+            .exti_pin_source = 0
+
+            /* Disabled for v4 needs more work.
+            .exti_port_source = GPIO_PortSourceGPIOB,
+            .exti_pin_source = GPIO_PinSource12,
+            .exti_line = EXTI_Line12,
+            .exti_irqn = EXTI15_10_IRQn
+            */
     };
     static const hmc5883Config_t nazeHmc5883Config_v5 = {
             .gpioAPB2Peripherals = RCC_APB2Periph_GPIOC,
             .gpioPin = Pin_14,
-            .gpioPort = GPIOC
+            .gpioPort = GPIOC,
+            .exti_port_source = GPIO_PortSourceGPIOC,
+            .exti_pin_source = GPIO_PinSource14,
+            .exti_line = EXTI_Line14,
+            .exti_irqn = EXTI15_10_IRQn
     };
     if (hardwareRevision < NAZE32_REV5) {
         hmc5883Config = &nazeHmc5883Config_v1_v4;
@@ -494,7 +508,11 @@ static void detectMag(magSensor_e magHardwareToUse)
     static const hmc5883Config_t spRacingF3Hmc5883Config = {
         .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
         .gpioPin = Pin_14,
-        .gpioPort = GPIOC
+        .gpioPort = GPIOC,
+        .exti_port_source = EXTI_PortSourceGPIOC,
+        .exti_pin_source = EXTI_PinSource14,
+        .exti_line = EXTI_Line14,
+        .exti_irqn = EXTI15_10_IRQn
     };
 
     hmc5883Config = &spRacingF3Hmc5883Config;

src/main/target/NAZE/target.h

@@ -70,9 +70,14 @@
 
 #define USE_FLASH_M25P16
 
-#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU data ready and BMP085 EOC
+#define EXTI15_10_CALLBACK_HANDLER_COUNT 3 // MPU data ready, MAG data ready, BMP085 EOC
+
+//#define DEBUG_MPU_DATA_READY_INTERRUPT
 #define USE_MPU_DATA_READY_SIGNAL
 
+//#define DEBUG_MAG_DATA_READY_INTERRUPT
+#define USE_MAG_DATA_READY_SIGNAL
+
 #define GYRO
 #define USE_GYRO_MPU3050
 #define USE_GYRO_MPU6050

src/main/target/SPRACINGF3/target.h

@@ -30,7 +30,14 @@
 
 #define USABLE_TIMER_CHANNEL_COUNT 17
 
+#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready
+
 #define USE_MPU_DATA_READY_SIGNAL
+#define ENSURE_MPU_DATA_READY_IS_LOW
+
+#define USE_MAG_DATA_READY_SIGNAL
+#define ENSURE_MAG_DATA_READY_IS_HIGH
+
 
 #define GYRO
 #define USE_GYRO_MPU6050