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(); 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); } }
static void mpu6000AccAndGyroInit(gyroDev_t *gyro) { busDevice_t * busDev = gyro->busDev; const gyroFilterAndRateConfig_t * config = mpuChooseGyroConfig(gyro->lpf, 1000000 / gyro->requestedSampleIntervalUs); gyro->sampleRateIntervalUs = 1000000 / config->gyroRateHz; gyroIntExtiInit(gyro); busSetSpeed(busDev, BUS_SPEED_INITIALIZATION); // Device Reset busWrite(busDev, MPU_RA_PWR_MGMT_1, BIT_H_RESET); delay(150); busWrite(busDev, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP); delay(150); // Clock Source PPL with Z axis gyro reference busWrite(busDev, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); delayMicroseconds(15); // Disable Primary I2C Interface busWrite(busDev, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS); delayMicroseconds(15); busWrite(busDev, MPU_RA_PWR_MGMT_2, 0x00); delayMicroseconds(15); // Accel Sample Rate 1kHz // Gyroscope Output Rate = 1kHz when the DLPF is enabled busWrite(busDev, MPU_RA_SMPLRT_DIV, config->gyroConfigValues[1]); delayMicroseconds(15); // Gyro +/- 1000 DPS Full Scale busWrite(busDev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); delayMicroseconds(15); // Accel +/- 8 G Full Scale busWrite(busDev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delayMicroseconds(15); busWrite(busDev, 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 busWrite(busDev, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); delayMicroseconds(15); #endif // Accel and Gyro DLPF Setting busWrite(busDev, MPU_RA_CONFIG, config->gyroConfigValues[0]); delayMicroseconds(1); busSetSpeed(busDev, BUS_SPEED_FAST); mpuGyroRead(gyro); if (((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) { failureMode(FAILURE_GYRO_INIT_FAILED); } }