From 5a10e75551f56597c1ac4aa0ec28d10cf8d127fe Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 28 Jun 2016 10:27:26 +1000 Subject: [PATCH] Update hmc5883l to use new IO --- src/main/drivers/compass_hmc5883l.c | 81 ++++++----------------------- src/main/drivers/compass_hmc5883l.h | 16 ++---- src/main/sensors/initialisation.c | 31 ++--------- src/main/target/NAZE/target.h | 3 +- src/main/target/SPRACINGF3/target.h | 9 ++-- 5 files changed, 31 insertions(+), 109 deletions(-) diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index a018ab0f52..c8c398fc3d 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -29,7 +29,8 @@ #include "system.h" #include "nvic.h" -#include "gpio.h" +#include "io.h" +#include "exti.h" #include "bus_i2c.h" #include "light_led.h" @@ -120,14 +121,14 @@ static float magGain[3] = { 1.0f, 1.0f, 1.0f }; static const hmc5883Config_t *hmc5883Config = NULL; -void MAG_DATA_READY_EXTI_Handler(void) +#ifdef USE_MAG_DATA_READY_SIGNAL + +static IO_t intIO; +static extiCallbackRec_t hmc5883_extiCallbackRec; + +void hmc5883_extiHandler(extiCallbackRec_t* cb) { - if (EXTI_GetITStatus(hmc5883Config->exti_line) == RESET) { - return; - } - - EXTI_ClearITPendingBit(hmc5883Config->exti_line); - + UNUSED(cb); #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 @@ -143,57 +144,26 @@ void MAG_DATA_READY_EXTI_Handler(void) lastCalledAt = now; #endif } +#endif static void hmc5883lConfigureDataReadyInterruptHandling(void) { #ifdef USE_MAG_DATA_READY_SIGNAL - if (!(hmc5883Config->exti_port_source && hmc5883Config->exti_pin_source)) { + if (!(hmc5883Config->intTag)) { 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 - + intIO = IOGetByTag(hmc5883Config->intTag); #ifdef ENSURE_MAG_DATA_READY_IS_HIGH - uint8_t status = GPIO_ReadInputDataBit(hmc5883Config->gpioPort, hmc5883Config->gpioPin); + uint8_t status = IORead(intIO); if (!status) { return; } #endif - registerExtiCallbackHandler(hmc5883Config->exti_irqn, 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); + EXTIHandlerInit(&hmc5883_extiCallbackRec, hmc5883_extiHandler); + EXTIConfig(intIO, &hmc5883_extiCallbackRec, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising); + EXTIEnable(intIO, true); #endif } @@ -221,25 +191,6 @@ void hmc5883lInit(void) int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow. bool bret = true; // Error indicator - gpio_config_t gpio; - - if (hmc5883Config) { -#ifdef STM32F303 - if (hmc5883Config->gpioAHBPeripherals) { - RCC_AHBPeriphClockCmd(hmc5883Config->gpioAHBPeripherals, ENABLE); - } -#endif -#ifdef STM32F10X - if (hmc5883Config->gpioAPB2Peripherals) { - RCC_APB2PeriphClockCmd(hmc5883Config->gpioAPB2Peripherals, ENABLE); - } -#endif - gpio.pin = hmc5883Config->gpioPin; - gpio.speed = Speed_2MHz; - gpio.mode = Mode_IN_FLOATING; - gpioInit(hmc5883Config->gpioPort, &gpio); - } - delay(50); i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias // Note that the very first measurement after a gain change maintains the same gain as the previous setting. diff --git a/src/main/drivers/compass_hmc5883l.h b/src/main/drivers/compass_hmc5883l.h index 53c4c9f3e5..035a2c936f 100644 --- a/src/main/drivers/compass_hmc5883l.h +++ b/src/main/drivers/compass_hmc5883l.h @@ -17,20 +17,10 @@ #pragma once -typedef struct hmc5883Config_s { -#ifdef STM32F303 - uint32_t gpioAHBPeripherals; -#endif -#ifdef STM32F10X - uint32_t gpioAPB2Peripherals; -#endif - uint16_t gpioPin; - GPIO_TypeDef *gpioPort; +#include "io.h" - uint8_t exti_port_source; - uint32_t exti_line; - uint8_t exti_pin_source; - IRQn_Type exti_irqn; +typedef struct hmc5883Config_s { + ioTag_t intTag; } hmc5883Config_t; bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 80d6169518..2d96a0130a 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -484,27 +484,12 @@ static void detectMag(magSensor_e magHardwareToUse) #ifdef USE_MAG_HMC5883 const hmc5883Config_t *hmc5883Config = 0; -#ifdef NAZE +#ifdef NAZE // TODO remove this target specific define static const hmc5883Config_t nazeHmc5883Config_v1_v4 = { - .gpioAPB2Peripherals = RCC_APB2Periph_GPIOB, - .gpioPin = Pin_12, - .gpioPort = GPIOB, - - /* 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 - */ + .intTag = IO_TAG(PB12) /* perhaps disabled? */ }; static const hmc5883Config_t nazeHmc5883Config_v5 = { - .gpioAPB2Peripherals = RCC_APB2Periph_GPIOC, - .gpioPin = Pin_14, - .gpioPort = GPIOC, - .exti_port_source = GPIO_PortSourceGPIOC, - .exti_line = EXTI_Line14, - .exti_pin_source = GPIO_PinSource14, - .exti_irqn = EXTI15_10_IRQn + .intTag = IO_TAG(MAG_INT_EXTI) }; if (hardwareRevision < NAZE32_REV5) { hmc5883Config = &nazeHmc5883Config_v1_v4; @@ -513,15 +498,9 @@ static void detectMag(magSensor_e magHardwareToUse) } #endif -#ifdef SPRACINGF3 +#ifdef MAG_INT_EXTI static const hmc5883Config_t spRacingF3Hmc5883Config = { - .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC, - .gpioPin = Pin_14, - .gpioPort = GPIOC, - .exti_port_source = EXTI_PortSourceGPIOC, - .exti_pin_source = EXTI_PinSource14, - .exti_line = EXTI_Line14, - .exti_irqn = EXTI15_10_IRQn + .intTag = IO_TAG(MAG_INT_EXTI) }; hmc5883Config = &spRacingF3Hmc5883Config; diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 9f6e3d2b22..95f531faf5 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -74,7 +74,8 @@ //#define DEBUG_MAG_DATA_READY_INTERRUPT #define USE_MAG_DATA_READY_SIGNAL - +#define MAG_INT_EXTI PC14 + #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 68413bbe0b..a4f44762dc 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -35,10 +35,6 @@ #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 #define GYRO_MPU6050_ALIGN CW270_DEG @@ -57,6 +53,11 @@ #define USE_MAG_HMC5883 #define MAG_HMC5883_ALIGN CW270_DEG +#define USE_MAG_DATA_READY_SIGNAL +#define ENSURE_MAG_DATA_READY_IS_HIGH +#define MAG_INT_EXTI PC14 + + #define USE_FLASHFS #define USE_FLASH_M25P16