diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c
index eacea46b0..57f9d3a81 100644
--- a/src/main/drivers/accgyro_mpu6050.c
+++ b/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();
- mpu6050Config = NULL; // avoid re-initialisation of GPIO;
- }
+ mpu6050GpioInit();
switch (mpuAccelTrim) {
case MPU_6050_HALF_RESOLUTION:
@@ -327,10 +404,7 @@ static void mpu6050AccRead(int16_t *accData)
static void mpu6050GyroInit(void)
{
- if (mpu6050Config) {
- mpu6050GpioInit();
- mpu6050Config = NULL; // avoid re-initialisation of GPIO;
- }
+ mpu6050GpioInit();
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)
diff --git a/src/main/drivers/accgyro_mpu6050.h b/src/main/drivers/accgyro_mpu6050.h
index e52c013ff..da25684cb 100644
--- a/src/main/drivers/accgyro_mpu6050.h
+++ b/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);
diff --git a/src/main/drivers/barometer_bmp085.c b/src/main/drivers/barometer_bmp085.c
index c67eb4e6d..6ebd4d8f7 100644
--- a/src/main/drivers/barometer_bmp085.c
+++ b/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;
diff --git a/src/main/drivers/nvic.h b/src/main/drivers/nvic.h
index 8c16e1b59..a8979f310 100644
--- a/src/main/drivers/nvic.h
+++ b/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
diff --git a/src/main/drivers/sonar_hcsr04.c b/src/main/drivers/sonar_hcsr04.c
index 9210b1352..2ab0c001d 100644
--- a/src/main/drivers/sonar_hcsr04.c
+++ b/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;
diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c
index f7696dded..ecc154a37 100644
--- a/src/main/drivers/system.c
+++ b/src/main/drivers/system.c
@@ -15,6 +15,7 @@
* along with Cleanflight. If not, see .
*/
+#include
#include
#include
#include
@@ -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();
}
-
-
diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h
index 0996b2d4e..6c36b5d1c 100644
--- a/src/main/drivers/system.h
+++ b/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);
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index 477025f84..8d79beddc 100644
--- a/src/main/sensors/initialisation.c
+++ b/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;
}
diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index e080224e0..17280ba99 100644
--- a/src/main/target/NAZE/target.h
+++ b/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
diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h
index f0eaabf1d..cab8cc849 100644
--- a/src/main/target/SPRACINGF3/target.h
+++ b/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