/**
* @brief Configures Gyro, accel and Filter ranges/setings
* @return 0 if successful, -1 if device has not been initialized
*/
int32_t PIOS_MPU6000_ConfigureRanges(
enum pios_mpu6000_range gyroRange,
enum pios_mpu6000_accel_range accelRange,
enum pios_mpu6000_filter filterSetting)
{
if (dev == NULL) {
return -1;
}
// TODO: check that changing the SPI clock speed is safe
// to do here given that interrupts are enabled and the bus has
// not been claimed/is not claimed during this call
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
// update filter settings
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0) {
;
}
// Sample rate divider, chosen upon digital filtering settings
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_SMPLRT_DIV_REG,
filterSetting == PIOS_MPU6000_LOWPASS_256_HZ ?
dev->cfg->Smpl_rate_div_no_dlp : dev->cfg->Smpl_rate_div_dlp) != 0) {
;
}
dev->filter = filterSetting;
// Gyro range
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_GYRO_CFG_REG, gyroRange) != 0) {
;
}
dev->gyro_range = gyroRange;
#if defined(PIOS_MPU6000_ACCEL)
// Set the accel range
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, accelRange) != 0) {
;
}
dev->accel_range = accelRange;
#endif
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
return 0;
}
/**
* Set the gyro range and store it locally for scaling
*/
void PIOS_MPU6000_SetGyroRange(enum pios_mpu60x0_range gyro_range)
{
PIOS_MPU6000_SetReg(PIOS_MPU60X0_GYRO_CFG_REG, gyro_range);
switch(gyro_range) {
case PIOS_MPU60X0_SCALE_250_DEG:
PIOS_SENSORS_SetMaxGyro(250);
break;
case PIOS_MPU60X0_SCALE_500_DEG:
PIOS_SENSORS_SetMaxGyro(500);
break;
case PIOS_MPU60X0_SCALE_1000_DEG:
PIOS_SENSORS_SetMaxGyro(1000);
break;
case PIOS_MPU60X0_SCALE_2000_DEG:
PIOS_SENSORS_SetMaxGyro(2000);
break;
}
pios_mpu6000_dev->gyro_range = gyro_range;
}
/**
* Set the sample rate in Hz by determining the nearest divisor
* @param[in] sample rate in Hz
*/
void PIOS_MPU6000_SetSampleRate(uint16_t samplerate_hz)
{
uint16_t filter_frequency = 8000;
if (pios_mpu6000_dev->filter != PIOS_MPU60X0_LOWPASS_256_HZ)
filter_frequency = 1000;
// limit samplerate to filter frequency
if (samplerate_hz > filter_frequency)
samplerate_hz = filter_frequency;
// calculate divisor, round to nearest integeter
int32_t divisor = (int32_t)(((float)filter_frequency / samplerate_hz) + 0.5f) - 1;
// limit resulting divisor to register value range
if (divisor < 0)
divisor = 0;
if (divisor > 0xff)
divisor = 0xff;
PIOS_MPU6000_SetReg(PIOS_MPU60X0_SMPLRT_DIV_REG, (uint8_t)divisor);
}
/**
* Configure the digital low-pass filter
*/
void PIOS_MPU6000_SetLPF(enum pios_mpu60x0_filter filter)
{
PIOS_MPU6000_SetReg(PIOS_MPU60X0_DLPF_CFG_REG, filter);
pios_mpu6000_dev->filter = filter;
}
void PIOS_MPU6000_SetAccelRange(enum pios_mpu60x0_accel_range accel_range)
{
PIOS_MPU6000_SetReg(PIOS_MPU60X0_ACCEL_CFG_REG, accel_range);
pios_mpu6000_dev->accel_range = accel_range;
}
/**
* @brief Initialize the MPU6000 3-axis gyro sensor
* \return none
* \param[in] PIOS_MPU6000_ConfigTypeDef struct to be used to configure sensor.
*
*/
static void PIOS_MPU6000_Config(const struct pios_mpu60x0_cfg *cfg)
{
#if defined(PIOS_MPU6000_SIMPLE_INIT_SEQUENCE)
// Reset chip registers
PIOS_MPU6000_SetReg(PIOS_MPU60X0_PWR_MGMT_REG, PIOS_MPU60X0_PWRMGMT_IMU_RST);
// Reset sensors signal path
PIOS_MPU6000_SetReg(PIOS_MPU60X0_USER_CTRL_REG, PIOS_MPU60X0_USERCTL_GYRO_RST);
// Give chip some time to initialize
PIOS_DELAY_WaitmS(10);
//Power management configuration
PIOS_MPU6000_SetReg(PIOS_MPU60X0_PWR_MGMT_REG, cfg->Pwr_mgmt_clk);
// User control
PIOS_MPU6000_SetReg(PIOS_MPU60X0_USER_CTRL_REG, cfg->User_ctl);
// Digital low-pass filter and scale
// set this before sample rate else sample rate calculation will fail
PIOS_MPU6000_SetLPF(cfg->default_filter);
// Sample rate
PIOS_MPU6000_SetSampleRate(cfg->default_samplerate);
// Set the gyro scale
PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG);
#if defined(PIOS_MPU6000_ACCEL)
// Set the accel scale
PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_8G);
#endif /* PIOS_MPU6000_ACCEL */
// Interrupt configuration
PIOS_MPU6000_SetReg(PIOS_MPU60X0_INT_CFG_REG, cfg->interrupt_cfg);
// Interrupt enable
PIOS_MPU6000_SetReg(PIOS_MPU60X0_INT_EN_REG, cfg->interrupt_en);
#else /* PIOS_MPU6000_SIMPLE_INIT_SEQUENCE */
/* This init sequence should really be dropped in favor of something
* less redundant but it seems to be hard to get it running well
* on all different targets.
*/
PIOS_MPU6000_ClaimBus();
PIOS_DELAY_WaitmS(1);
PIOS_MPU6000_ReleaseBus();
PIOS_DELAY_WaitmS(10);
// Reset chip
PIOS_MPU6000_SetReg(PIOS_MPU60X0_PWR_MGMT_REG, 0x80 | cfg->Pwr_mgmt_clk);
do {
PIOS_DELAY_WaitmS(5);
} while (PIOS_MPU6000_GetReg(PIOS_MPU60X0_PWR_MGMT_REG) & 0x80);
PIOS_DELAY_WaitmS(25);
// Reset chip and fifo
PIOS_MPU6000_SetReg(PIOS_MPU60X0_USER_CTRL_REG, 0x80 | 0x01 | 0x02 | 0x04);
do {
PIOS_DELAY_WaitmS(5);
} while (PIOS_MPU6000_GetReg(PIOS_MPU60X0_USER_CTRL_REG) & 0x07);
PIOS_DELAY_WaitmS(25);
//Power management configuration
PIOS_MPU6000_SetReg(PIOS_MPU60X0_PWR_MGMT_REG, cfg->Pwr_mgmt_clk);
// Interrupt configuration
PIOS_MPU6000_SetReg(PIOS_MPU60X0_INT_CFG_REG, cfg->interrupt_cfg);
// Interrupt configuration
PIOS_MPU6000_SetReg(PIOS_MPU60X0_INT_EN_REG, cfg->interrupt_en);
#if defined(PIOS_MPU6000_ACCEL)
// Set the accel scale
PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_8G);
#endif
// Digital low-pass filter and scale
// set this before sample rate else sample rate calculation will fail
PIOS_MPU6000_SetLPF(cfg->default_filter);
// Sample rate
PIOS_MPU6000_SetSampleRate(cfg->default_samplerate);
// Set the gyro scale
PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG);
// Interrupt configuration
PIOS_MPU6000_SetReg(PIOS_MPU60X0_USER_CTRL_REG, cfg->User_ctl);
//Power management configuration
PIOS_MPU6000_SetReg(PIOS_MPU60X0_PWR_MGMT_REG, cfg->Pwr_mgmt_clk);
// Interrupt configuration
PIOS_MPU6000_SetReg(PIOS_MPU60X0_INT_CFG_REG, cfg->interrupt_cfg);
// Interrupt configuration
PIOS_MPU6000_SetReg(PIOS_MPU60X0_INT_EN_REG, cfg->interrupt_en);
#endif /* PIOS_MPU6000_SIMPLE_INIT_SEQUENCE */
pios_mpu6000_dev->configured = true;
}
/**
* @brief Initialize the MPU6000 3-axis gyro sensor
* \return none
* \param[in] PIOS_MPU6000_ConfigTypeDef struct to be used to configure sensor.
*
*/
static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg)
{
PIOS_MPU6000_Test();
// Reset chip
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, PIOS_MPU6000_PWRMGMT_IMU_RST) != 0) {
;
}
PIOS_DELAY_WaitmS(50);
// Reset chip and fifo
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG,
PIOS_MPU6000_USERCTL_GYRO_RST |
PIOS_MPU6000_USERCTL_SIG_COND |
PIOS_MPU6000_USERCTL_FIFO_RST) != 0) {
;
}
// Wait for reset to finish
while (PIOS_MPU6000_GetReg(PIOS_MPU6000_USER_CTRL_REG) &
(PIOS_MPU6000_USERCTL_GYRO_RST |
PIOS_MPU6000_USERCTL_SIG_COND |
PIOS_MPU6000_USERCTL_FIFO_RST)) {
;
}
PIOS_DELAY_WaitmS(10);
// 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) {
;
}
// FIFO storage
#if defined(PIOS_MPU6000_ACCEL)
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store | PIOS_MPU6000_ACCEL_OUT) != 0) {
;
}
#else
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store) != 0) {
;
}
#endif
PIOS_MPU6000_ConfigureRanges(cfg->gyro_range, cfg->accel_range, cfg->filter);
// 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;
}
