void mpu6050_init() {
mpu6050_write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
delay(100); // Startup time delay
// Wake Up device and select GyroZ clock (better performance)
mpu6050_write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
mpu6050_write_reg(MPUREG_PWR_MGMT_2, 0);
// SAMPLE RATE
mpu6050_write_reg(MPUREG_SMPLRT_DIV, 0x00); // Sample rate = 1kHz
// FS & DLPF FS=1000º/s, DLPF = 42Hz (low pass filter)
mpu6050_write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_42HZ);
mpu6050_write_reg(MPUREG_GYRO_CONFIG, BITS_FS_1000DPS); // Gyro scale 1000º/s
mpu6050_write_reg(MPUREG_ACCEL_CONFIG, 0x08); // Accel scale +-4g
if (mpu6050_test_connection()) {
Serial.println("Connected to MPU6050!");
} else {
for(;;) {
Serial.println("Unable to connect to MPU6050.");
delay(1000);
}
}
// calibrate_gyro();
// calibrate_accel();
}
/**
* @brief init mpu6050 work mode power mode
* @param None
* @retval TRUE FALSE
**/
bool mpu6050_init(void )
{
i2c_init();
Delayms(10);
if( mpu6050_check_connection() != TRUE )
{
printf("mpu6050_check_connection fail \n");
return FALSE;
}
mpu6050_write_reg(PWR_CFG_ADDR,0x00); //配置电源管理模式 退出休眠和循环模式 时钟源为陀螺仪的X轴
mpu6050_write_reg(SMPLING_RATE_ADDR, 0x04); //设置陀螺仪的采样率分频系数 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
mpu6050_write_reg(FILTER_CFG_ADDR,0x02); //采样数据和外部引脚同步废除 设置低通滤波截至频率，数字低通滤波设置为100HZ
mpu6050_write_reg(GYRO_CFG_ADDR ,3<<3); //陀螺仪的量程 为2000degree/s 16.4 LSB/degree/s
mpu6050_write_reg(ACCEL_CFG_ADDR ,2<<3); //加速度计量程 为+-8g 4096 LSB/g
mpu6050_write_reg(INT_PIN_CFG ,0x32); //中断信号为高电平，推挽输出，直到有读取操作中断信号才消失，直通辅助I2C
mpu6050_write_reg(INT_ENABLE ,0x1) ; //数据准备产生中断
mpu6050_write_reg(USER_CTRL,0x00) ; //不使用辅助I2C
return TRUE;
}