void mpu6500GyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); delay(100); gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x07); delay(100); gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0); delay(100); gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); delay(15); const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, raGyroConfigData); delay(15); gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delay(15); gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf); delay(15); gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops delay(100); // Data ready interrupt configuration #ifdef USE_MPU9250_MAG gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN #else gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR #endif delay(15); #ifdef USE_MPU_DATA_READY_SIGNAL gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable #endif delay(15); }
static void mpu6050GyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); bool ack = gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 delay(100); ack = gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) ack = gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxCalculateDivider()); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure ack = gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) ack = gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec // ACC Init stuff. // Accel scale 8g (4096 LSB/g) ack = gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); ack = gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS #ifdef USE_MPU_DATA_READY_SIGNAL ack = gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); #endif UNUSED(ack); }
void icm20689GyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON); gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); delay(100); gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x03); delay(100); // gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0); // delay(100); gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); delay(15); const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData); delay(15); gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delay(15); gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); delay(15); gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops delay(100); // Data ready interrupt configuration // gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN delay(15); #ifdef USE_MPU_DATA_READY_SIGNAL gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable #endif spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_STANDARD); }