void Calculate_Gyro_Bias(void) { static unsigned int i = 0; static unsigned int j = 0; int temp_gyro_rate; long temp_gyro_angle; int temp_gyro_bias; static char done=0; if(done==0) { i++; j++; // this will rollover every ~1000 seconds if(j == 10) { printf("\rCalculating Gyro Bias..."); } if(j == 60) { // start a gyro bias calculation Start_Gyro_Bias_Calc(); } if(j == 300) { // terminate the gyro bias calculation Stop_Gyro_Bias_Calc(); // reset the gyro heading angle Reset_Gyro_Angle(); printf("Done\r"); } if(i >= 30 && j >= 300) { temp_gyro_bias = Get_Gyro_Bias(); temp_gyro_rate = Get_Gyro_Rate(); temp_gyro_angle = Get_Gyro_Angle(); printf(" Gyro Bias=%d\r\n", temp_gyro_bias); printf(" Gyro Rate=%d\r\n", temp_gyro_rate); printf("Gyro Angle=%d\r\n\r\n", (int)temp_gyro_angle); i = 0; done = 1; } } }
// 加速度计与陀螺仪初始化 void MPU6050_Init(void) { // 解除休眠状态 I2C_WriteByte(MPU6050_Addr, PWR_MGMT_1, 0x01); // 设置采样频率为1KHZ I2C_WriteByte(MPU6050_Addr, SMPLRT_DIV, 0x00); // 设置低通滤波的带宽为5HZ I2C_WriteByte(MPU6050_Addr, CONFIG, 0x06); // 开启旁路I2C I2C_WriteByte(MPU6050_Addr, INT_PIN_CFG, 0x42); // 打开FIFO操作 I2C_WriteByte(MPU6050_Addr, USER_CTRL, 0x40); // 设置陀螺仪采集范围为+-500°/s I2C_WriteByte(MPU6050_Addr, GYRO_CONFIG, 0x0B); // 设置加速度计采集范围为+-4g I2C_WriteByte(MPU6050_Addr, ACCEL_CONFIG, 0x08); // 获取零漂偏移量 Get_Gyro_Bias(); Get_Accel_Bias(); }