[应用相关] [stm32] MPU6050 HMC5883 Kalman 融合算法移植

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics-> All rights reserved->



 This software may be distributed and modified under the terms of the GNU

 General Public License version 2 (GPL2) as published by the Free Software

 Foundation and appearing in the file GPL2->TXT included in the packaging of

 this file-> Please note that GPL2 Section 2[b] requires that all works based

 on this software must also be made publicly available under the terms of

 the GPL2 ("Copyleft")->



 Contact information

 -------------------



 Kristian Lauszus, TKJ Electronics

 Web      :  http://www->tkjelectronics->com

 e-mail   :  kristianl@tkjelectronics->com

 */



#ifndef _Kalman_h

#define _Kalman_h

struct Kalman {

    /* Kalman filter variables */

    double Q_angle; // Process noise variance for the accelerometer

    double Q_bias; // Process noise variance for the gyro bias

    double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise



    double angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector

    double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector

    double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate



    double P[2][2]; // Error covariance matrix - This is a 2x2 matrix

    double K[2]; // Kalman gain - This is a 2x1 vector

    double y; // Angle difference

    double S; // Estimate error

};



void   Init(struct Kalman* klm){

    /* We will set the variables like so, these can also be tuned by the user */

    klm->Q_angle = 0.001;

    klm->Q_bias = 0.003;

    klm->R_measure = 0.03;



    klm->angle = 0; // Reset the angle

    klm->bias = 0; // Reset bias



    klm->P[0][0] = 0; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en->wikipedia->org/wiki/Kalman_filter#Example_application->2C_technical

    klm->P[0][1] = 0;

    klm->P[1][0] = 0;

    klm->P[1][1] = 0;

}



// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds

double getAngle(struct Kalman * klm, double newAngle, double newRate, double dt) {

    // KasBot V2  -  Kalman filter module - http://www->x-firm->com/?page_id=145

    // Modified by Kristian Lauszus

    // See my blog post for more information: http://blog->tkjelectronics->dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it

    

    // Discrete Kalman filter time update equations - Time Update ("Predict")

    // Update xhat - Project the state ahead

    /* Step 1 */

    klm->rate = newRate - klm->bias;

    klm->angle += dt * klm->rate;

    

    // Update estimation error covariance - Project the error covariance ahead

    /* Step 2 */

    klm->P[0][0] += dt * (dt*klm->P[1][1] - klm->P[0][1] - klm->P[1][0] + klm->Q_angle);

    klm->P[0][1] -= dt * klm->P[1][1];

    klm->P[1][0] -= dt * klm->P[1][1];

    klm->P[1][1] += klm->Q_bias * dt;

    

    // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")

    // Calculate Kalman gain - Compute the Kalman gain

    /* Step 4 */

    klm->S = klm->P[0][0] + klm->R_measure;

    /* Step 5 */

    klm->K[0] = klm->P[0][0] / klm->S;

    klm->K[1] = klm->P[1][0] / klm->S;

    

    // Calculate angle and bias - Update estimate with measurement zk (newAngle)

    /* Step 3 */

    klm->y = newAngle - klm->angle;

    /* Step 6 */

    klm->angle += klm->K[0] * klm->y;

    klm->bias += klm->K[1] * klm->y;

    

    // Calculate estimation error covariance - Update the error covariance

    /* Step 7 */

    klm->P[0][0] -= klm->K[0] * klm->P[0][0];

    klm->P[0][1] -= klm->K[0] * klm->P[0][1];

    klm->P[1][0] -= klm->K[1] * klm->P[0][0];

    klm->P[1][1] -= klm->K[1] * klm->P[0][1];

    

    return klm->angle;

}



void setAngle(struct Kalman* klm, double newAngle) { klm->angle = newAngle; } // Used to set angle, this should be set as the starting angle

double getRate(struct Kalman* klm) { return klm->rate; } // Return the unbiased rate



/* These are used to tune the Kalman filter */

void setQangle(struct Kalman* klm, double newQ_angle) { klm->Q_angle = newQ_angle; }

void setQbias(struct Kalman* klm, double newQ_bias) { klm->Q_bias = newQ_bias; }

void setRmeasure(struct Kalman* klm, double newR_measure) { klm->R_measure = newR_measure; }



