一、互补滤波原理：
陀螺仪动态响应特性良好，但计算姿态时，会产生累积误差。数字罗盘和加速度传感器测量姿态没有累积误差，但动态响应较差。因此，它们在频域上特性互补，可以采用互补滤波器融合这三种传感器的数据，提高测量精度和系统的动态性能。
二、实际程序操作过程
主要讲一下互补滤波程序
float ComplementaryFilter(float angle_m, float gyro_m)
{
gyro_m -= 0.55;
angleyy = K1 * angle_m + (1-K1) * (angleylast + gyro_m * dtt);
angleylast = angleyy;
return angleyy;
}
上面的程序中有两个传入参数，一个是MPU6050硬件算出来的角度，但是它会受加速度的影响，也就是，将mpu6050来回左右晃的时候，这个角度会有较大波动，理论上是保持不变的，这个角度的特点是变化比较迅速，但不是很准确。第二个参数是传回来的加速度，我们通过积分，可以得到短时间内的角度，这个值比较短时间内比较准，但是会有累计误差。所以我们综合两个值的优势，取权重，一般取K1=0.1，也就是说短时间的积分值占主要作用。综合两者的优势，我们发现效果特别明显，来回晃动，角度至基本保持不变·！
下面是完整的一套程序
#include"MPU6050.h"
float gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z;
float angleyy,angleylast;
float dtt = 0.005;
float K1 = 0.005;
void Delay(long t)
{ volatile long i=t,j;
while(i--)
{
j=1; //加大循环，就加大这个
while(j--);
}
}
void I2C_Init(void)
{
gpio_init (SCL_PIN, GPO, 1); //初始化gpio
gpio_init (SDA_PIN, GPO, 1); //初始化gpio
port_pull (SCL_PIN,ENABLE);
port_pull (SDA_PIN,ENABLE);
}
//**************************************
//I2C起始信号
//**************************************
void I2C_Start(void)
{
SDADIR_OUT();
SCLDIR_OUT();
SDA1(); //拉高数据线
SCL1(); //拉高时钟线
Delay(1); //延时
SDA0(); //产生下降沿
Delay(1); //延时
SCL0(); //拉低时钟线
}
//**************************************
//I2C停止信号
//**************************************
void I2C_Stop(void)
{
SDADIR_OUT();
SCLDIR_OUT();
SDA0(); //拉低数据线
SCL1(); //拉高时钟线
Delay(1); //延时
SDA1(); //产生上升沿
Delay(1); //延时
}
//**************************************
//I2C发送应答信号
//入口参数:ack (0:ACK 1:NAK)
//**************************************
void I2C_SendACK(unsigned char ack_dat)
{
SCL0();
SDADIR_OUT();
SCLDIR_OUT();
if(ack_dat)
SDA0();
else
SDA1();
SCL1(); //拉高时钟线
Delay(1); //延时
SCL0(); //拉低时钟线
Delay(1); //延时
}
//**************************************
//I2C接收应答信号
//**************************************
uint8_t I2C_RecvACK(void)
{
uint8_t cry;
SCL0();
SDADIR_IN();
SCLDIR_OUT();
SCL1();
Delay(1);
if(GET_SDA)
cry=1;
else
cry=0;
SCL0();
Delay(1);
return(cry);
}
//**************************************
//向I2C总线发送一个字节数据
//**************************************
void I2C_Senduint8_t(uint8_t dat)
{
uint8_t i;
SDADIR_OUT();
SCLDIR_OUT();
for (i=0; i<8; i++) //8位计数器
{
if(dat&0x80)
SDA1(); //送数据口
else
SDA0();
dat <<= 1; //移出数据的最高位
SCL1(); //拉高时钟线
Delay(1); //延时
SCL0(); //拉低时钟线
Delay(1); //延时
}
while(I2C_RecvACK());
}
//**************************************
//从I2C总线接收一个字节数据
//**************************************
uint8_t I2C_Recvuint8_t(void)
{
uint8_t i,cy;
uint8_t dat = 0;
SDADIR_OUT();
SDA1(); //使能内部上拉,准备读取数据,
SDADIR_IN();
for (i=0; i<8; i++) //8位计数器
{
dat <<= 1;
SCL1(); //拉高时钟线
Delay(1);
if(GET_SDA) cy = 1;
else cy = 0; //延时
dat |= cy; //读数据
SCL0(); //拉低时钟线
Delay(1); //延时
}
return dat;
}
uint8 read_ch(uint8 ack_x)
{
uint8 i;
uint8 c;
c=0;
SCL0();
Delay(1);
SDADIR_OUT();
SDA1(); //置数据线为输入方式
SDADIR_IN();
for(i=0;i<8;i++)
{
Delay(1);
SCL0(); //置时钟线为低，准备接收数据位
Delay(1);
SCL1(); //置时钟线为高，使数据线上数据有效
Delay(1);
c<<=1;
if(GET_SDA)
c+=1; //读数据位，将接收的数据存c
}
SDADIR_OUT();
SCL0();
Delay(1);
I2C_SendACK(ack_x);
return c;
}
//**************************************
//向I2C设备写入一个字节数据
//**************************************
void Single_WriteI2C(uint8_t REG_Address,uint8_t REG_data)
{
I2C_Start(); //起始信号
I2C_Senduint8_t(SlaveAddress); //发送设备地址+写信号
I2C_Senduint8_t(REG_Address); //内部寄存器地址，
I2C_Senduint8_t(REG_data); //内部寄存器数据，
I2C_Stop(); //发送停止信号
}
//**************************************
//从I2C设备读取一个字节数据
//**************************************
uint8_t Single_ReadI2C(uint8_t REG_Address)
{
uint8_t REG_data;
I2C_Start(); //起始信号
I2C_Senduint8_t(SlaveAddress); //发送设备地址+写信号
I2C_Senduint8_t(REG_Address); //发送存储单元地址，从0开始
I2C_Start(); //起始信号
I2C_Senduint8_t(SlaveAddress+1); //发送设备地址+读信号
REG_data=I2C_Recvuint8_t(); //读出寄存器数据
I2C_SendACK(0); //接收应答信号
I2C_Stop(); //停止信号
return REG_data;
}
//**************************************
//初始化MPU6050
//**************************************
void InitMPU6050(void)
{
I2C_Init();
//SCLIO |= 0x02;
//SDAIO |= 0x08;
Single_WriteI2C(PWR_MGMT_1, 0x00); //解除休眠状态
Single_WriteI2C(SMPLRT_DIV, 0x07);
Single_WriteI2C(CONFIG, 0x06);
Single_WriteI2C(GYRO_CONFIG, 0x18);
Single_WriteI2C(ACCEL_CONFIG, 0x10);
}
//**************************************
//合成数据
//**************************************
void I2C_Read_regs(uint8 dev_add, uint8 reg, uint8 *dat_add, uint8 num)
{
I2C_Start();
I2C_Senduint8_t(dev_add | 0x00); //发送器件地址加写位
I2C_Senduint8_t( reg ); //发送从机寄存器地址
I2C_Start();
I2C_Senduint8_t(dev_add | 0x01); //发送器件地址加读位
while(--num)
{
*dat_add = read_ch(1); //读取数据
dat_add++;
}
*dat_add = read_ch(0); //读取数据
I2C_Stop();
}
void Get_AccData(void)
{
uint8 dat[6];
I2C_Read_regs(SlaveAddress, ACCEL_XOUT_H, dat, 6);
acc_x = (int16)(((uint16)dat[0]<<8 | dat[1]))*9.8/4096.0;
acc_y = (int16)(((uint16)dat[2]<<8 | dat[3]))*9.8/4096.0;
acc_z = (int16)(((uint16)dat[4]<<8 | dat[5]))*9.8/4096.0;
}
void Get_Gyro(void)
{
uint8 dat[6];
I2C_Read_regs(SlaveAddress, GYRO_XOUT_H, dat, 6);
gyro_z = (int16)(((uint16)dat[0]<<8 | dat[1]))/16.4;
gyro_y = (int16)(((uint16)dat[2]<<8 | dat[3]))/16.4;
gyro_z = (int16)(((uint16)dat[4]<<8 | dat[5]))/16.4;
}
float ComplementaryFilter(float angle_m, float gyro_m)
{
gyro_m -= 0.55;
angleyy = K1 * angle_m + (1-K1) * (angleylast + gyro_m * dtt);
angleylast = angleyy;
return angleyy;
}
////////////////////////////////////////头文件////////////////////////////////////////
#ifndef _MPU6050_H_
#define _MPU6050_H_
#include "KEA_gpio.h"
//定义引脚
#define SCL_PIN PTH4
#define SDA_PIN PTH3
//#define SCL PTXn_T(SCL_PIN,OUT) //IIC时钟引脚定义
//#define SDA PTXn_T(SDA_PIN,OUT) //IIC数据引脚定义
//#define SCLIO PTXn_T(SCL_PIN,DDR)
//#define SDAIO PTXn_T(SDA_PIN,DDR)
#define GET_SDA gpio_get (SDA_PIN)
#define SDA0() gpio_set (SDA_PIN, 0) //SDA = 0
#define SDA1() gpio_set (SDA_PIN, 1) //SDA = 1
#define SCL0() gpio_set (SCL_PIN, 0) //SCL = 0
#define SCL1() gpio_set (SCL_PIN, 1) //SCL = 1
#define SDADIR_OUT() gpio_ddr (SDA_PIN, GPO) //输出方向 SDAIO = 1
#define SDADIR_IN() gpio_ddr (SDA_PIN, GPI) //输入方向 SDAIO = 0
#define SCLDIR_OUT() gpio_ddr(SCL_PIN,GPO)
#define SCLDIR_IN() gpio_ddr(SCL_PIN,GPI)
//测试样例，看 main_test 函数
//****************************************
// 定义MPU6050内部地址
//****************************************
#define SMPLRT_DIV 0x19 //陀螺仪采样率，典型值：0x07(125Hz)
#define CONFIG 0x1A //低通滤波频率，典型值：0x06(5Hz)
#define GYRO_CONFIG 0x1B //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
#define ACCEL_CONFIG 0x1C //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44
#define GYRO_YOUT_H 0x45
#define GYRO_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47
#define GYRO_ZOUT_L 0x48
#define PWR_MGMT_1 0x6B //电源管理，典型值：0x00(正常启用)
#define WHO_AM_I 0x75 //IIC地址寄存器(默认数值0x68，只读)
#define SlaveAddress 0xD0 //IIC写入时的地址字节数据，+1为读取
//****************************************
//函数声明
//****************************************
//MPU6050操作函数
extern float gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z;
void InitMPU6050(void); //初始化MPU6050
void I2C_Init(void);
void I2C_Start(void);
void I2C_Stop(void);
void Get_Gyro(void);
void Get_AccData(void);
void I2C_Senduint8_t(uint8_t data);
uint8_t I2C_Recvuint8_t(void);
uint8_t Single_ReadI2C(uint8_t REG_Address); //读取I2C数据
void Single_WriteI2C(uint8_t REG_Address,uint8_t REG_data); //向I2C写入数据
void I2C_Read_regs(uint8 dev_add, uint8 reg, uint8 *dat_add, uint8 num);
//int GetData(uint8_t REG_Address);
float ComplementaryFiltering(float angle_m, float gyro_m);
#endif