小四轴实验：MPU6050模块使用

参考文章：http://www.javashuo.com/article/p-fnjouxrd-nt.html

MPU6050模块以下图：

该模块测量的是三轴的角速度和角度量，三轴是指Roll、Pitch、Yaw，以下图：

这些信息咱们能够用来做为控制无人机的反馈信息，经过控制算法，最终使无人机的姿态（即MPU6050测得的数据）知足咱们须要的值，比方说三轴角速度为零，悬停时Roll=0，Pitch=0，Yaw=初始值。

模块与Arduino连线以下：

Andrino读取代码

mpu6050.h

#ifndef _MPU6050_H_
#define _MPU6050_H_
#include <Arduino.h>
#include <Kalman.h>
#include <Wire.h>
#include <Math.h>

extern float fLastRoll;
extern float fRollRate;//单位：°/ms
extern float fLastPitch;
extern float fPitchRate;//单位：°/ms
  
void mpu6050_dsp(void);//初始化
void mpu6050_read(long dT_us);//读取mpu6050数据

void WriteMPUReg(int nReg, unsigned char nVal);
unsigned char ReadMPUReg(int nReg);
void ReadAccGyr(int *pVals);
void Calibration();
float GetRoll(float *pRealVals, float fNorm);
float GetPitch(float *pRealVals, float fNorm);
float GetYaw(float *pRealVals, float fNorm);
void Rectify(int *pReadout, float *pRealVals);
#endif

mpu6050.cpp

#include <Kalman.h>
#include <Wire.h>
#include <Math.h>
#include <Arduino.h>
#include "mpu6050.h"

float fRad2Deg = 57.295779513f; //将弧度转为角度的乘数
const int MPU = 0x68; //MPU-6050的I2C地址
const int nValCnt = 7; //一次读取寄存器的数量

const int nCalibTimes = 1000; //校准时读数的次数
int calibData[nValCnt]; //校准数据

float fLastRoll = 0.0f; //上一次滤波获得的Roll角
float fLastPitch = 0.0f; //上一次滤波获得的Pitch角
Kalman kalmanRoll; //Roll角滤波器
Kalman kalmanPitch; //Pitch角滤波器

//对Roll角和Pitch角进行卡尔曼滤波 
float fNewRoll = 0.0f;
float fNewPitch = 0.0f;
//跟据滤波值计算角度速 
float fRollRate = 0.0f;
float fPitchRate = 0.0f;
  
//————————————————初始化函数————————————————————————————————
void mpu6050_dsp(void)
{ 
  Wire.begin(); //初始化Wire库
  WriteMPUReg(0x6B, 0); //启动MPU6050设备 （设置I2C接口，添加一个字节的寄存器变量）

  Calibration(); //执行校准
}

//———————————————读取mpu6050数据函数———————————————————————————————
void mpu6050_read(long dT_us)
{
  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;  
  }
  //Serial.print(dt);Serial.print("\n");
  //对Roll角和Pitch角进行卡尔曼滤波 
  fNewRoll = kalmanRoll.getAngle(fRoll, realVals[4], dT_us/1000000.0f);
  fNewPitch = kalmanPitch.getAngle(fPitch, realVals[5], dT_us/1000000.0f);
  //跟据滤波值计算角度速 
  fRollRate = (fNewRoll - fLastRoll)*1000000 / dT_us;   //单位：s
  fPitchRate = (fNewPitch - fLastPitch)*1000000 / dT_us;//单位：s
 
 //更新Roll角和Pitch角
  fLastRoll = fNewRoll;
  fLastPitch = fNewPitch;

  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");
}

//——————————————其余的调用函数—————————————————————————————————
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角。算法见文档。绕x轴旋转
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;   //fRad2Deg = 57.295779513f; //将弧度转为角度的乘数
}

//算得Pitch角。算法见文档。绕y轴旋转
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;
}

////计算Yaw角。绕z轴旋转，仅变化量
//float GetYaw(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;
  }
}

main.cpp

//测试代码
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600); //初始化串口，指定波特率
  mpu6050_dsp();
  delay(500);
  Serial.print("Init OK!\n");
}

void loop() {
  // put your main code here, to run repeatedly:
  mpu6050_read(1000);
  dalay(1);
}

读取性能

通过测试，读取mpu6050所需时间小于1ms。