diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index fe1675f0e..9a2bfb5e0 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -80,7 +80,7 @@ static const struct pios_exti_cfg pios_exti_bma180_cfg __exti_config = { .init = { .GPIO_Pin = GPIO_Pin_13, .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IPU, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, }, }, .irq = { @@ -102,6 +102,8 @@ static const struct pios_exti_cfg pios_exti_bma180_cfg __exti_config = { }; static const struct pios_bma180_cfg pios_bma180_cfg = { .exti_cfg = &pios_exti_bma180_cfg, + .bandwidth = BMA_BW_600HZ, + .range = BMA_RANGE_8G, }; #endif /* PIOS_INCLUDE_BMA180 */ @@ -118,7 +120,7 @@ static const struct pios_exti_cfg pios_exti_l3gd20_cfg __exti_config = { .init = { .GPIO_Pin = GPIO_Pin_3, .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IPU, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, }, }, .irq = { @@ -690,13 +692,8 @@ void PIOS_Board_Init(void) { #endif /* PIOS_INCLUDE_L3GD20 */ #if defined(PIOS_INCLUDE_BMA180) - PIOS_BMA180_Attach(pios_spi_flash_accel_id); - PIOS_BMA180_Init(&pios_bma180_cfg); + PIOS_BMA180_Init(pios_spi_flash_accel_id, 0, &pios_bma180_cfg); #endif /* PIOS_INCLUDE_BMA180 */ -#if defined(PIOS_INCLUDE_LED) - PIOS_LED_Init(&pios_led_cfg_cc3d); -#endif /* PIOS_INCLUDE_LED */ - break; default: PIOS_Assert(0); diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index b2dee8bab..6d83a7d11 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -397,8 +397,9 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData) // // accel_read_good = PIOS_BMA180_ReadFifo(&accel); // } - if(PIOS_BMA180_ReadAccels(&accel) < 0) + if(PIOS_BMA180_Test() < 0) return -1; + PIOS_BMA180_ReadAccels(&accel); accel_accum[0] += accel.x; accel_accum[1] += accel.y; accel_accum[2] += accel.z; diff --git a/flight/PiOS/Common/pios_bma180.c b/flight/PiOS/Common/pios_bma180.c index 7d5c53ef2..6acd0557f 100644 --- a/flight/PiOS/Common/pios_bma180.c +++ b/flight/PiOS/Common/pios_bma180.c @@ -35,8 +35,28 @@ #include "fifo_buffer.h" -static uint32_t PIOS_SPI_ACCEL; +enum pios_bma180_dev_magic { + PIOS_BMA180_DEV_MAGIC = 0xcbb93755, +}; +#define PIOS_BMA180_MAX_DOWNSAMPLE 10 +struct bma180_dev { + uint32_t spi_id; + uint32_t slave_num; + int16_t buffer[PIOS_BMA180_MAX_DOWNSAMPLE * sizeof(struct pios_bma180_data)]; + t_fifo_buffer fifo; + const struct pios_bma180_cfg * cfg; + enum bma180_bandwidth bandwidth; + enum bma180_range range; + enum pios_bma180_dev_magic magic; +}; + +//! Global structure for this device device +static struct bma180_dev * dev; + +//! Private functions +static struct bma180_dev * PIOS_BMA180_alloc(void); +static int32_t PIOS_BMA180_Validate(struct bma180_dev * dev); 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(enum bma180_bandwidth bw); @@ -44,30 +64,58 @@ static int32_t PIOS_BMA180_SetRange(enum bma180_range range); static int32_t PIOS_BMA180_Config(); static int32_t PIOS_BMA180_EnableIrq(); -#define PIOS_BMA180_MAX_DOWNSAMPLE 10 -static int16_t pios_bma180_buffer[PIOS_BMA180_MAX_DOWNSAMPLE * sizeof(struct pios_bma180_data)]; -static t_fifo_buffer pios_bma180_fifo; - -static const struct pios_bma180_cfg * dev_cfg; -static enum bma180_range range; - #define GRAV 9.81f /** - * @brief Initialize with good default settings + * @brief Allocate a new device */ -int32_t PIOS_BMA180_Init(const struct pios_bma180_cfg * cfg) +static struct bma180_dev * PIOS_BMA180_alloc(void) +{ + struct bma180_dev * bma180_dev; + + bma180_dev = (struct bma180_dev *)pvPortMalloc(sizeof(*bma180_dev)); + if (!bma180_dev) return (NULL); + + fifoBuf_init(&bma180_dev->fifo, (uint8_t *) bma180_dev->buffer, sizeof(bma180_dev->buffer)); + + bma180_dev->magic = PIOS_BMA180_DEV_MAGIC; + return(bma180_dev); +} + +/** + * @brief Validate the handle to the spi device + * @returns 0 for valid device or -1 otherwise + */ +static int32_t PIOS_BMA180_Validate(struct bma180_dev * dev) +{ + if (dev == NULL) + return -1; + if (dev->magic != PIOS_BMA180_DEV_MAGIC) + return -2; + if (dev->spi_id == 0) + return -3; + return 0; +} +/** + * @brief Initialize with good default settings + * @returns 0 for success, -1 for failure + */ +int32_t PIOS_BMA180_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_bma180_cfg * cfg) { - dev_cfg = cfg; // store config before enabling interrupt - - fifoBuf_init(&pios_bma180_fifo, (uint8_t *) pios_bma180_buffer, sizeof(pios_bma180_buffer)); - - PIOS_EXTI_Init(cfg->exti_cfg); + dev = PIOS_BMA180_alloc(); + if(dev == NULL) + return -1; + + dev->spi_id = spi_id; + dev->slave_num = slave_num; + dev->cfg = cfg; if(PIOS_BMA180_Config() < 0) return -1; PIOS_DELAY_WaituS(50); + PIOS_EXTI_Init(dev->cfg->exti_cfg); + while(PIOS_BMA180_EnableIrq() != 0); return 0; @@ -79,10 +127,15 @@ int32_t PIOS_BMA180_Init(const struct pios_bma180_cfg * cfg) */ int32_t PIOS_BMA180_ClaimBus() { - if(PIOS_SPI_ClaimBus(PIOS_SPI_ACCEL) != 0) { + if(PIOS_BMA180_Validate(dev) != 0) + return -1; + + if(PIOS_SPI_ClaimBus(dev->spi_id) != 0) { return -1; } - PIOS_SPI_RC_PinSet(PIOS_SPI_ACCEL,0,0); + + PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0); + return 0; } @@ -92,10 +145,14 @@ int32_t PIOS_BMA180_ClaimBus() */ int32_t PIOS_BMA180_ClaimBusISR() { - if(PIOS_SPI_ClaimBusISR(PIOS_SPI_ACCEL) != 0) { + if(PIOS_BMA180_Validate(dev) != 0) + return -1; + + if(PIOS_SPI_ClaimBusISR(dev->spi_id) != 0) { return -1; } - PIOS_SPI_RC_PinSet(PIOS_SPI_ACCEL,0,0); + + PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0); return 0; } @@ -105,8 +162,12 @@ int32_t PIOS_BMA180_ClaimBusISR() */ int32_t PIOS_BMA180_ReleaseBus() { - PIOS_SPI_RC_PinSet(PIOS_SPI_ACCEL,0,1); - return PIOS_SPI_ReleaseBus(PIOS_SPI_ACCEL); + if(PIOS_BMA180_Validate(dev) != 0) + return -1; + + PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,1); + + return PIOS_SPI_ReleaseBus(dev->spi_id); } /** @@ -116,13 +177,16 @@ int32_t PIOS_BMA180_ReleaseBus() */ int32_t PIOS_BMA180_GetReg(uint8_t reg) { + if(PIOS_BMA180_Validate(dev) != 0) + return -1; + uint8_t data; if(PIOS_BMA180_ClaimBus() != 0) return -1; - PIOS_SPI_TransferByte(PIOS_SPI_ACCEL,(0x80 | reg) ); // request byte - data = PIOS_SPI_TransferByte(PIOS_SPI_ACCEL,0 ); // receive response + PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte + data = PIOS_SPI_TransferByte(dev->spi_id,0 ); // receive response PIOS_BMA180_ReleaseBus(); return data; @@ -139,8 +203,8 @@ int32_t PIOS_BMA180_SetReg(uint8_t reg, uint8_t data) if(PIOS_BMA180_ClaimBus() != 0) return -1; - PIOS_SPI_TransferByte(PIOS_SPI_ACCEL, 0x7f & reg); - PIOS_SPI_TransferByte(PIOS_SPI_ACCEL, data); + PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg); + PIOS_SPI_TransferByte(dev->spi_id, data); PIOS_BMA180_ReleaseBus(); @@ -184,14 +248,17 @@ static int32_t PIOS_BMA180_Config() */ PIOS_DELAY_WaituS(20); - + + if(PIOS_BMA180_Validate(dev) != 0) + return -1; + while(PIOS_BMA180_EnableEeprom() != 0); while(PIOS_BMA180_SetReg(BMA_RESET, BMA_RESET_CODE) != 0); while(PIOS_BMA180_SetReg(BMA_OFFSET_LSB1, 0x81) != 0); while(PIOS_BMA180_SetReg(BMA_GAIN_Y, 0x81) != 0); while(PIOS_BMA180_SetReg(BMA_CTRREG3, 0xFF) != 0); - while(PIOS_BMA180_SelectBW(BMA_BW_600HZ) != 0); - while(PIOS_BMA180_SetRange(BMA_RANGE_8G) != 0); + while(PIOS_BMA180_SelectBW(dev->cfg->bandwidth) != 0); + while(PIOS_BMA180_SetRange(dev->cfg->range) != 0); while(PIOS_BMA180_DisableEeprom() != 0); return 0; @@ -206,6 +273,11 @@ static int32_t PIOS_BMA180_Config() */ static int32_t PIOS_BMA180_SelectBW(enum bma180_bandwidth bw) { + if(PIOS_BMA180_Validate(dev) != 0) + return -1; + + dev->bandwidth = bw; + uint8_t reg; reg = PIOS_BMA180_GetReg(BMA_BW_ADDR); reg = (reg & ~BMA_BW_MASK) | ((bw << BMA_BW_SHIFT) & BMA_BW_MASK); @@ -219,11 +291,15 @@ static int32_t PIOS_BMA180_SelectBW(enum bma180_bandwidth bw) * */ static int32_t PIOS_BMA180_SetRange(enum bma180_range new_range) -{ +{ + if(PIOS_BMA180_Validate(dev) != 0) + return -1; + uint8_t reg; - range = new_range; + + dev->range = new_range; reg = PIOS_BMA180_GetReg(BMA_RANGE_ADDR); - reg = (reg & ~BMA_RANGE_MASK) | ((range << BMA_RANGE_SHIFT) & BMA_RANGE_MASK); + reg = (reg & ~BMA_RANGE_MASK) | ((dev->range << BMA_RANGE_SHIFT) & BMA_RANGE_MASK); return PIOS_BMA180_SetReg(BMA_RANGE_ADDR, reg); } @@ -242,14 +318,6 @@ static int32_t PIOS_BMA180_EnableIrq() return 0; } -/** - * @brief Connect to the correct SPI bus - */ -void PIOS_BMA180_Attach(uint32_t spi_id) -{ - PIOS_SPI_ACCEL = spi_id; -} - /** * @brief Read a single set of values from the x y z channels * @param[out] data Int16 array of (x,y,z) sensor values @@ -266,7 +334,7 @@ int32_t PIOS_BMA180_ReadAccels(struct pios_bma180_data * data) if(PIOS_BMA180_ClaimBus() != 0) return -1; - if(PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,&buf[0],&rec[0],7,NULL) != 0) + if(PIOS_SPI_TransferBlock(dev->spi_id,&buf[0],&rec[0],7,NULL) != 0) return -2; PIOS_BMA180_ReleaseBus(); @@ -287,7 +355,10 @@ int32_t PIOS_BMA180_ReadAccels(struct pios_bma180_data * data) */ float PIOS_BMA180_GetScale() { - switch (range) { + if(PIOS_BMA180_Validate(dev) != 0) + return -1; + + switch (dev->cfg->range) { case BMA_RANGE_1G: return GRAV / 8192.0f; case BMA_RANGE_1_5G: @@ -313,10 +384,13 @@ float PIOS_BMA180_GetScale() */ int32_t PIOS_BMA180_ReadFifo(struct pios_bma180_data * buffer) { - if(fifoBuf_getUsed(&pios_bma180_fifo) < sizeof(*buffer)) + if(PIOS_BMA180_Validate(dev) != 0) + return -1; + + if(fifoBuf_getUsed(&dev->fifo) < sizeof(*buffer)) return -1; - fifoBuf_getData(&pios_bma180_fifo, (uint8_t *) buffer, sizeof(*buffer)); + fifoBuf_getData(&dev->fifo, (uint8_t *) buffer, sizeof(*buffer)); return 0; } @@ -336,7 +410,7 @@ int32_t PIOS_BMA180_Test() if(PIOS_BMA180_ClaimBus() != 0) return -1; - retval = PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,&buf[0],&rec[0],sizeof(buf),NULL); + retval = PIOS_SPI_TransferBlock(dev->spi_id,&buf[0],&rec[0],sizeof(buf),NULL); PIOS_BMA180_ReleaseBus(); if(retval != 0) @@ -358,8 +432,11 @@ int32_t PIOS_BMA180_Test() /** * @brief IRQ Handler. Read data from the BMA180 FIFO and push onto a local fifo. */ +int32_t bma180_irqs = 0; void PIOS_BMA180_IRQHandler(void) { + bma180_irqs++; + const static uint8_t pios_bma180_req_buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0}; uint8_t pios_bma180_dmabuf[8]; @@ -368,7 +445,7 @@ void PIOS_BMA180_IRQHandler(void) return; // Something else is using bus, miss this data } - PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,pios_bma180_req_buf,(uint8_t *) pios_bma180_dmabuf, + PIOS_SPI_TransferBlock(dev->spi_id,pios_bma180_req_buf,(uint8_t *) pios_bma180_dmabuf, sizeof(pios_bma180_dmabuf), NULL); // TODO: Make this conversion depend on configuration scale @@ -378,7 +455,7 @@ void PIOS_BMA180_IRQHandler(void) PIOS_BMA180_ReleaseBus(); // Must not return before releasing bus - if(fifoBuf_getFree(&pios_bma180_fifo) < sizeof(data)) + if(fifoBuf_getFree(&dev->fifo) < sizeof(data)) return; // Bottom two bits indicate new data and are constant zeros. Don't right @@ -391,7 +468,7 @@ void PIOS_BMA180_IRQHandler(void) data.z /= 4; data.temperature = pios_bma180_dmabuf[7]; - fifoBuf_putData(&pios_bma180_fifo, (uint8_t *) &data, sizeof(data)); + fifoBuf_putData(&dev->fifo, (uint8_t *) &data, sizeof(data)); } diff --git a/flight/PiOS/inc/pios_bma180.h b/flight/PiOS/inc/pios_bma180.h index f00ab03d6..ef9627f00 100644 --- a/flight/PiOS/inc/pios_bma180.h +++ b/flight/PiOS/inc/pios_bma180.h @@ -92,10 +92,12 @@ struct pios_bma180_data { struct pios_bma180_cfg { const struct pios_exti_cfg * exti_cfg; /* Pointer to the EXTI configuration */ + enum bma180_bandwidth bandwidth; + enum bma180_range range; }; /* Public Functions */ -extern int32_t PIOS_BMA180_Init(const struct pios_bma180_cfg * cfg); +extern int32_t PIOS_BMA180_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_bma180_cfg * cfg); extern void PIOS_BMA180_Attach(uint32_t spi_id); extern float PIOS_BMA180_GetScale(); extern int32_t PIOS_BMA180_ReadFifo(struct pios_bma180_data * buffer); diff --git a/flight/Revolution/System/pios_board.c b/flight/Revolution/System/pios_board.c index 32a17e938..9855767c2 100644 --- a/flight/Revolution/System/pios_board.c +++ b/flight/Revolution/System/pios_board.c @@ -1484,6 +1484,8 @@ static const struct pios_exti_cfg pios_exti_bma180_cfg __exti_config = { }; static const struct pios_bma180_cfg pios_bma180_cfg = { .exti_cfg = &pios_exti_bma180_cfg, + .bandwidth = BMA_BW_600HZ, + .range = BMA_RANGE_8G, }; #endif /* PIOS_INCLUDE_BMA180 */ @@ -1599,9 +1601,6 @@ void PIOS_Board_Init(void) { PIOS_DEBUG_Assert(0); } - PIOS_BMA180_Attach(pios_spi_accel_id); - PIOS_BMA180_Init(&pios_bma180_cfg); - /* Set up the SPI interface to the gyro */ if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { PIOS_DEBUG_Assert(0); @@ -1760,7 +1759,9 @@ void PIOS_Board_Init(void) { PIOS_DELAY_WaitmS(500); - +#if defined(PIOS_INCLUDE_BMA180) + PIOS_BMA180_Init(pios_spi_accel_id, 0, &pios_bma180_cfg); +#endif #if defined(PIOS_INCLUDE_MPU6000) PIOS_MPU6000_Attach(pios_spi_gyro_id); PIOS_MPU6000_Init(&pios_mpu6000_cfg); diff --git a/flight/board_hw_defs/coptercontrol/board_hw_defs.c b/flight/board_hw_defs/coptercontrol/board_hw_defs.c index 91b370753..e70ce9ed0 100644 --- a/flight/board_hw_defs/coptercontrol/board_hw_defs.c +++ b/flight/board_hw_defs/coptercontrol/board_hw_defs.c @@ -88,8 +88,9 @@ static const struct pios_spi_cfg pios_spi_gyro_cfg = { .SPI_CRCPolynomial = 7, .SPI_CPOL = SPI_CPOL_High, .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, /* 10 Mhz */ + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256, /* 10 Mhz */ }, + .use_crc = false, .dma = { .ahb_clk = RCC_AHBPeriph_DMA1, @@ -194,9 +195,9 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg = { .SPI_CRCPolynomial = 7, .SPI_CPOL = SPI_CPOL_High, .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256, }, - .use_crc = FALSE, + .use_crc = false, .dma = { .ahb_clk = RCC_AHBPeriph_DMA1,