double getQangle(struct Kalman* klm) { return klm->Q_angle; }

double getQbias(struct Kalman* klm) { return klm->Q_bias; }

double getRmeasure(struct Kalman* klm) { return klm->R_measure; }



#endif

#include "stm32f10x.h"



/*标志是否读出数据*/

char  test=0;

/*I2C从设备*/

unsigned char SlaveAddress;

/*模拟IIC端口输出输入定义*/

#define SCL_H         GPIOB->BSRR = GPIO_Pin_10

#define SCL_L         GPIOB->BRR  = GPIO_Pin_10 

#define SDA_H         GPIOB->BSRR = GPIO_Pin_11

#define SDA_L         GPIOB->BRR  = GPIO_Pin_11

#define SCL_read      GPIOB->IDR  & GPIO_Pin_10

#define SDA_read      GPIOB->IDR  & GPIO_Pin_11



/*I2C的端口初始化---------------------------------------*/

void I2C_GPIO_Config(void)

{

    GPIO_InitTypeDef  GPIO_InitStructure; 

    

    GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_10;

    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;  

    GPIO_Init(GPIOB, &GPIO_InitStructure);

    

    GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_11;

    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;

    GPIO_Init(GPIOB, &GPIO_InitStructure);

}



/*I2C的延时函数-----------------------------------------*/

void I2C_delay(void)

{

    u8 i=30; //这里可以优化速度    ，经测试最低到5还能写入

    while(i) 

    { 

        i--; 

    }  

}



/*I2C的等待5ms函数--------------------------------------*/

void delay5ms(void)

{

    int i=5000;  

    while(i) 

    { 

        i--; 

    }  

}



/*I2C启动函数-------------------------------------------*/

bool I2C_Start(void)

{

    SDA_H;

    SCL_H;

    I2C_delay();

    if(!SDA_read)return FALSE;    //SDA线为低电平则总线忙,退出

    SDA_L;

    I2C_delay();

    if(SDA_read) return FALSE;    //SDA线为高电平则总线出错,退出

    SDA_L;

    I2C_delay();

    return TRUE;

}



/*I2C停止函数-------------------------------------------*/

void I2C_Stop(void)

{

    SCL_L;

    I2C_delay();

    SDA_L;

    I2C_delay();

    SCL_H;

    I2C_delay();

    SDA_H;

    I2C_delay();

} 



/*I2C的ACK函数------------------------------------------*/

void I2C_Ack(void)

{    

    SCL_L;

    I2C_delay();

    SDA_L;

    I2C_delay();

    SCL_H;

    I2C_delay();

    SCL_L;

    I2C_delay();

}   



/*I2C的NoACK函数----------------------------------------*/

void I2C_NoAck(void)

{    

    SCL_L;

    I2C_delay();

    SDA_H;

    I2C_delay();

    SCL_H;

    I2C_delay();

    SCL_L;

    I2C_delay();

} 



/*I2C等待ACK函数----------------------------------------*/

bool I2C_WaitAck(void)      //返回为:=1有ACK,=0无ACK

{

    SCL_L;

    I2C_delay();

    SDA_H;            

    I2C_delay();

    SCL_H;

    I2C_delay();

    if(SDA_read)

    {

        SCL_L;

        I2C_delay();

        return FALSE;

    }

    SCL_L;

    I2C_delay();

    return TRUE;

}



/*I2C发送一个u8数据函数---------------------------------*/

void I2C_SendByte(u8 SendByte) //数据从高位到低位//

{

    u8 i=8;

    while(i--)

    {

        SCL_L;

        I2C_delay();

        if(SendByte&0x80)

            SDA_H;  

        else 

            SDA_L;   

        SendByte<<=1;

        I2C_delay();

        SCL_H;

        I2C_delay();

    }

    SCL_L;

}  



/*I2C读取一个u8数据函数---------------------------------*/

unsigned char I2C_RadeByte(void)  //数据从高位到低位//

{ 

    u8 i=8;

    u8 ReceiveByte=0;

    

    SDA_H;                

    while(i--)

    {

        ReceiveByte<<=1;      

        SCL_L;

        I2C_delay();

        SCL_H;

        I2C_delay();    

        if(SDA_read)

        {

            ReceiveByte|=0x01;

        }

    }

    SCL_L;

    return ReceiveByte;

}  



/*I2C向指定设备指定地址写入u8数据-----------------------*/

void Single_WriteI2C(unsigned char REG_Address,unsigned char REG_data)//单字节写入

{

    if(!I2C_Start())return;

    I2C_SendByte(SlaveAddress);   //发送设备地址+写信号//I2C_SendByte(((REG_Address & 0x0700) >>7) | SlaveAddress & 0xFFFE);//设置高起始地址+器件地址 

    if(!I2C_WaitAck()){I2C_Stop(); return;}

    I2C_SendByte(REG_Address );   //设置低起始地址      

    I2C_WaitAck();    

    I2C_SendByte(REG_data);

    I2C_WaitAck();   

    I2C_Stop(); 

    delay5ms();

}



/*I2C向指定设备指定地址读出u8数据-----------------------*/

unsigned char Single_ReadI2C(unsigned char REG_Address)//读取单字节

{   

    unsigned char REG_data;         

    if(!I2C_Start())return FALSE;

    I2C_SendByte(SlaveAddress); //I2C_SendByte(((REG_Address & 0x0700) >>7) | REG_Address & 0xFFFE);//设置高起始地址+器件地址 

    if(!I2C_WaitAck()){I2C_Stop();test=1; return FALSE;}

    I2C_SendByte((u8) REG_Address);   //设置低起始地址      

    I2C_WaitAck();

    I2C_Start();

    I2C_SendByte(SlaveAddress+1);

    I2C_WaitAck();

    

    REG_data= I2C_RadeByte();

    I2C_NoAck();

    I2C_Stop();

    //return TRUE;

    return REG_data;

}

#define    SlaveAddressMPU   0x68      //定义器件5883在IIC总线中的从地址



typedef unsigned char  uchar;



extern int accX, accY, accZ;

extern int gyroX, gyroY, gyroZ;

extern uchar    SlaveAddress;       //IIC写入时的地址字节数据，+1为读取

extern uchar Single_ReadI2C(uchar REG_Address);                        //读取I2C数据

extern void  Single_WriteI2C(uchar REG_Address,uchar REG_data);        //向I2C写入数据



//****************************************

// 定义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    TEMP_OUT_H        0x41

#define    TEMP_OUT_L        0x42

#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    MPU6050_Addr    0xD0    //IIC写入时的地址字节数据，+1为读取

//**************************************

//初始化MPU6050

//**************************************

void InitMPU6050()

{

    SlaveAddress=MPU6050_Addr;

    Single_WriteI2C(PWR_MGMT_1, 0x00);    //解除休眠状态

    Single_WriteI2C(SMPLRT_DIV, 0x07);// Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz

    Single_WriteI2C(CONFIG, 0x00);// Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling

    Single_WriteI2C(GYRO_CONFIG, 0x00);// Set Gyro Full Scale Range to ±250deg/s

    Single_WriteI2C(ACCEL_CONFIG, 0x00);// Set Accelerometer Full Scale Range to ±2g

    Single_WriteI2C(PWR_MGMT_1, 0x01);// PLL with X axis gyroscope reference and disable sleep mode

}

//**************************************

//// Get accelerometer and gyroscope values

//**************************************

void updateMPU6050()

{

    SlaveAddress=MPU6050_Addr;// Get accelerometer and gyroscope values



    accX=((Single_ReadI2C(ACCEL_XOUT_H)<<8)+Single_ReadI2C(ACCEL_XOUT_L));

    accY=-((Single_ReadI2C(ACCEL_YOUT_H)<<8)+Single_ReadI2C(ACCEL_YOUT_L));

    accZ=((Single_ReadI2C(ACCEL_ZOUT_H)<<8)+Single_ReadI2C(ACCEL_ZOUT_L));

    

    gyroX=-((Single_ReadI2C(GYRO_XOUT_H)<<8)+Single_ReadI2C(GYRO_XOUT_L));

    gyroY=((Single_ReadI2C(GYRO_YOUT_H)<<8)+Single_ReadI2C(GYRO_YOUT_L));

    gyroZ=-((Single_ReadI2C(GYRO_ZOUT_H)<<8)+Single_ReadI2C(GYRO_ZOUT_L));    

}

#define   uchar unsigned char

#define   uint unsigned int    



