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; }
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; }