/*
* @brief Read the identification bytes from the MPU6000 sensor
* \return ID read from MPU6000 or -1 if failure
*/
static int32_t PIOS_MPU6000_ReadID()
{
int32_t mpu6000_id = PIOS_MPU6000_GetReg(PIOS_MPU60X0_WHOAMI);
if (mpu6000_id < 0)
return -1;
return mpu6000_id;
}
/*
* @brief Read the identification bytes from the MPU6000 sensor
* \return ID read from MPU6000 or -1 if failure
*/
int32_t PIOS_MPU6000_ReadID()
{
int32_t mpu6000_id = PIOS_MPU6000_GetReg(PIOS_MPU6000_WHOAMI);
if (mpu6000_id < 0) {
return -1;
}
return mpu6000_id;
}
/**
* @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;
}