From 8a9bca25147b2234af55eb5d8a448b140f2b8a66 Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 30 Oct 2018 12:16:44 +0900 Subject: [PATCH] Unified EXTIConfig API for all MCU types --- src/main/drivers/accgyro/accgyro_mpu.c | 10 +-- src/main/drivers/accgyro/accgyro_spi_bmi160.c | 4 +- src/main/drivers/barometer/barometer_bmp085.c | 2 +- src/main/drivers/compass/compass_hmc5883l.c | 9 +-- src/main/drivers/exti.c | 62 ++++++++++--------- src/main/drivers/exti.h | 12 ++-- src/main/drivers/io.h | 8 +++ .../drivers/rangefinder/rangefinder_hcsr04.c | 2 +- src/main/drivers/rx/rx_a7105.c | 11 +--- src/main/target/OMNIBUSF7/target.h | 6 ++ 10 files changed, 62 insertions(+), 64 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 62350addb2..c16cbcfaca 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -134,17 +134,9 @@ static void mpuIntExtiInit(gyroDev_t *gyro) } #endif -#if defined (STM32F7) IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0); EXTIHandlerInit(&gyro->exti, mpuIntExtiHandler); - EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL)); // TODO - maybe pullup / pulldown ? -#else - IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0); - IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ? - - EXTIHandlerInit(&gyro->exti, mpuIntExtiHandler); - EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising); -#endif + EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, EXTI_TRIGGER_RISING); EXTIEnable(mpuIntIO, true); } #endif // MPU_INT_EXTI diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi160.c b/src/main/drivers/accgyro/accgyro_spi_bmi160.c index ac0af59796..ce297e7643 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi160.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi160.c @@ -252,10 +252,8 @@ static void bmi160IntExtiInit(gyroDev_t *gyro) IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag); IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0); - IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ? - EXTIHandlerInit(&gyro->exti, bmi160ExtiHandler); - EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising); + EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, EXTI_TRIGGER_RISING); // TODO - maybe pullup / pulldown ? EXTIEnable(mpuIntIO, true); } diff --git a/src/main/drivers/barometer/barometer_bmp085.c b/src/main/drivers/barometer/barometer_bmp085.c index 4dba22d157..d5758ce3c8 100644 --- a/src/main/drivers/barometer/barometer_bmp085.c +++ b/src/main/drivers/barometer/barometer_bmp085.c @@ -181,7 +181,7 @@ bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro) IOInit(eocIO, OWNER_BARO_EXTI, 0); IOConfigGPIO(eocIO, Mode_IN_FLOATING); EXTIHandlerInit(&bmp085_extiCallbackRec, bmp085_extiHandler); - EXTIConfig(eocIO, &bmp085_extiCallbackRec, NVIC_PRIO_BARO_EXTI, EXTI_Trigger_Rising); + EXTIConfig(eocIO, &bmp085_extiCallbackRec, NVIC_PRIO_BARO_EXTI, IOCFG_IN_FLOATING, EXTI_TRIGGER_RISING); EXTIEnable(eocIO, true); } #else diff --git a/src/main/drivers/compass/compass_hmc5883l.c b/src/main/drivers/compass/compass_hmc5883l.c index 91461ea05b..8f24db3cb7 100644 --- a/src/main/drivers/compass/compass_hmc5883l.c +++ b/src/main/drivers/compass/compass_hmc5883l.c @@ -172,18 +172,11 @@ static void hmc5883lConfigureDataReadyInterruptHandling(magDev_t* mag) } #endif -#if defined (STM32F7) IOInit(magIntIO, OWNER_COMPASS_EXTI, 0); EXTIHandlerInit(&mag->exti, hmc5883_extiHandler); - EXTIConfig(magIntIO, &mag->exti, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL)); + EXTIConfig(magIntIO, &mag->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, EXTI_TRIGGER_RISING); EXTIEnable(magIntIO, true); -#else - IOInit(magIntIO, OWNER_COMPASS_EXTI, 0); - IOConfigGPIO(magIntIO, IOCFG_IN_FLOATING); - EXTIHandlerInit(&mag->exti, hmc5883_extiHandler); - EXTIConfig(magIntIO, &mag->exti, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising); EXTIEnable(magIntIO, true); -#endif #else UNUSED(mag); #endif diff --git a/src/main/drivers/exti.c b/src/main/drivers/exti.c index 884bf5889f..b1a111f768 100644 --- a/src/main/drivers/exti.c +++ b/src/main/drivers/exti.c @@ -36,7 +36,7 @@ typedef struct { extiChannelRec_t extiChannelRecs[16]; -// IRQ gouping, same on 103 and 303 +// IRQ gouping, same on 103, 303, 40x and 7xx. #define EXTI_IRQ_GROUPS 7 // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 static const uint8_t extiGroups[16] = { 0, 1, 2, 3, 4, 5, 5, 5, 5, 5, 6, 6, 6, 6, 6, 6 }; @@ -66,6 +66,20 @@ static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = { # warning "Unknown CPU" #endif +static uint32_t triggerLookupTable[] = { +#if defined(STM32F7) + [EXTI_TRIGGER_RISING] = GPIO_MODE_IT_RISING, + [EXTI_TRIGGER_FALLING] = GPIO_MODE_IT_FALLING, + [EXTI_TRIGGER_BOTH] = GPIO_MODE_IT_RISING_FALLING +#elif defined(STM32F1) || defined(STM32F3) || defined(STM32F4) + [EXTI_TRIGGER_RISING] = EXTI_Trigger_Rising, + [EXTI_TRIGGER_FALLING] = EXTI_Trigger_Falling, + [EXTI_TRIGGER_BOTH] = EXTI_Trigger_Rising_Falling +#else +# warning "Unknown CPU" +#endif +}; + void EXTIInit(void) { @@ -92,48 +106,37 @@ void EXTIHandlerInit(extiCallbackRec_t *self, extiHandlerCallback *fn) self->fn = fn; } -#if defined(STM32F7) -void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t config) +void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t config, extiTrigger_t trigger) { - (void)config; - int chIdx; - chIdx = IO_GPIOPinIdx(io); - if (chIdx < 0) + int chIdx = IO_GPIOPinIdx(io); + + if (chIdx < 0) { return; - extiChannelRec_t *rec = &extiChannelRecs[chIdx]; + } + int group = extiGroups[chIdx]; + extiChannelRec_t *rec = &extiChannelRecs[chIdx]; + rec->handler = cb; + +#if defined(STM32F7) GPIO_InitTypeDef init = { .Pin = IO_Pin(io), - .Mode = GPIO_MODE_IT_RISING, - .Speed = GPIO_SPEED_FREQ_LOW, - .Pull = GPIO_NOPULL, + .Mode = GPIO_MODE_INPUT | IO_CONFIG_GET_MODE(config) | triggerLookupTable[trigger], + .Speed = IO_CONFIG_GET_SPEED(config), + .Pull = IO_CONFIG_GET_PULL(config), }; HAL_GPIO_Init(IO_GPIO(io), &init); - rec->handler = cb; - //uint32_t extiLine = IO_EXTI_Line(io); - - //EXTI_ClearITPendingBit(extiLine); if (extiGroupPriority[group] > irqPriority) { extiGroupPriority[group] = irqPriority; HAL_NVIC_SetPriority(extiGroupIRQn[group], NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority)); HAL_NVIC_EnableIRQ(extiGroupIRQn[group]); } -} #else + IOConfigGPIO(io, config); -void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, EXTITrigger_TypeDef trigger) -{ - int chIdx; - chIdx = IO_GPIOPinIdx(io); - if (chIdx < 0) - return; - extiChannelRec_t *rec = &extiChannelRecs[chIdx]; - int group = extiGroups[chIdx]; - - rec->handler = cb; #if defined(STM32F10X) GPIO_EXTILineConfig(IO_GPIO_PortSource(io), IO_GPIO_PinSource(io)); #elif defined(STM32F303xC) @@ -143,6 +146,7 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, EXTITrigger_Typ #else # warning "Unknown CPU" #endif + uint32_t extiLine = IO_EXTI_Line(io); EXTI_ClearITPendingBit(extiLine); @@ -150,7 +154,7 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, EXTITrigger_Typ EXTI_InitTypeDef EXTIInit; EXTIInit.EXTI_Line = extiLine; EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt; - EXTIInit.EXTI_Trigger = trigger; + EXTIInit.EXTI_Trigger = triggerLookupTable[trigger]; EXTIInit.EXTI_LineCmd = ENABLE; EXTI_Init(&EXTIInit); @@ -164,8 +168,8 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, EXTITrigger_Typ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); } -} #endif +} void EXTIRelease(IO_t io) { @@ -200,7 +204,7 @@ void EXTIEnable(IO_t io, bool enable) else EXTI->IMR &= ~(1 << extiLine); #else -# error "Unsupported target" +# error "Unknown CPU" #endif } diff --git a/src/main/drivers/exti.h b/src/main/drivers/exti.h index 94b7d7bd61..4274fc8cde 100644 --- a/src/main/drivers/exti.h +++ b/src/main/drivers/exti.h @@ -24,6 +24,12 @@ #include "drivers/io_types.h" +typedef enum { + EXTI_TRIGGER_RISING = 0, + EXTI_TRIGGER_FALLING = 1, + EXTI_TRIGGER_BOTH = 2 +} extiTrigger_t; + typedef struct extiCallbackRec_s extiCallbackRec_t; typedef void extiHandlerCallback(extiCallbackRec_t *self); @@ -34,10 +40,6 @@ struct extiCallbackRec_s { void EXTIInit(void); void EXTIHandlerInit(extiCallbackRec_t *cb, extiHandlerCallback *fn); -#if defined(STM32F7) -void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t config); -#else -void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, EXTITrigger_TypeDef trigger); -#endif +void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t config, extiTrigger_t trigger); void EXTIRelease(IO_t io); void EXTIEnable(IO_t io, bool enable); diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index b0a1d0f273..35e3c106d1 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -98,6 +98,14 @@ # warning "Unknown TARGET" #endif +#if defined(STM32F7) +// Expose these for EXTIConfig +#define IO_CONFIG_GET_MODE(cfg) (((cfg) >> 0) & 0x03) +#define IO_CONFIG_GET_SPEED(cfg) (((cfg) >> 2) & 0x03) +#define IO_CONFIG_GET_OTYPE(cfg) (((cfg) >> 4) & 0x01) +#define IO_CONFIG_GET_PULL(cfg) (((cfg) >> 5) & 0x03) +#endif + // declare available IO pins. Available pins are specified per target #include "io_def.h" diff --git a/src/main/drivers/rangefinder/rangefinder_hcsr04.c b/src/main/drivers/rangefinder/rangefinder_hcsr04.c index d9aca52a47..4407137a15 100644 --- a/src/main/drivers/rangefinder/rangefinder_hcsr04.c +++ b/src/main/drivers/rangefinder/rangefinder_hcsr04.c @@ -203,7 +203,7 @@ bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwa // Hardware detected - configure the driver #ifdef USE_EXTI EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler); - EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority! + EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, IOCFG_IN_FLOATING, EXTI_TRIGGER_BOTH); // TODO - priority! EXTIEnable(echoIO, true); #endif diff --git a/src/main/drivers/rx/rx_a7105.c b/src/main/drivers/rx/rx_a7105.c index 6bdb0a40b1..d4511be39b 100644 --- a/src/main/drivers/rx/rx_a7105.c +++ b/src/main/drivers/rx/rx_a7105.c @@ -54,18 +54,13 @@ void a7105extiHandler(extiCallbackRec_t* cb) } } -void A7105Init (uint32_t id) { +void A7105Init (uint32_t id) +{ spiDeviceByInstance(RX_SPI_INSTANCE); rxIntIO = IOGetByTag(IO_TAG(RX_IRQ_PIN)); /* config receiver IRQ pin */ IOInit(rxIntIO, OWNER_RX_SPI_CS, 0); -#ifdef STM32F7 EXTIHandlerInit(&a7105extiCallbackRec, a7105extiHandler); - EXTIConfig(rxIntIO, &a7105extiCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_PULLDOWN)); -#else - IOConfigGPIO(rxIntIO, IOCFG_IPD); - EXTIHandlerInit(&a7105extiCallbackRec, a7105extiHandler); - EXTIConfig(rxIntIO, &a7105extiCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising); -#endif + EXTIConfig(rxIntIO, &a7105extiCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IPD, EXTI_TRIGGER_RISING); EXTIEnable(rxIntIO, false); #ifdef RX_PA_TXEN_PIN diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h index 928c90c6df..570b148c75 100644 --- a/src/main/target/OMNIBUSF7/target.h +++ b/src/main/target/OMNIBUSF7/target.h @@ -223,6 +223,12 @@ #define VBAT_ADC_PIN PC3 #define RSSI_ADC_PIN PC5 +// Additional sensors ---------------------- +#define USE_RANGEFINDER +#define USE_RANGEFINDER_HCSR04 +#define RANGEFINDER_HCSR04_TRIGGER_PIN PB10 // TX3 for testing +#define RANGEFINDER_HCSR04_ECHO_PIN PB11 // RX3 for testing + //DEFAULTS---------------------------------- #define DEFAULT_FEATURES (FEATURE_OSD)