void mpu6500GyroInit(uint8_t lpf) { mpuIntExtiInit(); #ifdef NAZE // FIXME target specific code in driver code. gpio_config_t gpio; // MPU_INT output on rev5 hardware (PC13). rev4 was on PB13, conflicts with SPI devices if (hse_value == 12000000) { gpio.pin = Pin_13; gpio.speed = Speed_2MHz; gpio.mode = Mode_IN_FLOATING; gpioInit(GPIOC, &gpio); } #endif mpuIntExtiInit(); mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); delay(100); mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07); delay(100); mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0); delay(100); mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); delay(15); mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); delay(15); mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delay(15); mpuConfiguration.write(MPU_RA_CONFIG, lpf); delay(15); mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops delay(100); // Data ready interrupt configuration #ifdef USE_MPU9250_MAG mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN #else mpuConfiguration.write(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 #endif delay(15); #ifdef USE_MPU_DATA_READY_SIGNAL mpuConfiguration.write(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable #endif }
static void mpu6050GyroInit(uint16_t lpf) { bool ack; mpuIntExtiInit(); uint8_t mpuLowPassFilter = determineMPULPF(lpf); ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 delay(100); ack = 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 = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //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 = mpuConfiguration.write(MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) ack = 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 = mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); ack = 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 = mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); #endif UNUSED(ack); }
static void mpu6050AccInit(void) { mpuIntExtiInit(); switch (mpuDetectionResult.resolution) { case MPU_HALF_RESOLUTION: acc_1G = 256 * 8; break; case MPU_FULL_RESOLUTION: acc_1G = 512 * 8; break; } }
void mpu6000SpiGyroInit(uint8_t lpf) { mpuIntExtiInit(); mpu6000AccAndGyroInit(); mpuIntExtiInit(); spiResetErrorCounter(MPU6000_SPI_INSTANCE); 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) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) { spiResetErrorCounter(MPU6000_SPI_INSTANCE); failureMode(FAILURE_GYRO_INIT_FAILED); } }
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_CLOCK_INITIALIZATON); // Accel and Gyro DLPF Setting mpu6000WriteRegister(MPU6000_CONFIG, lpf); delayMicroseconds(1); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); // 18 MHz SPI clock int16_t data[3]; mpuGyroRead(data); if (((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) { failureMode(FAILURE_GYRO_INIT_FAILED); } }
void mpu9250SpiAccInit(void) { mpuIntExtiInit(); acc_1G = 512 * 8; }
void mpu6000SpiAccInit(void) { mpuIntExtiInit(); acc_1G = 512 * 4; }
void mpu6000SpiAccInit(acc_t *acc) { mpuIntExtiInit(); acc->acc_1G = 512 * 4; }
void mpu6500AccInit(void) { mpuIntExtiInit(); acc_1G = 512 * 8; }
void mpu6500AccInit(acc_t *acc) { mpuIntExtiInit(); acc->acc_1G = 512 * 4; }