rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

Some changes to MPU6000 driver to make it start more reliably on fresh powerup

Browse Source
This commit is contained in:
James Cotton 2011-11-26 14:12:12 -06:00
parent 1574c96c3a
commit 11ad135b27
2 changed files with 35 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
flight/PiOS/STM32F4xx/pios_mpu6000.c
View File

@ -48,7 +48,6 @@ static int16_t pios_mpu6000_buffer[PIOS_MPU6000_MAX_DOWNSAMPLE * sizeof(struct p
static t_fifo_buffer pios_mpu6000_fifo;
volatile bool mpu6000_configured = false;
volatile bool mpu6000_cb_ready = true;
static struct pios_mpu6000_cfg const * cfg;
@ -62,6 +61,9 @@ void PIOS_MPU6000_Init(const struct pios_mpu6000_cfg * new_cfg)
fifoBuf_init(&pios_mpu6000_fifo, (uint8_t *) pios_mpu6000_buffer, sizeof(pios_mpu6000_buffer));
/* Configure the MPU6050 Sensor */
PIOS_MPU6000_Config(cfg);
/* Configure EOC pin as input floating */
GPIO_Init(cfg->drdy.gpio, &cfg->drdy.init);
@ -72,8 +74,6 @@ void PIOS_MPU6000_Init(const struct pios_mpu6000_cfg * new_cfg)
/* Enable and set EOC EXTI Interrupt to the lowest priority */
NVIC_Init(&cfg->eoc_irq.init);
/* Configure the MPU6050 Sensor */
PIOS_MPU6000_Config(cfg);
}
/**
@ -84,38 +84,46 @@ void PIOS_MPU6000_Init(const struct pios_mpu6000_cfg * new_cfg)
*/
static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg)
{
mpu6000_cb_ready = true;
// Reset chip and fifo
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, 0x01 | 0x02 | 0x04) != 0);
PIOS_DELAY_WaituS(20);
// FIFO storage
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store) != 0);
// Sample rate divider
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_SMPLRT_DIV_REG, cfg->Smpl_rate_div) != 0) ;
// Digital low-pass filter and scale
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, cfg->filter) != 0) ;
// Digital low-pass filter and scale
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_GYRO_CFG_REG, cfg->gyro_range) != 0) ;
// Interrupt configuration
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, cfg->User_ctl) != 0) ;
// Interrupt configuration
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0) ;
// Wait for reset to finish
while (PIOS_MPU6000_GetReg(PIOS_MPU6000_USER_CTRL_REG) & 0x07);
//Power management configuration
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0) ;
// Interrupt configuration
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_CFG_REG, cfg->interrupt_cfg) != 0) ;
// Interrupt configuration
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0) ;
if(PIOS_MPU6000_GetReg(PIOS_MPU6000_INT_EN_REG) != cfg->interrupt_en)
return;
// FIFO storage
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store) != 0);
// Sample rate divider
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_SMPLRT_DIV_REG, cfg->Smpl_rate_div) != 0) ;
// Digital low-pass filter and scale
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, cfg->filter) != 0) ;
// Digital low-pass filter and scale
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_GYRO_CFG_REG, cfg->gyro_range) != 0) ;
// Interrupt configuration
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, cfg->User_ctl) != 0) ;
// Interrupt configuration
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0) ;
// Interrupt configuration
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_CFG_REG, cfg->interrupt_cfg) != 0) ;
// Interrupt configuration
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0) ;
if((PIOS_MPU6000_GetReg(PIOS_MPU6000_INT_EN_REG)) != cfg->interrupt_en)
return;
mpu6000_configured = true;
}

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

@ -175,7 +175,7 @@ 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_2,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8,
},
.use_crc = false,
.dma = {