diff --git a/flight/Modules/Sensors/sensors.c b/flight/Modules/Sensors/sensors.c index 8d85ee7dd..929cab662 100644 --- a/flight/Modules/Sensors/sensors.c +++ b/flight/Modules/Sensors/sensors.c @@ -158,8 +158,8 @@ uint32_t sensor_dt_us; static void SensorsTask(void *parameters) { portTickType lastSysTime; - uint32_t accel_samples; - uint32_t gyro_samples; + uint32_t accel_samples = 0; + uint32_t gyro_samples = 0; int32_t accel_accum[3] = {0, 0, 0}; int32_t gyro_accum[3] = {0,0,0}; float gyro_scaling = 0; @@ -301,35 +301,28 @@ static void SensorsTask(void *parameters) case 0x02: // MPU6000 board #if defined(PIOS_INCLUDE_MPU6000) { - struct pios_mpu6000_data gyro; + struct pios_mpu6000_data mpu6000_data; + xQueueHandle queue = PIOS_MPU6000_GetQueue(); - count = 0; - while((read_good = PIOS_MPU6000_ReadFifo(&gyro)) != 0 && !error) - error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error; - if (error) - continue; - while(read_good == 0) { - count++; - - gyro_accum[0] += gyro.gyro_x; - gyro_accum[1] += gyro.gyro_y; - gyro_accum[2] += gyro.gyro_z; - - accel_accum[0] += gyro.accel_x; - accel_accum[1] += gyro.accel_y; - accel_accum[2] += gyro.accel_z; - - read_good = PIOS_MPU6000_ReadFifo(&gyro); + while(xQueueReceive(queue, (void *) &mpu6000_data, gyro_samples == 0 ? 10 : 0) != errQUEUE_EMPTY) + { + gyro_accum[0] += mpu6000_data.gyro_x; + gyro_accum[1] += mpu6000_data.gyro_y; + gyro_accum[2] += mpu6000_data.gyro_z; + + accel_accum[0] += mpu6000_data.accel_x; + accel_accum[1] += mpu6000_data.accel_y; + accel_accum[2] += mpu6000_data.accel_z; + + gyro_samples ++; + accel_samples ++; } - gyro_samples = count; + gyro_scaling = PIOS_MPU6000_GetScale(); - - accel_samples = count; accel_scaling = PIOS_MPU6000_GetAccelScale(); - // Get temp from last reading - gyrosData.temperature = 35.0f + ((float) gyro.temperature + 512.0f) / 340.0f; - accelsData.temperature = 35.0f + ((float) gyro.temperature + 512.0f) / 340.0f; + gyrosData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f; + accelsData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f; } #endif /* PIOS_INCLUDE_MPU6000 */ break; diff --git a/flight/PiOS/Common/pios_mpu6000.c b/flight/PiOS/Common/pios_mpu6000.c index 421ff7117..56e8cccbd 100644 --- a/flight/PiOS/Common/pios_mpu6000.c +++ b/flight/PiOS/Common/pios_mpu6000.c @@ -154,7 +154,7 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg) // FIFO storage #if defined(PIOS_MPU6000_ACCEL) - // Set the accel to 8g mode + // Set the accel scale while (PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, cfg->accel_range) != 0); while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store | PIOS_MPU6000_ACCEL_OUT) != 0); diff --git a/flight/PiOS/STM32F4xx/pios_spi.c b/flight/PiOS/STM32F4xx/pios_spi.c index 51906e268..87a8e0e9e 100644 --- a/flight/PiOS/STM32F4xx/pios_spi.c +++ b/flight/PiOS/STM32F4xx/pios_spi.c @@ -196,39 +196,47 @@ out_fail: } /** -* (Re-)initialises SPI peripheral clock rate -* -* \param[in] spi SPI number (0 or 1) -* \param[in] spi_prescaler configures the SPI speed -* \return 0 if no error -* \return -1 if disabled SPI port selected -* \return -3 if invalid spi_prescaler selected -*/ + * (Re-)initialises SPI peripheral clock rate + * + * \param[in] spi SPI number (0 or 1) + * \param[in] spi_prescaler configures the SPI speed: + * + * \return 0 if no error + * \return -1 if disabled SPI port selected + * \return -3 if invalid spi_prescaler selected + */ int32_t PIOS_SPI_SetClockSpeed(uint32_t spi_id, SPIPrescalerTypeDef spi_prescaler) { struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - + bool valid = PIOS_SPI_validate(spi_dev); PIOS_Assert(valid) - + SPI_InitTypeDef SPI_InitStructure; - + if (spi_prescaler >= 8) { /* Invalid prescaler selected */ return -3; } - + /* Start with a copy of the default configuration for the peripheral */ SPI_InitStructure = spi_dev->cfg->init; - + /* Adjust the prescaler for the peripheral's clock */ - /* XXX is this correct? */ SPI_InitStructure.SPI_BaudRatePrescaler = ((uint16_t) spi_prescaler & 7) << 3; - + /* Write back the new configuration */ SPI_Init(spi_dev->cfg->regs, &SPI_InitStructure); - - /* XXX what gets selected by this? */ + PIOS_SPI_TransferByte(spi_id, 0xFF); return 0; } @@ -620,17 +628,6 @@ int32_t PIOS_SPI_Busy(uint32_t spi_id) return(0); } -void PIOS_SPI_SetPrescalar(uint32_t spi_id, uint32_t prescaler) -{ - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid); - PIOS_Assert(IS_SPI_BAUDRATE_PRESCALER(prescaler)); - - spi_dev->cfg->regs->CR1 = (spi_dev->cfg->regs->CR1 & ~0x0038) | prescaler; -} - void PIOS_SPI_IRQ_Handler(uint32_t spi_id) { struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; diff --git a/flight/Revolution/System/pios_board.c b/flight/Revolution/System/pios_board.c index c1e51f880..0d57da254 100644 --- a/flight/Revolution/System/pios_board.c +++ b/flight/Revolution/System/pios_board.c @@ -174,11 +174,12 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { .exti_cfg = &pios_exti_mpu6000_cfg, .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, // Clock at 8 khz, downsampled by 8 for 1khz - .Smpl_rate_div = 7, + .Smpl_rate_div = 7, .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, .User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN, .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, + .accel_range = PIOS_MPU6000_ACCEL_8G, .gyro_range = PIOS_MPU6000_SCALE_500_DEG, .filter = PIOS_MPU6000_LOWPASS_256_HZ }; @@ -815,8 +816,7 @@ void PIOS_Board_Init(void) { break; case 0x02: #if defined(PIOS_INCLUDE_MPU6000) - PIOS_MPU6000_Attach(pios_spi_gyro_id); - PIOS_MPU6000_Init(&pios_mpu6000_cfg); + PIOS_MPU6000_Init(pios_spi_gyro_id,0, &pios_mpu6000_cfg); #endif break; default: