/* * @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; }