Exemple #1
0
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;
        }
    }

}
Exemple #2
0
// 加速度计与陀螺仪初始化
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();
}