bool mpu6000SpiGyroDetect(const mpu6000Config_t *configToUse, gyro_t *gyro, uint16_t lpf) { mpu6000Config = configToUse; if (!mpu6000SpiDetect()) { return false; } spiResetErrorCounter(MPU6000_SPI_INSTANCE); mpu6000AccAndGyroInit(); uint8_t mpuLowPassFilter = BITS_DLPF_CFG_42HZ; int16_t data[3]; // default lpf is 42Hz if (lpf == 256) mpuLowPassFilter = BITS_DLPF_CFG_256HZ; else if (lpf >= 188) mpuLowPassFilter = BITS_DLPF_CFG_188HZ; else if (lpf >= 98) mpuLowPassFilter = BITS_DLPF_CFG_98HZ; else if (lpf >= 42) mpuLowPassFilter = BITS_DLPF_CFG_42HZ; else if (lpf >= 20) mpuLowPassFilter = BITS_DLPF_CFG_20HZ; else if (lpf >= 10) mpuLowPassFilter = BITS_DLPF_CFG_10HZ; else if (lpf > 0) mpuLowPassFilter = BITS_DLPF_CFG_5HZ; else mpuLowPassFilter = BITS_DLPF_CFG_256HZ; spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); // Determine the new sample divider mpu6000WriteRegister(MPU6000_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); delayMicroseconds(1); // Accel and Gyro DLPF Setting mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter); delayMicroseconds(1); mpu6000SpiGyroRead(data); if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) { spiResetErrorCounter(MPU6000_SPI_INSTANCE); return false; } gyro->init = mpu6000SpiGyroInit; gyro->read = mpu6000SpiGyroRead; gyro->intStatus = checkMPU6000DataReady; // 16.4 dps/lsb scalefactor gyro->scale = 1.0f / 16.4f; //gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f; delay(100); return true; }
static void mpu6000AccAndGyroInit(uint8_t lpf) { if (mpuSpi6000InitDone) { return; } spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_SLOW_CLOCK); //low speed for writing to slow registers mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET); delay(50); mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET); delay(50); verifympu6000WriteRegister(MPU_RA_PWR_MGMT_1, 0x0B); //temp sensor disabled Z axis is timer // Disable Primary I2C Interface mpu6000WriteRegister(MPU_RA_USER_CTRL, BIT_I2C_IF_DIS|5); verifympu6000WriteRegister(MPU_RA_PWR_MGMT_2, 0x00); // Accel Sample Rate 1kHz // Gyroscope Output Rate = 1kHz when the DLPF is enabled verifympu6000WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Gyro +/- 1000 DPS Full Scale verifympu6000WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); // Accel +/- 8 G Full Scale verifympu6000WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); verifympu6000WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR #if defined(USE_MPU_DATA_READY_SIGNAL) mpu6000WriteRegister(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly. delayMicroseconds(100); verifympu6000WriteRegister(MPU_RA_PWR_MGMT_1, 0x0B); //need to redo MPU_RA_PWR_MGMT_1 verifympu6000WriteRegister(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); //should work correctly this time #endif // Accel and Gyro DLPF Setting if (lpf == 4) { verifympu6000WriteRegister(MPU6000_CONFIG, 1); //1KHz, 188DLPF } else if (lpf < 4) { verifympu6000WriteRegister(MPU6000_CONFIG, 7); //8KHz, Raw } else if (lpf > 4) { verifympu6000WriteRegister(MPU6000_CONFIG, 0); //8KHz, 256DLPF } // Clock Source PPL with Z axis gyro reference verifympu6000WriteRegister(MPU_RA_PWR_MGMT_1, 0x09); mpuSpi6000InitDone = true; //init done }
static void mpu6000AccAndGyroInit(void) { if (mpuSpi6000InitDone) { return; } spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); // Device Reset mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET); delay(150); mpu6000WriteRegister(MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP); delay(150); // Clock Source PPL with Z axis gyro reference mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); delayMicroseconds(15); // Disable Primary I2C Interface mpu6000WriteRegister(MPU_RA_USER_CTRL, BIT_I2C_IF_DIS); delayMicroseconds(15); mpu6000WriteRegister(MPU_RA_PWR_MGMT_2, 0x00); delayMicroseconds(15); // Accel Sample Rate 1kHz // Gyroscope Output Rate = 1kHz when the DLPF is enabled mpu6000WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); delayMicroseconds(15); // Gyro +/- 1000 DPS Full Scale mpu6000WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); delayMicroseconds(15); // Accel +/- 8 G Full Scale mpu6000WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delayMicroseconds(15); mpu6000WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR delayMicroseconds(15); #ifdef USE_MPU_DATA_READY_SIGNAL mpu6000WriteRegister(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); delayMicroseconds(15); #endif spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); // 18 MHz SPI clock delayMicroseconds(1); mpuSpi6000InitDone = true; }
bool verifympu6000WriteRegister(uint8_t reg, uint8_t data) { uint8_t in; uint8_t attemptsRemaining = 20; mpu6000WriteRegister(reg, data); delayMicroseconds(100); do { mpu6000SlowReadRegister(reg, 1, &in); if (in == data) { return true; } else { mpu6000WriteRegister(reg, data); delayMicroseconds(100); } } while (attemptsRemaining--); return false; }
void mpu6000AccAndGyroInit(void) { if (mpuSpi6000InitDone) { return; } spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); // Device Reset mpu6000WriteRegister(MPU6000_PWR_MGMT_1, BIT_H_RESET); delay(150); mpu6000WriteRegister(MPU6000_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP); delay(150); // Clock Source PPL with Z axis gyro reference mpu6000WriteRegister(MPU6000_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); delayMicroseconds(1); // Disable Primary I2C Interface mpu6000WriteRegister(MPU6000_USER_CTRL, BIT_I2C_IF_DIS); delayMicroseconds(1); mpu6000WriteRegister(MPU6000_PWR_MGMT_2, 0x00); delayMicroseconds(1); // Accel Sample Rate 1kHz // Gyroscope Output Rate = 1kHz when the DLPF is enabled mpu6000WriteRegister(MPU6000_SMPLRT_DIV, 0x00); delayMicroseconds(1); // Accel +/- 8 G Full Scale mpu6000WriteRegister(MPU6000_ACCEL_CONFIG, BITS_FS_8G); delayMicroseconds(1); // Gyro +/- 1000 DPS Full Scale mpu6000WriteRegister(MPU6000_GYRO_CONFIG, BITS_FS_2000DPS); delayMicroseconds(1); #ifdef USE_MPU_DATA_READY_SIGNAL // Set MPU Data Ready Signal mpu6000WriteRegister(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); delayMicroseconds(1); #endif mpuSpi6000InitDone = true; }
bool mpu6000SpiDetect(void) { uint8_t in; uint8_t attemptsRemaining = 5; #ifdef MPU6000_CS_PIN mpuSpi6000CsPin = IOGetByTag(IO_TAG(MPU6000_CS_PIN)); #endif IOInit(mpuSpi6000CsPin, OWNER_MPU, RESOURCE_SPI_CS, 0); IOConfigGPIO(mpuSpi6000CsPin, SPI_IO_CS_CFG); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET); do { delay(150); mpu6000ReadRegister(MPU_RA_WHO_AM_I, 1, &in); if (in == MPU6000_WHO_AM_I_CONST) { break; } if (!attemptsRemaining) { return false; } } while (attemptsRemaining--); mpu6000ReadRegister(MPU_RA_PRODUCT_ID, 1, &in); /* look for a product ID we recognise */ // verify product revision switch (in) { case MPU6000ES_REV_C4: case MPU6000ES_REV_C5: case MPU6000_REV_C4: case MPU6000_REV_C5: case MPU6000ES_REV_D6: case MPU6000ES_REV_D7: case MPU6000ES_REV_D8: case MPU6000_REV_D6: case MPU6000_REV_D7: case MPU6000_REV_D8: case MPU6000_REV_D9: case MPU6000_REV_D10: return true; } return false; }
bool mpu6000SpiDetect(void) { uint8_t in; uint8_t attemptsRemaining = 5; if (mpuSpi6000InitDone) { return true; } spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); mpu6000WriteRegister(MPU6000_PWR_MGMT_1, BIT_H_RESET); do { delay(150); mpu6000ReadRegister(MPU6000_WHOAMI, &in, 1); if (in == MPU6000_WHO_AM_I_CONST) { break; } if (!attemptsRemaining) { return false; } } while (attemptsRemaining--); mpu6000ReadRegister(MPU6000_PRODUCT_ID, &in, 1); /* look for a product ID we recognise */ // verify product revision switch (in) { case MPU6000ES_REV_C4: case MPU6000ES_REV_C5: case MPU6000_REV_C4: case MPU6000_REV_C5: case MPU6000ES_REV_D6: case MPU6000ES_REV_D7: case MPU6000ES_REV_D8: case MPU6000_REV_D6: case MPU6000_REV_D7: case MPU6000_REV_D8: case MPU6000_REV_D9: case MPU6000_REV_D10: return true; } return false; }
bool mpu6000SpiDetect(void) { uint8_t in; uint8_t attemptsRemaining = 5; spiSetDivisor(MPU6000_SPI_INSTANCE, LOW_SPEED_SPI); mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET); return true; do { delay(150); mpu6000ReadRegister(MPU_RA_WHO_AM_I, 1, &in); if (in == MPU6000_WHO_AM_I_CONST) { break; } if (!attemptsRemaining) { return false; } } while (attemptsRemaining--); mpu6000ReadRegister(MPU_RA_PRODUCT_ID, 1, &in); /* look for a product ID we recognise */ // verify product revision switch (in) { case MPU6000ES_REV_C4: case MPU6000ES_REV_C5: case MPU6000_REV_C4: case MPU6000_REV_C5: case MPU6000ES_REV_D6: case MPU6000ES_REV_D7: case MPU6000ES_REV_D8: case MPU6000_REV_D6: case MPU6000_REV_D7: case MPU6000_REV_D8: case MPU6000_REV_D9: case MPU6000_REV_D10: return true; } return false; }
void mpu6000SpiGyroInit(uint8_t lpf) { mpuIntExtiInit(); mpu6000AccAndGyroInit(); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); // Accel and Gyro DLPF Setting mpu6000WriteRegister(MPU6000_CONFIG, lpf); delayMicroseconds(1); int16_t data[3]; mpuGyroRead(data); if (((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) { failureMode(FAILURE_GYRO_INIT_FAILED); } }
void mpu6000SpiGyroInit(uint8_t lpf) { mpuIntExtiInit(); mpu6000AccAndGyroInit(); spiResetErrorCounter(MPU6000_SPI_INSTANCE); spiSetDivisor(MPU6000_SPI_INSTANCE, LOW_SPEED_SPI); // Accel and Gyro DLPF Setting mpu6000WriteRegister(MPU6000_CONFIG, lpf); delayMicroseconds(1); int16_t data[3]; mpuGyroRead(data); if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) { spiResetErrorCounter(MPU6000_SPI_INSTANCE); failureMode(FAILURE_GYRO_INIT_FAILED); } }
void resetGyro (void) { // Device Reset mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET); delay(150); }
bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf) { if (!mpu6000SpiDetect()) { return false; } spiResetErrorCounter(MPU6000_SPI_INSTANCE); mpu6000AccAndGyroInit(); uint8_t mpuLowPassFilter = BITS_DLPF_CFG_42HZ; int16_t data[3]; // default lpf is 42Hz switch (lpf) { case 256: mpuLowPassFilter = BITS_DLPF_CFG_256HZ; break; case 188: mpuLowPassFilter = BITS_DLPF_CFG_188HZ; break; case 98: mpuLowPassFilter = BITS_DLPF_CFG_98HZ; break; default: case 42: mpuLowPassFilter = BITS_DLPF_CFG_42HZ; break; case 20: mpuLowPassFilter = BITS_DLPF_CFG_20HZ; break; case 10: mpuLowPassFilter = BITS_DLPF_CFG_10HZ; break; case 5: mpuLowPassFilter = BITS_DLPF_CFG_5HZ; break; case 0: mpuLowPassFilter = BITS_DLPF_CFG_2100HZ_NOLPF; break; } spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); // Accel and Gyro DLPF Setting mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter); delayMicroseconds(1); mpu6000SpiGyroRead(data); if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) { spiResetErrorCounter(MPU6000_SPI_INSTANCE); return false; } gyro->init = mpu6000SpiGyroInit; gyro->read = mpu6000SpiGyroRead; // 16.4 dps/lsb scalefactor gyro->scale = 1.0f / 16.4f; //gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f; delay(100); return true; }