對於互補濾波的解釋

一、互補濾波原理：

陀螺儀動態響應特性良好，但計算姿態時，會產生累積誤差。數字羅盤和加速度傳感器測量姿態沒有累積誤差，但動態響應較差。因此，它們在頻域上特性互補，可以採用互補濾波器融合這三種傳感器的數據，提高測量精度和系統的動態性能。

二、實際程序操作過程

主要講一下互補濾波程序

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