//定义器件在IIC总线中的从地址,根据ALT  ADDRESS地址引脚不同修改

#define    HMC5883_Addr   0x3C    //磁场传感器器件地址



unsigned char BUF[8];                         //接收数据缓存区                   

extern uchar    SlaveAddress;               //IIC写入时的地址字节数据，+1为读取



extern int magX, magY, magZ;    //hmc最原始数据

extern uchar SlaveAddress;       //IIC写入时的地址字节数据，+1为读取

extern uchar Single_ReadI2C(uchar REG_Address);                        //读取I2C数据

extern void  Single_WriteI2C(uchar REG_Address,uchar REG_data);        //向I2C写入数据

//**************************************

//初始化HMC5883，根据需要请参考pdf进行修改

//**************************************

void InitHMC5883()

{

    SlaveAddress=HMC5883_Addr;

    Single_WriteI2C(0x02,0x00);  //

    Single_WriteI2C(0x01,0xE0);  //

}

//**************************************

//从HMC5883连续读取6个数据放在BUF中

//**************************************

void updateHMC5883()

{

    SlaveAddress=HMC5883_Addr;

    Single_WriteI2C(0x00,0x14); 

    Single_WriteI2C(0x02,0x00); 

//    Delayms(10);

    

    BUF[1]=Single_ReadI2C(0x03);//OUT_X_L_A

    BUF[2]=Single_ReadI2C(0x04);//OUT_X_H_A

    BUF[3]=Single_ReadI2C(0x07);//OUT_Y_L_A

    BUF[4]=Single_ReadI2C(0x08);//OUT_Y_H_A

    BUF[5]=Single_ReadI2C(0x05);//OUT_Z_L_A

    BUF[6]=Single_ReadI2C(0x06);//OUT_Y_H_A

    

    magX=(BUF[1] << 8) | BUF[2]; //Combine MSB and LSB of X Data output register

    magY=(BUF[3] << 8) | BUF[4]; //Combine MSB and LSB of Y Data output register

    magZ=(BUF[5] << 8) | BUF[6]; //Combine MSB and LSB of Z Data output register



//    if(magX>0x7fff)magX-=0xffff;//补码表示滴~所以要转化一下      

//    if(magY>0x7fff)magY-=0xffff;    

//     if(magZ>0x7fff)magZ-=0xffff;

}

#include "stm32f10x.h"



void USART1_Configuration(void);

void USART1_SendData(u8 SendData);

extern void Delayms(vu32 m);



void USART1_Configuration()

{

    GPIO_InitTypeDef GPIO_InitStructure;

    USART_InitTypeDef USART_InitStructure;

    USART_ClockInitTypeDef  USART_ClockInitStructure;



    RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB ,ENABLE );//| RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOD, ENABLE  );

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1 |RCC_APB2Periph_USART1, ENABLE  );



    /* Configure USART1 Tx (PA.09) as alternate function push-pull */

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;                 //    选中管脚9

    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;         // 复用推挽输出

    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;         // 最高输出速率50MHz

    GPIO_Init(GPIOA, &GPIO_InitStructure);                 // 选择A端口

    

    /* Configure USART1 Rx (PA.10) as input floating */

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;              //选中管脚10

    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;      //浮空输入

    GPIO_Init(GPIOA, &GPIO_InitStructure);                  //选择A端口





    USART_ClockInitStructure.USART_Clock = USART_Clock_Disable;            // 时钟低电平活动

    USART_ClockInitStructure.USART_CPOL = USART_CPOL_Low;                // 时钟低电平

    USART_ClockInitStructure.USART_CPHA = USART_CPHA_2Edge;                // 时钟第二个边沿进行数据捕获

    USART_ClockInitStructure.USART_LastBit = USART_LastBit_Disable;        // 最后一位数据的时钟脉冲不从SCLK输出

    /* Configure the USART1 synchronous paramters */

    USART_ClockInit(USART1, &USART_ClockInitStructure);                    // 时钟参数初始化设置

    

    USART_InitStructure.USART_BaudRate = 9600;                          // 波特率为：115200

    USART_InitStructure.USART_WordLength = USART_WordLength_8b;              // 8位数据

    USART_InitStructure.USART_StopBits = USART_StopBits_1;                  // 在帧结尾传输1个停止位

    USART_InitStructure.USART_Parity = USART_Parity_No ;                  // 奇偶失能

    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;    // 硬件流控制失能

    

    USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;          // 发送使能+接收使能

    /* Configure USART1 basic and asynchronous paramters */

    USART_Init(USART1, &USART_InitStructure);

    

    /* Enable USART1 */

    USART_ClearFlag(USART1, USART_IT_RXNE);             //清中断，以免一启用中断后立即产生中断

    USART_ITConfig(USART1,USART_IT_RXNE, ENABLE);        //使能USART1中断源

    USART_Cmd(USART1, ENABLE);                            //USART1总开关：开启 

}

