1 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics-> All rights reserved->  2   3  This software may be distributed and modified under the terms of the GNU  4  General Public License version 2 (GPL2) as published by the Free Software  5  Foundation and appearing in the file GPL2->TXT included in the packaging of  6  this file-> Please note that GPL2 Section 2[b] requires that all works based  7  on this software must also be made publicly available under the terms of  8  the GPL2 ("Copyleft")->  9  10  Contact information 11  ------------------- 12  13  Kristian Lauszus, TKJ Electronics 14  Web      :  http://www->tkjelectronics->com 15  e-mail   :  kristianl@tkjelectronics->com 16  */ 17  18 #ifndef _Kalman_h 19 #define _Kalman_h 20 struct Kalman { 21     /* Kalman filter variables */ 22     double Q_angle; // Process noise variance for the accelerometer 23     double Q_bias; // Process noise variance for the gyro bias 24     double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise 25  26     double angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector 27     double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector 28     double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate 29  30     double P[2][2]; // Error covariance matrix - This is a 2x2 matrix 31     double K[2]; // Kalman gain - This is a 2x1 vector 32     double y; // Angle difference 33     double S; // Estimate error 34 }; 35  36 void   Init(struct Kalman* klm){ 37     /* We will set the variables like so, these can also be tuned by the user */ 38     klm->Q_angle = 0.001; 39     klm->Q_bias = 0.003; 40     klm->R_measure = 0.03; 41  42     klm->angle = 0; // Reset the angle 43     klm->bias = 0; // Reset bias 44  45     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 46     klm->P[0][1] = 0; 47     klm->P[1][0] = 0; 48     klm->P[1][1] = 0; 49 } 50  51 // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds 52 double getAngle(struct Kalman * klm, double newAngle, double newRate, double dt) { 53     // KasBot V2  -  Kalman filter module - http://www->x-firm->com/?page_id=145 54     // Modified by Kristian Lauszus 55     // See my blog post for more information: http://blog->tkjelectronics->dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it 56      57     // Discrete Kalman filter time update equations - Time Update ("Predict") 58     // Update xhat - Project the state ahead 59     /* Step 1 */ 60     klm->rate = newRate - klm->bias; 61     klm->angle += dt * klm->rate; 62      63     // Update estimation error covariance - Project the error covariance ahead 64     /* Step 2 */ 65     klm->P[0][0] += dt * (dt*klm->P[1][1] - klm->P[0][1] - klm->P[1][0] + klm->Q_angle); 66     klm->P[0][1] -= dt * klm->P[1][1]; 67     klm->P[1][0] -= dt * klm->P[1][1]; 68     klm->P[1][1] += klm->Q_bias * dt; 69      70     // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") 71     // Calculate Kalman gain - Compute the Kalman gain 72     /* Step 4 */ 73     klm->S = klm->P[0][0] + klm->R_measure; 74     /* Step 5 */ 75     klm->K[0] = klm->P[0][0] / klm->S; 76     klm->K[1] = klm->P[1][0] / klm->S; 77      78     // Calculate angle and bias - Update estimate with measurement zk (newAngle) 79     /* Step 3 */ 80     klm->y = newAngle - klm->angle; 81     /* Step 6 */ 82     klm->angle += klm->K[0] * klm->y; 83     klm->bias += klm->K[1] * klm->y; 84      85     // Calculate estimation error covariance - Update the error covariance 86     /* Step 7 */ 87     klm->P[0][0] -= klm->K[0] * klm->P[0][0]; 88     klm->P[0][1] -= klm->K[0] * klm->P[0][1]; 89     klm->P[1][0] -= klm->K[1] * klm->P[0][0]; 90     klm->P[1][1] -= klm->K[1] * klm->P[0][1]; 91      92     return klm->angle; 93 } 94  95 void setAngle(struct Kalman* klm, double newAngle) { klm->angle = newAngle; } // Used to set angle, this should be set as the starting angle 96 double getRate(struct Kalman* klm) { return klm->rate; } // Return the unbiased rate 97  98 /* These are used to tune the Kalman filter */ 99 void setQangle(struct Kalman* klm, double newQ_angle) { klm->Q_angle = newQ_angle; }100 void setQbias(struct Kalman* klm, double newQ_bias) { klm->Q_bias = newQ_bias; }101 void setRmeasure(struct Kalman* klm, double newR_measure) { klm->R_measure = newR_measure; }102 103 double getQangle(struct Kalman* klm) { return klm->Q_angle; }104 double getQbias(struct Kalman* klm) { return klm->Q_bias; }105 double getRmeasure(struct Kalman* klm) { return klm->R_measure; }106 107 #endif

  1 #include "stm32f10x.h"  2   3 /*标志是否读出数据*/  4 char  test=0;  5 /*I2C从设备*/  6 unsigned char SlaveAddress;  7 /*模拟IIC端口输出输入定义*/  8 #define SCL_H         GPIOB->BSRR = GPIO_Pin_10  9 #define SCL_L         GPIOB->BRR  = GPIO_Pin_10  10 #define SDA_H         GPIOB->BSRR = GPIO_Pin_11 11 #define SDA_L         GPIOB->BRR  = GPIO_Pin_11 12 #define SCL_read      GPIOB->IDR  & GPIO_Pin_10 13 #define SDA_read      GPIOB->IDR  & GPIO_Pin_11 14  15 /*I2C的端口初始化---------------------------------------*/ 16 void I2C_GPIO_Config(void) 17 { 18     GPIO_InitTypeDef  GPIO_InitStructure;  19      20     GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_10; 21     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 22     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;   23     GPIO_Init(GPIOB, &GPIO_InitStructure); 24      25     GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_11; 26     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 27     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD; 28     GPIO_Init(GPIOB, &GPIO_InitStructure); 29 } 30  31 /*I2C的延时函数-----------------------------------------*/ 32 void I2C_delay(void) 33 { 34     u8 i=30; //这里可以优化速度    ，经测试最低到5还能写入 35     while(i)  36     {  37         i--;  38     }   39 } 40  41 /*I2C的等待5ms函数--------------------------------------*/ 42 void delay5ms(void) 43 { 44     int i=5000;   45     while(i)  46     {  47         i--;  48     }   49 } 50  51 /*I2C启动函数-------------------------------------------*/ 52 bool I2C_Start(void) 53 { 54     SDA_H; 55     SCL_H; 56     I2C_delay(); 57     if(!SDA_read)return FALSE;    //SDA线为低电平则总线忙,退出 58     SDA_L; 59     I2C_delay(); 60     if(SDA_read) return FALSE;    //SDA线为高电平则总线出错,退出 61     SDA_L; 62     I2C_delay(); 63     return TRUE; 64 } 65  66 /*I2C停止函数-------------------------------------------*/ 67 void I2C_Stop(void) 68 { 69     SCL_L; 70     I2C_delay(); 71     SDA_L; 72     I2C_delay(); 73     SCL_H; 74     I2C_delay(); 75     SDA_H; 76     I2C_delay(); 77 }  78  79 /*I2C的ACK函数------------------------------------------*/ 80 void I2C_Ack(void) 81 {     82     SCL_L; 83     I2C_delay(); 84     SDA_L; 85     I2C_delay(); 86     SCL_H; 87     I2C_delay(); 88     SCL_L; 89     I2C_delay(); 90 }    91  92 /*I2C的NoACK函数----------------------------------------*/ 93 void I2C_NoAck(void) 94 {     95     SCL_L; 96     I2C_delay(); 97     SDA_H; 98     I2C_delay(); 99     SCL_H;100     I2C_delay();101     SCL_L;102     I2C_delay();103 } 104 105 /*I2C等待ACK函数----------------------------------------*/106 bool I2C_WaitAck(void)      //返回为:=1有ACK,=0无ACK107 {108     SCL_L;109     I2C_delay();110     SDA_H;            111     I2C_delay();112     SCL_H;113     I2C_delay();114     if(SDA_read)115     {116         SCL_L;117         I2C_delay();118         return FALSE;119     }120     SCL_L;121     I2C_delay();122     return TRUE;123 }124 125 /*I2C发送一个u8数据函数---------------------------------*/126 void I2C_SendByte(u8 SendByte) //数据从高位到低位//127 {128     u8 i=8;129     while(i--)130     {131         SCL_L;132         I2C_delay();133         if(SendByte&0x80)134             SDA_H;  135         else 136             SDA_L;   137         SendByte<<=1;138         I2C_delay();139         SCL_H;140         I2C_delay();141     }142     SCL_L;143 }  144 145 /*I2C读取一个u8数据函数---------------------------------*/146 unsigned char I2C_RadeByte(void)  //数据从高位到低位//147 { 148     u8 i=8;149     u8 ReceiveByte=0;150     151     SDA_H;                152     while(i--)153     {154         ReceiveByte<<=1;      155         SCL_L;156         I2C_delay();157         SCL_H;158         I2C_delay();    159         if(SDA_read)160         {161             ReceiveByte|=0x01;162         }163     }164     SCL_L;165     return ReceiveByte;166 }  167 168 /*I2C向指定设备指定地址写入u8数据-----------------------*/169 void Single_WriteI2C(unsigned char REG_Address,unsigned char REG_data)//单字节写入170 {171     if(!I2C_Start())return;172     I2C_SendByte(SlaveAddress);   //发送设备地址+写信号//I2C_SendByte(((REG_Address & 0x0700) >>7) | SlaveAddress & 0xFFFE);//设置高起始地址+器件地址 173     if(!I2C_WaitAck()){I2C_Stop(); return;}174     I2C_SendByte(REG_Address );   //设置低起始地址      175     I2C_WaitAck();    176     I2C_SendByte(REG_data);177     I2C_WaitAck();   178     I2C_Stop(); 179     delay5ms();180 }181 182 /*I2C向指定设备指定地址读出u8数据-----------------------*/183 unsigned char Single_ReadI2C(unsigned char REG_Address)//读取单字节184 {   185     unsigned char REG_data;         186     if(!I2C_Start())return FALSE;187     I2C_SendByte(SlaveAddress); //I2C_SendByte(((REG_Address & 0x0700) >>7) | REG_Address & 0xFFFE);//设置高起始地址+器件地址 188     if(!I2C_WaitAck()){I2C_Stop();test=1; return FALSE;}189     I2C_SendByte((u8) REG_Address);   //设置低起始地址      190     I2C_WaitAck();191     I2C_Start();192     I2C_SendByte(SlaveAddress+1);193     I2C_WaitAck();194     195     REG_data=http://www.mamicode.com/ I2C_RadeByte();196     I2C_NoAck();197     I2C_Stop();198     //return TRUE;199     return REG_data;200 }

 1 #define    SlaveAddressMPU   0x68      //定义器件5883在IIC总线中的从地址 2  3 typedef unsigned char  uchar; 4  5 extern int accX, accY, accZ; 6 extern int gyroX, gyroY, gyroZ; 7 extern uchar    SlaveAddress;       //IIC写入时的地址字节数据，+1为读取 8 extern uchar Single_ReadI2C(uchar REG_Address);                        //读取I2C数据 9 extern void  Single_WriteI2C(uchar REG_Address,uchar REG_data);        //向I2C写入数据10 11 //****************************************12 // 定义MPU6050内部地址13 //****************************************14 #define    SMPLRT_DIV        0x19    //陀螺仪采样率，典型值：0x07(125Hz)15 #define    CONFIG            0x1A    //低通滤波频率，典型值：0x06(5Hz)16 #define    GYRO_CONFIG        0x1B    //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)17 #define    ACCEL_CONFIG    0x1C    //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)18 #define    ACCEL_XOUT_H    0x3B19 #define    ACCEL_XOUT_L    0x3C20 #define    ACCEL_YOUT_H    0x3D21 #define    ACCEL_YOUT_L    0x3E22 #define    ACCEL_ZOUT_H    0x3F23 #define    ACCEL_ZOUT_L    0x4024 #define    TEMP_OUT_H        0x4125 #define    TEMP_OUT_L        0x4226 #define    GYRO_XOUT_H        0x4327 #define    GYRO_XOUT_L        0x44    28 #define    GYRO_YOUT_H        0x4529 #define    GYRO_YOUT_L        0x4630 #define    GYRO_ZOUT_H        0x4731 #define    GYRO_ZOUT_L        0x4832 #define    PWR_MGMT_1        0x6B    //电源管理，典型值：0x00(正常启用)33 #define    WHO_AM_I        0x75    //IIC地址寄存器(默认数值0x68，只读)34 #define    MPU6050_Addr    0xD0    //IIC写入时的地址字节数据，+1为读取35 //**************************************36 //初始化MPU605037 //**************************************38 void InitMPU6050()39 {40     SlaveAddress=MPU6050_Addr;41     Single_WriteI2C(PWR_MGMT_1, 0x00);    //解除休眠状态42     Single_WriteI2C(SMPLRT_DIV, 0x07);// Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz43     Single_WriteI2C(CONFIG, 0x00);// Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling44     Single_WriteI2C(GYRO_CONFIG, 0x00);// Set Gyro Full Scale Range to ±250deg/s45     Single_WriteI2C(ACCEL_CONFIG, 0x00);// Set Accelerometer Full Scale Range to ±2g46     Single_WriteI2C(PWR_MGMT_1, 0x01);// PLL with X axis gyroscope reference and disable sleep mode47 }48 //**************************************49 //// Get accelerometer and gyroscope values50 //**************************************51 void updateMPU6050()52 {53     SlaveAddress=MPU6050_Addr;// Get accelerometer and gyroscope values54 55     accX=((Single_ReadI2C(ACCEL_XOUT_H)<<8)+Single_ReadI2C(ACCEL_XOUT_L));56     accY=-((Single_ReadI2C(ACCEL_YOUT_H)<<8)+Single_ReadI2C(ACCEL_YOUT_L));57     accZ=((Single_ReadI2C(ACCEL_ZOUT_H)<<8)+Single_ReadI2C(ACCEL_ZOUT_L));58     59     gyroX=-((Single_ReadI2C(GYRO_XOUT_H)<<8)+Single_ReadI2C(GYRO_XOUT_L));60     gyroY=((Single_ReadI2C(GYRO_YOUT_H)<<8)+Single_ReadI2C(GYRO_YOUT_L));61     gyroZ=-((Single_ReadI2C(GYRO_ZOUT_H)<<8)+Single_ReadI2C(GYRO_ZOUT_L));    62 }

 1 #define   uchar unsigned char 2 #define   uint unsigned int     3  4 //定义器件在IIC总线中的从地址,根据ALT  ADDRESS地址引脚不同修改 5 #define    HMC5883_Addr   0x3C    //磁场传感器器件地址 6  7 unsigned char BUF[8];                         //接收数据缓存区                    8 extern uchar    SlaveAddress;               //IIC写入时的地址字节数据，+1为读取 9 10 extern int magX, magY, magZ;    //hmc最原始数据11 extern uchar SlaveAddress;       //IIC写入时的地址字节数据，+1为读取12 extern uchar Single_ReadI2C(uchar REG_Address);                        //读取I2C数据13 extern void  Single_WriteI2C(uchar REG_Address,uchar REG_data);        //向I2C写入数据14 //**************************************15 //初始化HMC5883，根据需要请参考pdf进行修改16 //**************************************17 void InitHMC5883()18 {19     SlaveAddress=HMC5883_Addr;20     Single_WriteI2C(0x02,0x00);  //21     Single_WriteI2C(0x01,0xE0);  //22 }23 //**************************************24 //从HMC5883连续读取6个数据放在BUF中25 //**************************************26 void updateHMC5883()27 {28     SlaveAddress=HMC5883_Addr;29     Single_WriteI2C(0x00,0x14); 30     Single_WriteI2C(0x02,0x00); 31 //    Delayms(10);32     33     BUF[1]=Single_ReadI2C(0x03);//OUT_X_L_A34     BUF[2]=Single_ReadI2C(0x04);//OUT_X_H_A35     BUF[3]=Single_ReadI2C(0x07);//OUT_Y_L_A36     BUF[4]=Single_ReadI2C(0x08);//OUT_Y_H_A37     BUF[5]=Single_ReadI2C(0x05);//OUT_Z_L_A38     BUF[6]=Single_ReadI2C(0x06);//OUT_Y_H_A39     40     magX=(BUF[1] << 8) | BUF[2]; //Combine MSB and LSB of X Data output register41     magY=(BUF[3] << 8) | BUF[4]; //Combine MSB and LSB of Y Data output register42     magZ=(BUF[5] << 8) | BUF[6]; //Combine MSB and LSB of Z Data output register43 44 //    if(magX>0x7fff)magX-=0xffff;//补码表示滴~所以要转化一下      45 //    if(magY>0x7fff)magY-=0xffff;    46 //     if(magZ>0x7fff)magZ-=0xffff;47 }

 1 #include "stm32f10x.h" 2  3 void USART1_Configuration(void); 4 void USART1_SendData(u8 SendData); 5 extern void Delayms(vu32 m); 6  7 void USART1_Configuration() 8 { 9     GPIO_InitTypeDef GPIO_InitStructure;10     USART_InitTypeDef USART_InitStructure;11     USART_ClockInitTypeDef  USART_ClockInitStructure;12 13     RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB ,ENABLE );//| RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOD, ENABLE  );14     RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1 |RCC_APB2Periph_USART1, ENABLE  );15 16     /* Configure USART1 Tx (PA.09) as alternate function push-pull */17     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;                 //    选中管脚918     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;         // 复用推挽输出19     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;         // 最高输出速率50MHz20     GPIO_Init(GPIOA, &GPIO_InitStructure);                 // 选择A端口21     22     /* Configure USART1 Rx (PA.10) as input floating */23     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;              //选中管脚1024     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;      //浮空输入25     GPIO_Init(GPIOA, &GPIO_InitStructure);                  //选择A端口26 27 28     USART_ClockInitStructure.USART_Clock = USART_Clock_Disable;            // 时钟低电平活动29     USART_ClockInitStructure.USART_CPOL = USART_CPOL_Low;                // 时钟低电平30     USART_ClockInitStructure.USART_CPHA = USART_CPHA_2Edge;                // 时钟第二个边沿进行数据捕获31     USART_ClockInitStructure.USART_LastBit = USART_LastBit_Disable;        // 最后一位数据的时钟脉冲不从SCLK输出32     /* Configure the USART1 synchronous paramters */33     USART_ClockInit(USART1, &USART_ClockInitStructure);                    // 时钟参数初始化设置34     35     USART_InitStructure.USART_BaudRate = 9600;                          // 波特率为：11520036     USART_InitStructure.USART_WordLength = USART_WordLength_8b;              // 8位数据37     USART_InitStructure.USART_StopBits = USART_StopBits_1;                  // 在帧结尾传输1个停止位38     USART_InitStructure.USART_Parity = USART_Parity_No ;                  // 奇偶失能39     USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;    // 硬件流控制失能40     41     USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;          // 发送使能+接收使能42     /* Configure USART1 basic and asynchronous paramters */43     USART_Init(USART1, &USART_InitStructure);44     45     /* Enable USART1 */46     USART_ClearFlag(USART1, USART_IT_RXNE);             //清中断，以免一启用中断后立即产生中断47     USART_ITConfig(USART1,USART_IT_RXNE, ENABLE);        //使能USART1中断源48     USART_Cmd(USART1, ENABLE);                            //USART1总开关：开启 49 }50 void  USART1_SendData(u8 SendData)51 {52     USART_SendData(USART1, SendData);53     while(USART_GetFlagStatus(USART1, USART_FLAG_TC)==RESET);54 }

 1 #include "stm32f10x.h" 2  3  4 void Delay(vu32 nCount) 5 { 6     for(; nCount != 0; nCount--); 7 } 8 void Delayms(vu32 m) 9 {10     u32 i;    11     for(; m != 0; m--)    12         for (i=0; i<50000; i++);13 }

  1 #include "stm32f10x.h"  2 #include "Kalman.h"   3 #include <math.h>  4 #define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf  5   6 struct Kalman kalmanX, kalmanY, kalmanZ; // Create the Kalman instances  7   8 /* IMU Data MPU6050 AND HMC5883 Data*/  9 int accX, accY, accZ; 10 int gyroX, gyroY, gyroZ; 11 int magX, magY, magZ; 12  13  14 double roll, pitch, yaw; // Roll and pitch are calculated using the accelerometer while yaw is calculated using the magnetometer 15  16 double gyroXangle, gyroYangle, gyroZangle; // Angle calculate using the gyro only 只用陀螺仪计算角度 17 double compAngleX, compAngleY, compAngleZ; // Calculated angle using a complementary filter  用电磁计计算角度 18 double kalAngleX, kalAngleY, kalAngleZ; // Calculated angle using a Kalman filter    用kalman计算角度 19  20 //uint32_t timer,micros; //上一次时间与当前时间 21 uint8_t i2cData[14]; // Buffer for I2C data 22  23 #define MAG0MAX 603 24 #define MAG0MIN -578 25  26 #define MAG1MAX 542 27 #define MAG1MIN -701 28  29 #define MAG2MAX 547 30 #define MAG2MIN -556 31  32 #define RAD_TO_DEG 57.295779513082320876798154814105  // 弧度转角度的转换率 33 #define DEG_TO_RAD 0.01745329251994329576923690768489 // 角度转弧度的转换率 34  35 float magOffset[3] = { (MAG0MAX + MAG0MIN) / 2, (MAG1MAX + MAG1MIN) / 2, (MAG2MAX + MAG2MIN) / 2 }; 36 double magGain[3]; 37  38 void  SYSTICK_Configuration(void);                                //系统滴答中断配置 39 void  RCC_Configuration(void); 40 void  updatePitchRoll(void);                                    //根据加速计刷新Pitch和Roll数据 41 void  updateYaw(void);                                            //根据磁力计刷新Yaw角 42 void  InitAll(void);                                            //循环前的初始化 43 void  func(void);                                                //循环里的内容 44 extern void InitMPU6050(void);                                    //初始化MPU6050 45 extern void InitHMC5883(void);                                    //初始化HMC5883 46 extern void updateMPU6050(void);                                //Get accelerometer and gyroscope values 47 extern void updateHMC5883(void);                                //Get magnetometer values 48 extern void USART1_Configuration(void);                            //串口初始化 49 extern void USART1_SendData(u8 SendData);                        //串口发送函数 50 extern void I2C_GPIO_Config(void);                                //I2C初始化函数 51 /**************************************************************************** 52 * 名    称：int main(void) 53 * 功    能：主函数 54 * 入口参数：无 55 * 出口参数：无 56 * 说    明： 57 * 调用方法：无  58 ****************************************************************************/  59 int main(void) 60 { 61       RCC_Configuration();                   //系统时钟配置     62       USART1_Configuration(); 63       I2C_GPIO_Config(); 64       InitHMC5883(); 65     InitMPU6050(); 66     InitAll();     67 //    SYSTICK_Configuration();                 68      while(1) 69     { 70         func(); 71       } 72 } 73 ///* 74 //系统滴答中断配置 75 //*/ 76 //void SYSTICK_Configuration(void) 77 //{ 78 //    micros=0;//全局计数时间归零 79 //     if (SysTick_Config(72000))            //时钟节拍中断时1000ms一次  用于定时  80 //       {  81 //        /* Capture error */  82 ////        while (1); 83 //       } 84 //} 85 ///* 86 //当前时间++.为了防止溢出当其大于2^20时，令其归零 87 //*/ 88 //void SysTickHandler(void) 89 //{ 90 //     micros++; 91 //    if(micros>(1<<20)) 92 //          micros=0; 93 //} 94 /**************************************************************************** 95 * 名    称：void RCC_Configuration(void) 96 * 功    能：系统时钟配置为72MHZ 97 * 入口参数：无 98 * 出口参数：无 99 * 说    明：100 * 调用方法：无 101 ****************************************************************************/ 102 void RCC_Configuration(void)103 {   104     SystemInit();105 }106 107 void InitAll()108 {109     /* Set Kalman and gyro starting angle */110     updateMPU6050();111     updateHMC5883();112     updatePitchRoll();113     updateYaw();114     115     setAngle(&kalmanX,roll); // First set roll starting angle116     gyroXangle = roll;117     compAngleX = roll;118     119     setAngle(&kalmanY,pitch); // Then pitch120     gyroYangle = pitch;121     compAngleY = pitch;122     123     setAngle(&kalmanZ,yaw); // And finally yaw124     gyroZangle = yaw;125     compAngleZ = yaw;126     127 //    timer = micros; // Initialize the timer    128 }129 130 void send(double xx,double yy,double zz)131 {132     int    a[3];133      u8 i,sendData[12];       134     a[0]=(int)xx;a[1]=(int)yy;a[2]=(int)zz;135     for(i=0;i<3;i++)136     {137         if(a[i]<0){138             sendData[i*4]=‘-‘;139             a[i]=-a[i];140         }141         else sendData[i*4]=‘ ‘;142         sendData[i*4+1]=(u8)(a[i]%1000/100+0x30);143         sendData[i*4+2]=(u8)(a[i]%100/10+0x30);144         sendData[i*4+3]=(u8)(a[i]%10+0x30);145     }146     for(i=0;i<12;i++)147     {148         USART1_SendData(sendData[i]);149     }150     USART1_SendData(0x0D);151     USART1_SendData(0x0A);152 }153 154 void func()155 {156     double gyroXrate,gyroYrate,gyroZrate,dt=0.01;157     /* Update all the IMU values */158     updateMPU6050();159     updateHMC5883();160     161 //    dt = (double)(micros - timer) / 1000; // Calculate delta time162 //    timer = micros;163 //    if(dt<0)dt+=(1<<20);    //时间是周期性的，有可能当前时间小于上次时间，因为这个周期远大于两次积分时间，所以最多相差1<<20164 165     /* Roll and pitch estimation */166     updatePitchRoll();             //用采集的加速计的值计算roll和pitch的值167     gyroXrate = gyroX / 131.0;     // Convert to deg/s    把陀螺仪的角加速度按照当初设定的量程转换为°/s168     gyroYrate = gyroY / 131.0;     // Convert to deg/s169     170     #ifdef RESTRICT_PITCH        //如果上面有#define RESTRICT_PITCH就采用这种方法计算，防止出现-180和180之间的跳跃171     // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees172     if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {173         setAngle(&kalmanX,roll);174         compAngleX = roll;175         kalAngleX = roll;176         gyroXangle = roll;177     } else178     kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter179     180     if (fabs(kalAngleX) > 90)181         gyroYrate = -gyroYrate; // Invert rate, so it fits the restricted accelerometer reading182     kalAngleY = getAngle(&kalmanY,pitch, gyroYrate, dt);183     #else184     // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees185     if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {186         kalmanY.setAngle(pitch);187         compAngleY = pitch;188         kalAngleY = pitch;189         gyroYangle = pitch;190     } else191     kalAngleY = getAngle(&kalmanY, pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter192     193     if (abs(kalAngleY) > 90)194         gyroXrate = -gyroXrate; // Invert rate, so it fits the restricted accelerometer reading195     kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter196     #endif197     198     199     /* Yaw estimation */200     updateYaw();201     gyroZrate = gyroZ / 131.0; // Convert to deg/s202     // This fixes the transition problem when the yaw angle jumps between -180 and 180 degrees203     if ((yaw < -90 && kalAngleZ > 90) || (yaw > 90 && kalAngleZ < -90)) {204         setAngle(&kalmanZ,yaw);205         compAngleZ = yaw;206         kalAngleZ = yaw;207         gyroZangle = yaw;208     } else209     kalAngleZ = getAngle(&kalmanZ, yaw, gyroZrate, dt); // Calculate the angle using a Kalman filter210     211     212     /* Estimate angles using gyro only */213     gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter214     gyroYangle += gyroYrate * dt;215     gyroZangle += gyroZrate * dt;216     //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate from the Kalman filter217     //gyroYangle += kalmanY.getRate() * dt;218     //gyroZangle += kalmanZ.getRate() * dt;219     220     /* Estimate angles using complimentary filter */221     compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter222     compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;223     compAngleZ = 0.93 * (compAngleZ + gyroZrate * dt) + 0.07 * yaw;224     225     // Reset the gyro angles when they has drifted too much226     if (gyroXangle < -180 || gyroXangle > 180)227         gyroXangle = kalAngleX;228     if (gyroYangle < -180 || gyroYangle > 180)229         gyroYangle = kalAngleY;230     if (gyroZangle < -180 || gyroZangle > 180)231         gyroZangle = kalAngleZ;232     233     234     send(roll,pitch,yaw);235 //    send(gyroXangle,gyroYangle,gyroZangle);236 //    send(compAngleX,compAngleY,compAngleZ);237 //    send(kalAngleX,kalAngleY,kalAngleZ);238 //    send(kalAngleY,compAngleY,gyroYangle);239 240 241     /* Print Data */242 //    //#if 1243 //    printf("%lf %lf %lf %lf\n",roll,gyroXangle,compAngleX,kalAngleX);244 //    printf("%lf %lf %lf %lf\n",pitch,gyroYangle,compAngleY,kalAngleY);245 //    printf("%lf %lf %lf %lf\n",yaw,gyroZangle,compAngleZ,kalAngleZ);246     //#endif247     248 //    //#if 0 // Set to 1 to print the IMU data249 //    printf("%lf %lf %lf\n",accX / 16384.0,accY / 16384.0,accZ / 16384.0);250 //    printf("%lf %lf %lf\n",gyroXrate,gyroYrate,gyroZrate);251 //    printf("%lf %lf %lf\n",magX,magY,magZ);252     //#endif253     254     //#if 0 // Set to 1 to print the temperature255     //Serial.print("\t");256     //257     //double temperature = (double)tempRaw / 340.0 + 36.53;258     //Serial.print(temperature); Serial.print("\t");259     //#endif260 //    delay(10);261 } 262 263 //****************************************264 //根据加速计刷新Pitch和Roll数据265 //这里采用两种方法计算roll和pitch，如果最上面没有#define RESTRICT_PITCH就采用第二种计算方法266 //****************************************267 void updatePitchRoll() {268     // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26269     // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2270     // It is then converted from radians to degrees271     #ifdef RESTRICT_PITCH // Eq. 25 and 26272     roll = atan2(accY,accZ) * RAD_TO_DEG;273     pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;274     #else // Eq. 28 and 29275     roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;276     pitch = atan2(-accX, accZ) * RAD_TO_DEG;277     #endif278 }279 //****************************************280 //根据磁力计刷新Yaw角281 //****************************************282 void updateYaw() { // See: http://www.freescale.com/files/sensors/doc/app_note/AN4248.pdf283     double rollAngle,pitchAngle,Bfy,Bfx;  284     285     magX *= -1; // Invert axis - this it done here, as it should be done after the calibration286     magZ *= -1;287     288     magX *= magGain[0];289     magY *= magGain[1];290     magZ *= magGain[2];291     292     magX -= magOffset[0];293     magY -= magOffset[1];294     magZ -= magOffset[2];295     296     297     rollAngle  = kalAngleX * DEG_TO_RAD;298     pitchAngle = kalAngleY * DEG_TO_RAD;299     300     Bfy = magZ * sin(rollAngle) - magY * cos(rollAngle);301     Bfx = magX * cos(pitchAngle) + magY * sin(pitchAngle) * sin(rollAngle) + magZ * sin(pitchAngle) * cos(rollAngle);302     yaw = atan2(-Bfy, Bfx) * RAD_TO_DEG;303     304     yaw *= -1;305 }