MPU6050的数据接口用的是I2C总线协议，因此我们须要Wire程序库的帮助来实现Arduino与MPU6050之间的通信。我们的Arduino编程环境中须要安装Wire库。
2 MPU6050的数据格式
我们感兴趣的数据位于0x3B到0x48这14个字节的寄存器中。这些数据会被动态更新，更新频率最高可达1000HZ。下面列出干系寄存器的地址，数据的名称。把稳，每个数据都是2个字节。
0x3B，加速度计的X轴分量ACC_X0x3D，加速度计的Y轴分量ACC_Y0x3F，加速度计的Z轴分量ACC_Z0x41，当前温度TEMP0x43，绕X轴旋转的角速率GYR_X0x45，绕Y轴旋转的角速率GYR_Y0x47，绕Z轴旋转的角速率GYR_Z
3 连线方法
//MPU-UNO
//VCC-VCC
//GND-GND
//SCL-A5
//SDA-A4
4 实物连线
5 程序
//连线方法//MPU-UNO//VCC-VCC//GND-GND//SCL-A5//SDA-A4#include <Kalman.h>#include <Wire.h>#include <Math.h>float fRad2Deg = 57.295779513f; //将弧度转为角度的乘数const int MPU = 0x68; //MPU-6050的I2C地址const int nValCnt = 7; //一次读取寄存器的数量const int nCalibTimes = 1000; //校定时读数的次数int calibData[nValCnt]; //校准数据unsigned long nLastTime = 0; //上一次读数的韶光float fLastRoll = 0.0f; //上一次滤波得到的Roll角float fLastPitch = 0.0f; //上一次滤波得到的Pitch角Kalman kalmanRoll; //Roll角滤波器Kalman kalmanPitch; //Pitch角滤波器void setup() { Serial.begin(9600); //初始化串口，指定波特率 Wire.begin(); //初始化Wire库 WriteMPUReg(0x6B, 0); //启动MPU6050设备 Calibration(); //实行校准 nLastTime = micros(); //记录当前韶光}void loop() { int readouts[nValCnt]; ReadAccGyr(readouts); //读出丈量值 float realVals[7]; Rectify(readouts, realVals); //根据校准的偏移量进行纠正 //打算加速度向量的模长，均以g为单位 float fNorm = sqrt(realVals[0] realVals[0] + realVals[1] realVals[1] + realVals[2] realVals[2]); float fRoll = GetRoll(realVals, fNorm); //打算Roll角 if (realVals[1] > 0) { fRoll = -fRoll; } float fPitch = GetPitch(realVals, fNorm); //打算Pitch角 if (realVals[0] < 0) { fPitch = -fPitch; } //打算两次丈量的韶光间隔dt，以秒为单位 unsigned long nCurTime = micros(); float dt = (double)(nCurTime - nLastTime) / 1000000.0; //对Roll角和Pitch角进行卡尔曼滤波 float fNewRoll = kalmanRoll.getAngle(fRoll, realVals[4], dt); float fNewPitch = kalmanPitch.getAngle(fPitch, realVals[5], dt); //跟据滤波值打算角度速 float fRollRate = (fNewRoll - fLastRoll) / dt; float fPitchRate = (fNewPitch - fLastPitch) / dt; //更新Roll角和Pitch角 fLastRoll = fNewRoll; fLastPitch = fNewPitch; //更新本次测的韶光 nLastTime = nCurTime; //向串口打印输出Roll角和Pitch角，运行时在Arduino的串口监视器中查看 Serial.print("Roll:"); Serial.print(fNewRoll); Serial.print('('); Serial.print(fRollRate); Serial.print("),\tPitch:"); Serial.print(fNewPitch); Serial.print('('); Serial.print(fPitchRate); Serial.print(")\n"); delay(10);}//向MPU6050写入一个字节的数据//指定寄存器地址与一个字节的值void WriteMPUReg(int nReg, unsigned char nVal) { Wire.beginTransmission(MPU); Wire.write(nReg); Wire.write(nVal); Wire.endTransmission(true);}//从MPU6050读出一个字节的数据//指定寄存器地址，返回读出的值unsigned char ReadMPUReg(int nReg) { Wire.beginTransmission(MPU); Wire.write(nReg); Wire.requestFrom(MPU, 1, true); Wire.endTransmission(true); return Wire.read();}//从MPU6050读出加速度计三个分量、温度和三个角速率计//保存在指定的数组中void ReadAccGyr(int pVals) { Wire.beginTransmission(MPU); Wire.write(0x3B); Wire.requestFrom(MPU, nValCnt 2, true); Wire.endTransmission(true); for (long i = 0; i < nValCnt; ++i) { pVals[i] = Wire.read() << 8 | Wire.read(); }}//对大量读数进行统计，校准均匀偏移量void Calibration(){ float valSums[7] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0}; //先求和 for (int i = 0; i < nCalibTimes; ++i) { int mpuVals[nValCnt]; ReadAccGyr(mpuVals); for (int j = 0; j < nValCnt; ++j) { valSums[j] += mpuVals[j]; } } //再求均匀 for (int i = 0; i < nValCnt; ++i) { calibData[i] = int(valSums[i] / nCalibTimes); } calibData[2] += 16384; //设芯片Z轴竖直向下，设定静态事情点。}//算得Roll角。算法见文档。float GetRoll(float pRealVals, float fNorm) { float fNormXZ = sqrt(pRealVals[0] pRealVals[0] + pRealVals[2] pRealVals[2]); float fCos = fNormXZ / fNorm; return acos(fCos) fRad2Deg;}//算得Pitch角。算法见文档。float GetPitch(float pRealVals, float fNorm) { float fNormYZ = sqrt(pRealVals[1] pRealVals[1] + pRealVals[2] pRealVals[2]); float fCos = fNormYZ / fNorm; return acos(fCos) fRad2Deg;}//对读数进行纠正，肃清偏移，并转换为物理量。公式见文档。void Rectify(int pReadout, float pRealVals) { for (int i = 0; i < 3; ++i) { pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 16384.0f; } pRealVals[3] = pReadout[3] / 340.0f + 36.53; for (int i = 4; i < 7; ++i) { pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 131.0f; }}
6 验证数据
