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; }
void mpu6000SpiGyroInit(uint8_t lpf) { debug[3]++; mpuIntExtiInit(); mpu6000AccAndGyroInit(lpf); spiResetErrorCounter(MPU6000_SPI_INSTANCE); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_FAST_CLOCK); //high speed now that we don't need to write to the slow registers 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 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); } }
bool mpu6000SpiAccDetect(acc_t *acc) { if (!mpu6000SpiDetect()) { return false; } spiResetErrorCounter(MPU6000_SPI_INSTANCE); mpu6000AccAndGyroInit(); acc->init = mpu6000SpiAccInit; acc->read = mpu6000SpiAccRead; delay(100); return true; }
bool hmc5983DetectSpi(sensor_t *mag) { int16_t data[3]; int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow. spiResetErrorCounter(); uint8_t hmc5983Status = 0; uint8_t i, calibrationSteps = 2; setSPIdivisor( 8); // 4.5 MHz SPI clock ENABLE_HMC5983; spiTransferByte(HMC5983_CONFIG_REG_A); spiTransferByte(TS | SENSOR_CONFIG | POSITIVE_BIAS_CONFIGURATION); DISABLE_HMC5983; delay(50); ENABLE_HMC5983; spiTransferByte(HMC5983_CONFIG_REG_B); spiTransferByte(SENSOR_INIT_GAIN); DISABLE_HMC5983; delay(20); readHMC5983(data); for (i = 0; i < 10; i++) { ENABLE_HMC5983; spiTransferByte(HMC5983_MODE_REG); spiTransferByte(OP_MODE_SINGLE); DISABLE_HMC5983; delay(20); while ((hmc5983Status && STATUS_RDY) == 0x00) { ENABLE_HMC5983; spiTransferByte(HMC5983_STATUS_REG + 0x80); hmc5983Status = spiTransferByte(0x00); DISABLE_HMC5983; } if (!readHMC5983(data)) // check for valid data break; // breaks out of the for loop if sensor saturated. xyz_total[X] += data[X]; xyz_total[Y] += data[Y]; xyz_total[Z] += data[Z]; } if (i == 10) // loop completed calibrationSteps--; ENABLE_HMC5983; spiTransferByte(HMC5983_CONFIG_REG_A); spiTransferByte(SENSOR_CONFIG | NEGATIVE_BIAS_CONFIGURATION); DISABLE_HMC5983; delay(20); readHMC5983(data); for (i = 0; i < 10; i++) { ENABLE_HMC5983; spiTransferByte(HMC5983_MODE_REG); spiTransferByte(OP_MODE_SINGLE); DISABLE_HMC5983; delay(20); while ((hmc5983Status && STATUS_RDY) == 0x00) { ENABLE_HMC5983; spiTransferByte(HMC5983_STATUS_REG + 0x80); hmc5983Status = spiTransferByte(0x00); DISABLE_HMC5983; } if (!readHMC5983(data)) break; xyz_total[X] += data[X]; xyz_total[Y] += data[Y]; xyz_total[Z] += data[Z]; } if (i == 10) // loop completed calibrationSteps--; ENABLE_HMC5983; spiTransferByte(HMC5983_CONFIG_REG_A); spiTransferByte(TS | SENSOR_CONFIG | NORMAL_MEASUREMENT_CONFIGURATION); DISABLE_HMC5983; ENABLE_HMC5983; spiTransferByte(HMC5983_CONFIG_REG_B); spiTransferByte(SENSOR_GAIN); DISABLE_HMC5983; delay(50); ENABLE_HMC5983; spiTransferByte(HMC5983_MODE_REG); spiTransferByte(OP_MODE_CONTINUOUS); DISABLE_HMC5983; delay(20); readHMC5983(data); if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter() != 0) { spiResetErrorCounter(); return false; } // magGain[X] = fabsf(660.0f * HMC5983_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]); // magGain[Y] = fabsf(660.0f * HMC5983_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]); // magGain[Z] = fabsf(660.0f * HMC5983_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]); mag->init = hmc5983Init; mag->read = readHMC5983; return (calibrationSteps == 0); }
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; }