void  USART1_SendData(u8 SendData)

{

    USART_SendData(USART1, SendData);

    while(USART_GetFlagStatus(USART1, USART_FLAG_TC)==RESET);

}

#include "stm32f10x.h"





void Delay(vu32 nCount)

{

    for(; nCount != 0; nCount--);

}

void Delayms(vu32 m)

{

    u32 i;    

    for(; m != 0; m--)    

        for (i=0; i<50000; i++);

}

#include "stm32f10x.h"

#include "Kalman.h" 

#include <math.h>

#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf



struct Kalman kalmanX, kalmanY, kalmanZ; // Create the Kalman instances



/* IMU Data MPU6050 AND HMC5883 Data*/

int accX, accY, accZ;

int gyroX, gyroY, gyroZ;

int magX, magY, magZ;





double roll, pitch, yaw; // Roll and pitch are calculated using the accelerometer while yaw is calculated using the magnetometer



double gyroXangle, gyroYangle, gyroZangle; // Angle calculate using the gyro only 只用陀螺仪计算角度

double compAngleX, compAngleY, compAngleZ; // Calculated angle using a complementary filter  用电磁计计算角度

double kalAngleX, kalAngleY, kalAngleZ; // Calculated angle using a Kalman filter    用kalman计算角度



//uint32_t timer,micros; //上一次时间与当前时间

uint8_t i2cData[14]; // Buffer for I2C data



#define MAG0MAX 603

#define MAG0MIN -578



#define MAG1MAX 542

#define MAG1MIN -701



#define MAG2MAX 547

#define MAG2MIN -556



#define RAD_TO_DEG 57.295779513082320876798154814105  // 弧度转角度的转换率

#define DEG_TO_RAD 0.01745329251994329576923690768489 // 角度转弧度的转换率



float magOffset[3] = { (MAG0MAX + MAG0MIN) / 2, (MAG1MAX + MAG1MIN) / 2, (MAG2MAX + MAG2MIN) / 2 };

double magGain[3];



void  SYSTICK_Configuration(void);                                //系统滴答中断配置

void  RCC_Configuration(void);

void  updatePitchRoll(void);                                    //根据加速计刷新Pitch和Roll数据

void  updateYaw(void);                                            //根据磁力计刷新Yaw角

void  InitAll(void);                                            //循环前的初始化

void  func(void);                                                //循环里的内容

extern void InitMPU6050(void);                                    //初始化MPU6050

extern void InitHMC5883(void);                                    //初始化HMC5883

extern void updateMPU6050(void);                                //Get accelerometer and gyroscope values

extern void updateHMC5883(void);                                //Get magnetometer values

extern void USART1_Configuration(void);                            //串口初始化

extern void USART1_SendData(u8 SendData);                        //串口发送函数

extern void I2C_GPIO_Config(void);                                //I2C初始化函数

/****************************************************************************

* 名    称：int main(void)

* 功    能：主函数

* 入口参数：无

* 出口参数：无

* 说    明：

* 调用方法：无 

****************************************************************************/ 

int main(void)

{

      RCC_Configuration();                   //系统时钟配置    

      USART1_Configuration();

      I2C_GPIO_Config();

      InitHMC5883();

    InitMPU6050();

    InitAll();    

//    SYSTICK_Configuration();                

     while(1)

    {

        func();

      }

}

///*

//系统滴答中断配置

//*/

//void SYSTICK_Configuration(void)

//{

//    micros=0;//全局计数时间归零

//     if (SysTick_Config(72000))            //时钟节拍中断时1000ms一次  用于定时 

//       { 

//        /* Capture error */ 

////        while (1);

//       }

//}

///*

//当前时间++.为了防止溢出当其大于2^20时，令其归零

//*/

//void SysTickHandler(void)

//{

//     micros++;

//    if(micros>(1<<20))

//          micros=0;

//}

/****************************************************************************

* 名    称：void RCC_Configuration(void)

* 功    能：系统时钟配置为72MHZ

* 入口参数：无

* 出口参数：无

* 说    明：

* 调用方法：无 

****************************************************************************/ 

void RCC_Configuration(void)

{   

    SystemInit();

}



void InitAll()

{

    /* Set Kalman and gyro starting angle */

    updateMPU6050();

    updateHMC5883();

    updatePitchRoll();

    updateYaw();

    

    setAngle(&kalmanX,roll); // First set roll starting angle

    gyroXangle = roll;

    compAngleX = roll;

    

    setAngle(&kalmanY,pitch); // Then pitch

    gyroYangle = pitch;

    compAngleY = pitch;

    

    setAngle(&kalmanZ,yaw); // And finally yaw

    gyroZangle = yaw;

    compAngleZ = yaw;

    

//    timer = micros; // Initialize the timer    

}



void send(double xx,double yy,double zz)

{

    int    a[3];

     u8 i,sendData[12];       

    a[0]=(int)xx;a[1]=(int)yy;a[2]=(int)zz;

    for(i=0;i<3;i++)

    {

        if(a[i]<0){

            sendData[i*4]='-';

            a[i]=-a[i];

        }

        else sendData[i*4]=' ';

        sendData[i*4+1]=(u8)(a[i]%1000/100+0x30);

        sendData[i*4+2]=(u8)(a[i]%100/10+0x30);

        sendData[i*4+3]=(u8)(a[i]%10+0x30);

    }

    for(i=0;i<12;i++)

    {

        USART1_SendData(sendData[i]);

    }

    USART1_SendData(0x0D);

    USART1_SendData(0x0A);

}



void func()

{

    double gyroXrate,gyroYrate,gyroZrate,dt=0.01;

    /* Update all the IMU values */

    updateMPU6050();

    updateHMC5883();

    

//    dt = (double)(micros - timer) / 1000; // Calculate delta time

//    timer = micros;

//    if(dt<0)dt+=(1<<20);    //时间是周期性的，有可能当前时间小于上次时间，因为这个周期远大于两次积分时间，所以最多相差1<<20



    /* Roll and pitch estimation */

    updatePitchRoll();             //用采集的加速计的值计算roll和pitch的值

    gyroXrate = gyroX / 131.0;     // Convert to deg/s    把陀螺仪的角加速度按照当初设定的量程转换为°/s

    gyroYrate = gyroY / 131.0;     // Convert to deg/s

    

    #ifdef RESTRICT_PITCH        //如果上面有#define RESTRICT_PITCH就采用这种方法计算，防止出现-180和180之间的跳跃

    // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees

    if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {

        setAngle(&kalmanX,roll);

        compAngleX = roll;

        kalAngleX = roll;

        gyroXangle = roll;

    } else

    kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

    

    if (fabs(kalAngleX) > 90)

        gyroYrate = -gyroYrate; // Invert rate, so it fits the restricted accelerometer reading

    kalAngleY = getAngle(&kalmanY,pitch, gyroYrate, dt);

    #else

    // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees

    if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {

        kalmanY.setAngle(pitch);

        compAngleY = pitch;

        kalAngleY = pitch;

        gyroYangle = pitch;

    } else

    kalAngleY = getAngle(&kalmanY, pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter

    

    if (abs(kalAngleY) > 90)

        gyroXrate = -gyroXrate; // Invert rate, so it fits the restricted accelerometer reading

    kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

    #endif

    

    

    /* Yaw estimation */

    updateYaw();

    gyroZrate = gyroZ / 131.0; // Convert to deg/s

    // This fixes the transition problem when the yaw angle jumps between -180 and 180 degrees

    if ((yaw < -90 && kalAngleZ > 90) || (yaw > 90 && kalAngleZ < -90)) {

        setAngle(&kalmanZ,yaw);

        compAngleZ = yaw;

        kalAngleZ = yaw;

        gyroZangle = yaw;

    } else

    kalAngleZ = getAngle(&kalmanZ, yaw, gyroZrate, dt); // Calculate the angle using a Kalman filter

    

    

    /* Estimate angles using gyro only */

    gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter

    gyroYangle += gyroYrate * dt;

    gyroZangle += gyroZrate * dt;

    //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate from the Kalman filter

    //gyroYangle += kalmanY.getRate() * dt;

    //gyroZangle += kalmanZ.getRate() * dt;

    

    /* Estimate angles using complimentary filter */

    compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter

    compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;

    compAngleZ = 0.93 * (compAngleZ + gyroZrate * dt) + 0.07 * yaw;

    

    // Reset the gyro angles when they has drifted too much

    if (gyroXangle < -180 || gyroXangle > 180)

        gyroXangle = kalAngleX;

    if (gyroYangle < -180 || gyroYangle > 180)

        gyroYangle = kalAngleY;

    if (gyroZangle < -180 || gyroZangle > 180)

        gyroZangle = kalAngleZ;

    

    

    send(roll,pitch,yaw);

//    send(gyroXangle,gyroYangle,gyroZangle);

//    send(compAngleX,compAngleY,compAngleZ);

//    send(kalAngleX,kalAngleY,kalAngleZ);

//    send(kalAngleY,compAngleY,gyroYangle);





    /* Print Data */

//    //#if 1

//    printf("%lf %lf %lf %lf\n",roll,gyroXangle,compAngleX,kalAngleX);

//    printf("%lf %lf %lf %lf\n",pitch,gyroYangle,compAngleY,kalAngleY);

//    printf("%lf %lf %lf %lf\n",yaw,gyroZangle,compAngleZ,kalAngleZ);

    //#endif

    

//    //#if 0 // Set to 1 to print the IMU data

//    printf("%lf %lf %lf\n",accX / 16384.0,accY / 16384.0,accZ / 16384.0);

//    printf("%lf %lf %lf\n",gyroXrate,gyroYrate,gyroZrate);

//    printf("%lf %lf %lf\n",magX,magY,magZ);

    //#endif

    

    //#if 0 // Set to 1 to print the temperature

    //Serial.print("\t");

    //

    //double temperature = (double)tempRaw / 340.0 + 36.53;

    //Serial.print(temperature); Serial.print("\t");

    //#endif

//    delay(10);

} 



//****************************************

//根据加速计刷新Pitch和Roll数据

//这里采用两种方法计算roll和pitch，如果最上面没有#define RESTRICT_PITCH就采用第二种计算方法

//****************************************

void updatePitchRoll() {

    // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26

    // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2

    // It is then converted from radians to degrees

    #ifdef RESTRICT_PITCH // Eq. 25 and 26

    roll = atan2(accY,accZ) * RAD_TO_DEG;

    pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;

    #else // Eq. 28 and 29

    roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;

    pitch = atan2(-accX, accZ) * RAD_TO_DEG;

    #endif

}

//****************************************

//根据磁力计刷新Yaw角

//****************************************

void updateYaw() { // See: http://www.freescale.com/files/sensors/doc/app_note/AN4248.pdf

    double rollAngle,pitchAngle,Bfy,Bfx;  

    

    magX *= -1; // Invert axis - this it done here, as it should be done after the calibration

    magZ *= -1;

    

    magX *= magGain[0];

    magY *= magGain[1];

    magZ *= magGain[2];

    

    magX -= magOffset[0];

    magY -= magOffset[1];

    magZ -= magOffset[2];

    

    

    rollAngle  = kalAngleX * DEG_TO_RAD;

    pitchAngle = kalAngleY * DEG_TO_RAD;

    

    Bfy = magZ * sin(rollAngle) - magY * cos(rollAngle);

    Bfx = magX * cos(pitchAngle) + magY * sin(pitchAngle) * sin(rollAngle) + magZ * sin(pitchAngle) * cos(rollAngle);

    yaw = atan2(-Bfy, Bfx) * RAD_TO_DEG;

    

    yaw *= -1;

}

int main(void)

{

      RCC_Configuration();                   //系统时钟配置    

      USART1_Configuration();

      I2C_GPIO_Config();

      InitHMC5883();

    InitMPU6050();

    InitAll();    

//    SYSTICK_Configuration();                

     while(1)

    {

        func();

      }

}

void InitAll()

{

    /* Set Kalman and gyro starting angle */

    updateMPU6050();

    updateHMC5883();

    updatePitchRoll();

    updateYaw();

    

    setAngle(&kalmanX,roll); // First set roll starting angle

    gyroXangle = roll;

    compAngleX = roll;

    

    setAngle(&kalmanY,pitch); // Then pitch

    gyroYangle = pitch;

    compAngleY = pitch;

    

    setAngle(&kalmanZ,yaw); // And finally yaw

    gyroZangle = yaw;

    compAngleZ = yaw;

    

//    timer = micros; // Initialize the timer    

}

void func()

{

    double gyroXrate,gyroYrate,gyroZrate,dt=0.01;

    /* Update all the IMU values */

    updateMPU6050();

    updateHMC5883();

    

//    dt = (double)(micros - timer) / 1000; // Calculate delta time

//    timer = micros;

//    if(dt<0)dt+=(1<<20);    //时间是周期性的，有可能当前时间小于上次时间，因为这个周期远大于两次积分时间，所以最多相差1<<20



    /* Roll and pitch estimation */

    updatePitchRoll();             //用采集的加速计的值计算roll和pitch的值

    gyroXrate = gyroX / 131.0;     // Convert to deg/s    把陀螺仪的角加速度按照当初设定的量程转换为°/s

    gyroYrate = gyroY / 131.0;     // Convert to deg/s

    

    #ifdef RESTRICT_PITCH        //如果上面有#define RESTRICT_PITCH就采用这种方法计算，防止出现-180和180之间的跳跃

    // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees

    if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {

        setAngle(&kalmanX,roll);

        compAngleX = roll;

        kalAngleX = roll;

        gyroXangle = roll;

    } else

    kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

    

    if (fabs(kalAngleX) > 90)

        gyroYrate = -gyroYrate; // Invert rate, so it fits the restricted accelerometer reading

    kalAngleY = getAngle(&kalmanY,pitch, gyroYrate, dt);

    #else

    // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees

    if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {

        kalmanY.setAngle(pitch);

        compAngleY = pitch;

        kalAngleY = pitch;

        gyroYangle = pitch;

    } else

    kalAngleY = getAngle(&kalmanY, pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter

    

    if (abs(kalAngleY) > 90)

        gyroXrate = -gyroXrate; // Invert rate, so it fits the restricted accelerometer reading

    kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

    #endif

    

    

    /* Yaw estimation */

    updateYaw();

    gyroZrate = gyroZ / 131.0; // Convert to deg/s

    // This fixes the transition problem when the yaw angle jumps between -180 and 180 degrees

    if ((yaw < -90 && kalAngleZ > 90) || (yaw > 90 && kalAngleZ < -90)) {

        setAngle(&kalmanZ,yaw);

        compAngleZ = yaw;

        kalAngleZ = yaw;

        gyroZangle = yaw;

    } else

    kalAngleZ = getAngle(&kalmanZ, yaw, gyroZrate, dt); // Calculate the angle using a Kalman filter

    

    

    /* Estimate angles using gyro only */

    gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter

    gyroYangle += gyroYrate * dt;

    gyroZangle += gyroZrate * dt;

    //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate from the Kalman filter

    //gyroYangle += kalmanY.getRate() * dt;

    //gyroZangle += kalmanZ.getRate() * dt;

    

    /* Estimate angles using complimentary filter */互补滤波算法

    compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter

    compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;

    compAngleZ = 0.93 * (compAngleZ + gyroZrate * dt) + 0.07 * yaw;

    

    // Reset the gyro angles when they has drifted too much

    if (gyroXangle < -180 || gyroXangle > 180)

        gyroXangle = kalAngleX;

    if (gyroYangle < -180 || gyroYangle > 180)

        gyroYangle = kalAngleY;

    if (gyroZangle < -180 || gyroZangle > 180)

        gyroZangle = kalAngleZ;

    

    

    send(roll,pitch,yaw);

//    send(gyroXangle,gyroYangle,gyroZangle);

//    send(compAngleX,compAngleY,compAngleZ);

//    send(kalAngleX,kalAngleY,kalAngleZ);

//    send(kalAngleY,compAngleY,gyroYangle);

}