首先介绍一下mpu6050，反正就是挺好用的，自己百度吧，写多了看也看不懂。
重要的地方讲讲。首先是mpu6050配置
void set_gyro_registers(){
//Setup the MPU-6050
if(eeprom_data[31] == 1){
Wire.beginTransmission(gyro_address); //Start communication with the address found during search.
Wire.write(0x6B); //We want to write to the PWR_MGMT_1 register (6B hex)
Wire.write(0x00); //Set the register bits as 00000000 to activate the gyro
Wire.endTransmission(); //End the transmission with the gyro.
Wire.beginTransmission(gyro_address); //Start communication with the address found during search.
Wire.write(0x1B); //We want to write to the GYRO_CONFIG register (1B hex)
Wire.write(0x08); //Set the register bits as 00001000 (500dps full scale)
Wire.endTransmission(); //End the transmission with the gyro
Wire.beginTransmission(gyro_address); //Start communication with the address found during search.
Wire.write(0x1C); //We want to write to the ACCEL_CONFIG register (1A hex)
Wire.write(0x10); //Set the register bits as 00010000 (+/- 8g full scale range)
Wire.endTransmission(); //End the transmission with the gyro
//Let's perform a random register check to see if the values are written correct
Wire.beginTransmission(gyro_address); //Start communication with the address found during search
Wire.write(0x1B); //Start reading @ register 0x1B
Wire.endTransmission(); //End the transmission
Wire.requestFrom(gyro_address, 1); //Request 1 bytes from the gyro
while(Wire.available() < 1); //Wait until the 6 bytes are received
if(Wire.read() != 0x08){ //Check if the value is 0x08
digitalWrite(12,HIGH); //Turn on the warning led
while(1)delay(10); //Stay in this loop for ever
}
Wire.beginTransmission(gyro_address); //Start communication with the address found during search
Wire.write(0x1A); //We want to write to the CONFIG register (1A hex)
Wire.write(0x03); //Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz)
Wire.endTransmission(); //End the transmission with the gyro
}
}
简单地说，配置mpu6050使用内部晶振，陀螺仪量程500dps full scale，500dps陀螺仪转速为1dps时输出为65.5，这个数据查Datasheet,加速度计量程为+/- 8g。配置大概就这样子，让我们开始下面的程序设计。
我们采用互补滤波，对于mpu6050来说，加速度计对小车的加速度比较敏感，取瞬时值计算倾角误差比较大；而陀螺仪积分得到的角度不受加速度的影响，但随着时间的增加，积分漂移和温度漂移带来的误差比较大。加速度计要滤掉高频信号，简单地说就是因为小车震动产生的噪声，陀螺仪要滤掉低频信号，就是避免漂移。
总之：1.陀螺仪为基础的IMU不容易受到振动影响，时间长了它会漂移；2.加速度计为基础的IMU很容易被振动影响，把加速度计取平均可以消除振动的影响；3.最终的角度大部分取决于陀螺仪测得的角度，其中一小部分取决于加速度计测得的角度。这样就解决了我们存在的问题，思想就是互补滤波。
下面是代码部分：
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;很简单
接下来，我们看看如何读取MPU6050的原始数据：
//IMU读取机速度温度陀螺仪的原始数据
void gyro_signalen(){
Wire.beginTransmission(gyro_address); //Start communication with the gyro
Wire.write(0x43); //Start reading @ register 43h and auto increment with every read
Wire.endTransmission(); //End the transmission
Wire.requestFrom(gyro_address,6); //Request 6 bytes from the gyro
while(Wire.available() < 6); //Wait until the 6 bytes are received
gyro_roll=Wire.read()<<8|Wire.read(); //Read high and low part of the angular data
if(cal_int == 2000)gyro_roll -= gyro_roll_cal; //Only compensate after the calibration
gyro_pitch=Wire.read()<<8|Wire.read(); //Read high and low part of the angular data
if(cal_int == 2000)gyro_pitch -= gyro_pitch_cal; //Only compensate after the calibration
gyro_yaw=Wire.read()<<8|Wire.read(); //Read high and low part of the angular data
if(cal_int == 2000)gyro_yaw -= gyro_yaw_cal; //Only compensate after the calibration
Wire.beginTransmission(gyro_address); //Start communication with the gyro
Wire.write(0x3B); //Start reading @ register 43h and auto increment with every read
Wire.endTransmission(); //End the transmission
Wire.requestFrom(gyro_address,8); //Request 6 bytes from the gyro
while(Wire.available() < 6); //Wait until the 6 bytes are received
accx = Wire.read()<<8|Wire.read(); //Add the low and high byte to the acc_x variable.
if(cal_int == 2000)accx -= accx;
accy = Wire.read()<<8|Wire.read();
if(cal_int == 2000)accy -= accy;
accz = Wire.read()<<8|Wire.read();
if(cal_int == 2000)accz -= accz;
temperature = Wire.read()<<8|Wire.read();
if(cal_int == 2000)temperature -= temperature;
}
需要注意的是，陀螺仪每次使用前都要校准，首先算出平均的offset，然后再减去这个offset：
//采集2000个数据进行校准
for (cal_int = 0; cal_int < 2000 ; cal_int ++){ //Take 2000 readings for calibration.
if(cal_int % 100 == 0)Serial.print(F(".")); //Print dot to indicate calibration.
gyro_signalen(); //Read the gyro output.
gyro_roll_cal += gyro_roll; //Ad roll value to gyro_roll_cal.
gyro_pitch_cal += gyro_pitch; //Ad pitch value to gyro_pitch_cal.
gyro_yaw_cal += gyro_yaw; //Ad yaw value to gyro_yaw_cal.
delay(4); //Wait 3 milliseconds before the next loop.
}
//计算平均值
gyro_roll_cal /= 2000; //Divide the roll total by 2000.
gyro_pitch_cal /= 2000; //Divide the pitch total by 2000.
gyro_yaw_cal /= 2000; //Divide the yaw total by 2000.
//减去offset
参照上面写的读取原始数据部分。
将读取的陀螺仪原始数据/65.5就可以得到陀螺仪转动的dps，知道转动这些角度的时间就可以计算出陀螺仪的角度。
下面是程序运行的效果：
I2C clock speed is correctly set to 400kHz.
===================================================
Gyro calibration
===================================================
Don't move the quadcopter!! Calibration starts in 3 seconds
Calibrating the gyro, this will take +/- 8 seconds
Please wait....................
Axis 1 offset=-228.23
Axis 2 offset=93.73
Axis 3 offset=23.80
Yaw=-0.21
Yaw=0.03
Yaw=0.23
接下来我们开始使用加速度计计算mpu6050的倾斜角度：
我打算以250Hz的频率读取MPU6050的数据，然后计算角度。采用的基本思想就是积分。
//0.0000611 = 1 / (250Hz / 65.5)//1.9为修正系数
angle_pitch += gyro_pitch * 0.0000611*1.9; //Calculate the traveled pitch angle and add this to the angle_pitch variable.
angle_roll += gyro_roll * 0.0000611*1.9; //Calculate the traveled roll angle and add this to the angle_roll variable.
angle_yaw += gyro_yaw * 0.0000611*1.8;
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
angle_pitch -= angle_roll * sin(gyro_yaw * 0.000001066*1.8); //If the IMU has yawed transfer the roll angle to the pitch angel.
angle_roll += angle_pitch * sin(gyro_yaw * 0.000001066*1.8); //If the IMU has yawed transfer the pitch angle to the roll angel.
//Accelerometer angle calculations
acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z)); //Calculate the total accelerometer vector.
if(abs(acc_y) < acc_total_vector){ //Prevent the asin function to produce a NaN
angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; //Calculate the pitch angle.
}
if(abs(acc_x) < acc_total_vector){ //Prevent the asin function to produce a NaN
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296; //Calculate the roll angle.
}