rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

Make revolution work with the FreeRTOS queue based MPU6000 driver

Browse Source
This commit is contained in:
James Cotton 2012-04-03 03:38:44 -05:00
parent bb41bc1472
commit e82323af32
4 changed files with 48 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
flight/Modules/Sensors/sensors.c
View File

@ -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++;
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;
gyro_accum[0] += gyro.gyro_x;
gyro_accum[1] += gyro.gyro_y;
gyro_accum[2] += gyro.gyro_z;
accel_accum[0] += mpu6000_data.accel_x;
accel_accum[1] += mpu6000_data.accel_y;
accel_accum[2] += mpu6000_data.accel_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);
gyro_samples ++;
accel_samples ++;
}
gyro_samples = count;
gyro_scaling = PIOS_MPU6000_GetScale();
accel_samples = count;
gyro_scaling = PIOS_MPU6000_GetScale();
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;

2
flight/PiOS/Common/pios_mpu6000.c
View File

@ -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);

39
flight/PiOS/STM32F4xx/pios_spi.c
View File

@ -196,14 +196,24 @@ 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:
* <UL>
* <LI>PIOS_SPI_PRESCALER_2: sets clock rate 27.7~ nS @ 72 MHz (36 MBit/s) (only supported for spi==0, spi1 uses 4 instead)
* <LI>PIOS_SPI_PRESCALER_4: sets clock rate 55.5~ nS @ 72 MHz (18 MBit/s)
* <LI>PIOS_SPI_PRESCALER_8: sets clock rate 111.1~ nS @ 72 MHz (9 MBit/s)
* <LI>PIOS_SPI_PRESCALER_16: sets clock rate 222.2~ nS @ 72 MHz (4.5 MBit/s)
* <LI>PIOS_SPI_PRESCALER_32: sets clock rate 444.4~ nS @ 72 MHz (2.25 MBit/s)
* <LI>PIOS_SPI_PRESCALER_64: sets clock rate 888.8~ nS @ 72 MHz (1.125 MBit/s)
* <LI>PIOS_SPI_PRESCALER_128: sets clock rate 1.7~ nS @ 72 MHz (0.562 MBit/s)
* <LI>PIOS_SPI_PRESCALER_256: sets clock rate 3.5~ nS @ 72 MHz (0.281 MBit/s)
* </UL>
* \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;
@ -222,13 +232,11 @@ int32_t PIOS_SPI_SetClockSpeed(uint32_t spi_id, SPIPrescalerTypeDef spi_prescale
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;

4
flight/Revolution/System/pios_board.c
View File

@ -179,6 +179,7 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
.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: