From 9ede84680e233976cf6ef9e7e04bc83310c86b2d Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 26 May 2011 13:57:41 -0500 Subject: [PATCH] OP-378: Start working on IRQ driven BMA180 reading and also fixed the mag order --- flight/PiOS/Boards/STM3210E_INS.h | 13 +++++- flight/PiOS/Common/pios_bma180.c | 76 +++++++++++++++++++++++++------ flight/PiOS/Common/pios_hmc5883.c | 5 ++ flight/PiOS/STM32F10x/pios_exti.c | 16 ++++++- flight/PiOS/inc/pios_bma180.h | 4 ++ 5 files changed, 95 insertions(+), 19 deletions(-) diff --git a/flight/PiOS/Boards/STM3210E_INS.h b/flight/PiOS/Boards/STM3210E_INS.h index a987b9e4e..e19ef51e0 100644 --- a/flight/PiOS/Boards/STM3210E_INS.h +++ b/flight/PiOS/Boards/STM3210E_INS.h @@ -207,8 +207,17 @@ extern uint32_t pios_com_aux_id; //------------------------ // BMA180 //------------------------ -#define PIOS_BMA_ENABLE PIOS_SPI_RC_PinSet(PIOS_SPI_ACCEL,0) -#define PIOS_BMA_DISABLE PIOS_SPI_RC_PinSet(PIOS_SPI_ACCEL,1) +#define PIOS_BMA180_ENABLE PIOS_SPI_RC_PinSet(PIOS_SPI_ACCEL,0) +#define PIOS_BMA180_DISABLE PIOS_SPI_RC_PinSet(PIOS_SPI_ACCEL,1) +#define PIOS_BMA180_DRDY_GPIO_PORT GPIOC +#define PIOS_BMA180_DRDY_GPIO_PIN GPIO_Pin_4 +#define PIOS_BMA180_DRDY_PORT_SOURCE GPIO_PortSourceGPIOC +#define PIOS_BMA180_DRDY_PIN_SOURCE GPIO_PinSource4 +#define PIOS_BMA180_DRDY_CLK RCC_APB2Periph_GPIOC +#define PIOS_BMA180_DRDY_EXTI_LINE EXTI_Line4 +#define PIOS_BMA180_DRDY_IRQn EXTI4_IRQn +#define PIOS_BMA180_DRDY_PRIO PIOS_IRQ_PRIO_LOW + //------------------------ // PIOS_HMC5883 diff --git a/flight/PiOS/Common/pios_bma180.c b/flight/PiOS/Common/pios_bma180.c index e2d6fe727..3f97748c3 100644 --- a/flight/PiOS/Common/pios_bma180.c +++ b/flight/PiOS/Common/pios_bma180.c @@ -38,6 +38,47 @@ static int32_t PIOS_BMA180_GetReg(uint8_t reg); static int32_t PIOS_BMA180_SetReg(uint8_t reg, uint8_t data); static int32_t PIOS_BMA180_SelectBW(uint8_t bw); static int32_t PIOS_BMA180_SetRange(uint8_t range); +static int32_t PIOS_BMA180_EnableIrq(); +volatile bool pios_bma180_data_ready = false; + +/** + * @brief Initialize with good default settings + */ +void PIOS_BMA180_Init() +{ + GPIO_InitTypeDef GPIO_InitStructure; + EXTI_InitTypeDef EXTI_InitStructure; + NVIC_InitTypeDef NVIC_InitStructure; + + /* Enable DRDY GPIO clock */ + RCC_APB2PeriphClockCmd(PIOS_BMA180_DRDY_CLK | RCC_APB2Periph_AFIO, ENABLE); + + /* Configure EOC pin as input floating */ + GPIO_InitStructure.GPIO_Pin = PIOS_BMA180_DRDY_GPIO_PIN; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; + GPIO_Init(PIOS_BMA180_DRDY_GPIO_PORT, &GPIO_InitStructure); + + /* Configure the End Of Conversion (EOC) interrupt */ + GPIO_EXTILineConfig(PIOS_BMA180_DRDY_PORT_SOURCE, PIOS_BMA180_DRDY_PIN_SOURCE); + EXTI_InitStructure.EXTI_Line = PIOS_BMA180_DRDY_EXTI_LINE; + EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt; + EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising; + EXTI_InitStructure.EXTI_LineCmd = ENABLE; + EXTI_Init(&EXTI_InitStructure); + + /* Enable and set EOC EXTI Interrupt to the lowest priority */ + NVIC_InitStructure.NVIC_IRQChannel = PIOS_BMA180_DRDY_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_BMA180_DRDY_PRIO; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + + pios_bma180_data_ready = false; + + PIOS_BMA180_SelectBW(BMA_BW_150HZ); + PIOS_BMA180_SetRange(BMA_RANGE_8G); + PIOS_BMA180_EnableIrq(); +} /** * @brief Claim the SPI bus for the accel communications and select this chip @@ -47,7 +88,7 @@ int32_t PIOS_BMA180_ClaimBus() { if(PIOS_SPI_ClaimBus(PIOS_SPI_ACCEL) != 0) return -1; - PIOS_BMA_ENABLE; + PIOS_BMA180_ENABLE; return 0; } @@ -57,7 +98,7 @@ int32_t PIOS_BMA180_ClaimBus() */ int32_t PIOS_BMA180_ReleaseBus() { - PIOS_BMA_DISABLE; + PIOS_BMA180_DISABLE; return PIOS_SPI_ReleaseBus(PIOS_SPI_ACCEL); } @@ -128,6 +169,11 @@ static int32_t PIOS_BMA180_SetRange(uint8_t range) return PIOS_BMA180_SetReg(BMA_RANGE_ADDR, reg); } +static int32_t PIOS_BMA180_EnableIrq() +{ + return PIOS_BMA180_SetReg(BMA_CTRREG3, BMA_NEW_DAT_INT); +} + /** * @brief Connect to the correct SPI bus */ @@ -136,17 +182,6 @@ void PIOS_BMA180_Attach(uint32_t spi_id) PIOS_SPI_ACCEL = spi_id; } -/** - * @brief Initialize with good default settings - */ -void PIOS_BMA180_Init() -{ - if(0){ - PIOS_BMA180_SelectBW(BMA_BW_150HZ); - PIOS_BMA180_SetRange(BMA_RANGE_8G); - } -} - /** * @brief Read a single set of values from the x y z channels * @param[out] data Int16 array of (x,y,z) sensor values @@ -197,13 +232,16 @@ int32_t PIOS_BMA180_Test() // Read chip ID then version ID uint8_t buf[3] = {0x80 | BMA_CHIPID_ADDR, 0, 0}; uint8_t rec[3] = {0,0, 0}; + int32_t retval; if(PIOS_BMA180_ClaimBus() != 0) return -1; - if(PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,&buf[0],&rec[0],sizeof(buf),NULL) != 0) - return -2; + retval = PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,&buf[0],&rec[0],sizeof(buf),NULL); PIOS_BMA180_ReleaseBus(); + if(retval != 0) + return -2; + int16_t data[3]; if(PIOS_BMA180_ReadAccels(data) != 0) return -3; @@ -217,6 +255,14 @@ int32_t PIOS_BMA180_Test() return 0; } +/** + * @brief IRQ Handler + */ +void PIOS_BMA180_IRQHandler(void) +{ + pios_bma180_data_ready = true; +} + /** * @} * @} diff --git a/flight/PiOS/Common/pios_hmc5883.c b/flight/PiOS/Common/pios_hmc5883.c index 1e5849cce..feb7256f0 100644 --- a/flight/PiOS/Common/pios_hmc5883.c +++ b/flight/PiOS/Common/pios_hmc5883.c @@ -232,6 +232,11 @@ void PIOS_HMC5883_ReadMag(int16_t out[3]) + buffer[2 * i + 1]) * 1000 / PIOS_HMC5883_Sensitivity_8_1Ga; break; } + + // Data reads out as X,Z,Y + int16_t temp = out[2]; + out[2] = out[1]; + out[1] = temp; } diff --git a/flight/PiOS/STM32F10x/pios_exti.c b/flight/PiOS/STM32F10x/pios_exti.c index e20a179e6..1367ac466 100644 --- a/flight/PiOS/STM32F10x/pios_exti.c +++ b/flight/PiOS/STM32F10x/pios_exti.c @@ -87,17 +87,29 @@ void EXTI9_5_IRQHandler(void) /** * Handle external line 4 interrupt requests */ -#if defined(PIOS_INCLUDE_USB) +#if defined(PIOS_INCLUDE_IMU3000) +extern void PIOS_IMU3000_IRQHandler(); +#endif + void EXTI4_IRQHandler(void) { +#if defined(PIOS_INCLUDE_USB) if (EXTI_GetITStatus(PIOS_USB_DETECT_EXTI_LINE) != RESET) { /* Clear the EXTI line pending bit */ EXTI_ClearITPendingBit(PIOS_USB_DETECT_EXTI_LINE); } -} #endif /* PIOS_INCLUDE_USB */ +#if defined (PIOS_INCLUDE_BMA180) + if (EXTI_GetITStatus(PIOS_BMA180_DRDY_EXTI_LINE) != RESET) { + PIOS_BMA180_IRQHandler(); + EXTI_ClearITPendingBit(PIOS_BMA180_DRDY_EXTI_LINE); + } +#endif +} +#if defined(PIOS_INCLUDE_IMU3000) extern void PIOS_IMU3000_IRQHandler(); +#endif void EXTI1_IRQHandler(void) { #if defined(PIOS_INCLUDE_IMU3000) diff --git a/flight/PiOS/inc/pios_bma180.h b/flight/PiOS/inc/pios_bma180.h index e46bd2d18..74cd7f995 100644 --- a/flight/PiOS/inc/pios_bma180.h +++ b/flight/PiOS/inc/pios_bma180.h @@ -42,6 +42,7 @@ #define BMA_WE_ADDR 0x0D #define BMA_BW_ADDR 0x20 #define BMA_RANGE_ADDR 0x35 +#define BMA_CTRREG3 0x21 /* Accel range */ #define BMA_RANGE_MASK 0x0E @@ -68,6 +69,8 @@ #define BMA_BW_HP1HZ 0x08 // High-pass, 1Hz #define BMA_BW_BP0_300HZ 0x09 // Band-pass, 0.3Hz-300Hz +#define BMA_NEW_DAT_INT 0x02 + struct pios_bma180_data { int16_t x; int16_t y; @@ -79,6 +82,7 @@ void PIOS_BMA180_Attach(uint32_t spi_id); void PIOS_BMA180_Init(); int32_t PIOS_BMA180_ReadAccels(int16_t * data); int32_t PIOS_BMA180_Test(); +void PIOS_BMA180_IRQHandler(void); #endif /* PIOS_BMA180_H */