一、互补滤波原理:
陀螺仪动态响应特性良好,但计算姿态时,会产生累积误差。数字罗盘和加速度传感器测量姿态没有累积误差,但动态响应较差。因此,它们在频域上特性互补,可以采用互补滤波器融合这三种传感器的数据,提高测量精度和系统的动态性能。
二、实际程序操作过程
主要讲一下互补滤波程序
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); //初始化gpiogpio_init (SDA_PIN, GPO, 1); //初始化gpioport_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(); //送数据口elseSDA0();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