[stm32] MPU6050 HMC5883 Kalman 融合算法移植

 

一、卡尔曼滤波九轴融合算法stm32尝试

 

1、Kalman滤波文件[.h已经封装为结构体]

  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
Kalman.h

2、I2C总线代码[这里把MPU和HMC挂接到上面,通过改变SlaveAddress的值来和不同的设备通信]

  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无ACK

107 {

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= I2C_RadeByte();

196     I2C_NoAck();

197     I2C_Stop();

198     //return TRUE;

199     return REG_data;

200 }
I2C.c

3、MPU6050的驱动代码[updataMPU6050中获取数据为什么一正一负不是很清楚,是按照GitHub上arduino版本改的]

 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    0x3B

19 #define    ACCEL_XOUT_L    0x3C

20 #define    ACCEL_YOUT_H    0x3D

21 #define    ACCEL_YOUT_L    0x3E

22 #define    ACCEL_ZOUT_H    0x3F

23 #define    ACCEL_ZOUT_L    0x40

24 #define    TEMP_OUT_H        0x41

25 #define    TEMP_OUT_L        0x42

26 #define    GYRO_XOUT_H        0x43

27 #define    GYRO_XOUT_L        0x44    

28 #define    GYRO_YOUT_H        0x45

29 #define    GYRO_YOUT_L        0x46

30 #define    GYRO_ZOUT_H        0x47

31 #define    GYRO_ZOUT_L        0x48

32 #define    PWR_MGMT_1        0x6B    //电源管理,典型值:0x00(正常启用)

33 #define    WHO_AM_I        0x75    //IIC地址寄存器(默认数值0x68,只读)

34 #define    MPU6050_Addr    0xD0    //IIC写入时的地址字节数据,+1为读取

35 //**************************************

36 //初始化MPU6050

37 //**************************************

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) = 1000Hz

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

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

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

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

47 }

48 //**************************************

49 //// Get accelerometer and gyroscope values

50 //**************************************

51 void updateMPU6050()

52 {

53     SlaveAddress=MPU6050_Addr;// Get accelerometer and gyroscope values

54 

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 }
MPU6050.c

4、HMC5883的驱动代码[updataHMC5883直接获取源数据,并未做大的处理]

 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_A

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

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

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

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

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

39     

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

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

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

43 

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

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

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

47 }
HMC5883.c

5、USART简单的单字节发送的串口驱动文件

 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;                 //    选中管脚9

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

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

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

21     

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

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

24     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;                          // 波特率为:115200

36     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 }
USART.c

6、非精确延时函数集[其他文件所需的一些延时放在这里]

 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 }
DELAY.c

7、main函数文件

  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 angle

116     gyroXangle = roll;

117     compAngleX = roll;

118     

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

120     gyroYangle = pitch;

121     compAngleY = pitch;

122     

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

124     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 time

162 //    timer = micros;

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

164 

165     /* Roll and pitch estimation */

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

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

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

169     

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

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

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

173         setAngle(&kalmanX,roll);

174         compAngleX = roll;

175         kalAngleX = roll;

176         gyroXangle = roll;

177     } else

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

179     

180     if (fabs(kalAngleX) > 90)

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

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

183     #else

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

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

186         kalmanY.setAngle(pitch);

187         compAngleY = pitch;

188         kalAngleY = pitch;

189         gyroYangle = pitch;

190     } else

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

192     

193     if (abs(kalAngleY) > 90)

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

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

196     #endif

197     

198     

199     /* Yaw estimation */

200     updateYaw();

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

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

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

204         setAngle(&kalmanZ,yaw);

205         compAngleZ = yaw;

206         kalAngleZ = yaw;

207         gyroZangle = yaw;

208     } else

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

210     

211     

212     /* Estimate angles using gyro only */

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

214     gyroYangle += gyroYrate * dt;

215     gyroZangle += gyroZrate * dt;

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

217     //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 filter

222     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 much

226     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 1

243 //    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     //#endif

247     

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

249 //    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     //#endif

253     

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

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

256     //

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

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

259     //#endif

260 //    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. 26

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

270     // It is then converted from radians to degrees

271     #ifdef RESTRICT_PITCH // Eq. 25 and 26

272     roll = atan2(accY,accZ) * RAD_TO_DEG;

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

274     #else // Eq. 28 and 29

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

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

277     #endif

278 }

279 //****************************************

280 //根据磁力计刷新Yaw角

281 //****************************************

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

283     double rollAngle,pitchAngle,Bfy,Bfx;  

284     

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

286     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 }
main.c

 

 

程序说明:

 1 int main(void)

 2 {

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

 4       USART1_Configuration();

 5       I2C_GPIO_Config();

 6       InitHMC5883();

 7     InitMPU6050();

 8     InitAll();    

 9 //    SYSTICK_Configuration();                

10      while(1)

11     {

12         func();

13       }

14 }
  • 主函数首先初始化系统时钟、串口、I2C总线、HMC5883磁力计和MPU6050加速计&陀螺仪,这里重点讲InitAll()函数和func()函数
 1 void InitAll()

 2 {

 3     /* Set Kalman and gyro starting angle */

 4     updateMPU6050();

 5     updateHMC5883();

 6     updatePitchRoll();

 7     updateYaw();

 8     

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

10     gyroXangle = roll;

11     compAngleX = roll;

12     

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

14     gyroYangle = pitch;

15     compAngleY = pitch;

16     

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

18     gyroZangle = yaw;

19     compAngleZ = yaw;

20     

21 //    timer = micros; // Initialize the timer    

22 }
  • 第4、5两行从传感器中读取原数据,第6行函数根据加速计的值由空间几何的知识刷新Pitch和Roll数据,第7行函数根据复杂计算(这个实在看不懂,大概是磁力计有偏差,一方面进行误差校正,另一方面还用到了kalman滤波的数据,挺麻烦的)其实就是刷新yaw的值。
  • 后面把kalman滤波值、陀螺仪计量值、磁力计计算值都赋值为上面计算的roll、pitch、yaw的值。
 1 void func()

 2 {

 3     double gyroXrate,gyroYrate,gyroZrate,dt=0.01;

 4     /* Update all the IMU values */

 5     updateMPU6050();

 6     updateHMC5883();

 7     

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

 9 //    timer = micros;

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

11 

12     /* Roll and pitch estimation */

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

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

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

16     

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

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

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

20         setAngle(&kalmanX,roll);

21         compAngleX = roll;

22         kalAngleX = roll;

23         gyroXangle = roll;

24     } else

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

26     

27     if (fabs(kalAngleX) > 90)

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

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

30     #else

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

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

33         kalmanY.setAngle(pitch);

34         compAngleY = pitch;

35         kalAngleY = pitch;

36         gyroYangle = pitch;

37     } else

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

39     

40     if (abs(kalAngleY) > 90)

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

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

43     #endif

44     

45     

46     /* Yaw estimation */

47     updateYaw();

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

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

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

51         setAngle(&kalmanZ,yaw);

52         compAngleZ = yaw;

53         kalAngleZ = yaw;

54         gyroZangle = yaw;

55     } else

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

57     

58     

59     /* Estimate angles using gyro only */

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

61     gyroYangle += gyroYrate * dt;

62     gyroZangle += gyroZrate * dt;

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

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

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

66     

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

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

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

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

71     

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

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

74         gyroXangle = kalAngleX;

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

76         gyroYangle = kalAngleY;

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

78         gyroZangle = kalAngleZ;

79     

80     

81     send(roll,pitch,yaw);

82 //    send(gyroXangle,gyroYangle,gyroZangle);

83 //    send(compAngleX,compAngleY,compAngleZ);

84 //    send(kalAngleX,kalAngleY,kalAngleZ);

85 //    send(kalAngleY,compAngleY,gyroYangle);

86 } 
  • 5、6两行获取传感器原数据
  • 8~10行计算两次测量的时间差dt[因为我采用很多方法试验来计算时间差都不奏效,所以最终还是放弃了这种算法,还是用我原来的DMP算法,DMP对水平方向的很好,z方向的不好,要用磁力计来纠正!可以参考这里面的算法!]
  • 13~56行是用kalman滤波来求当前的3个角并稳值
  • 60~62行是用陀螺仪的角速度积分获得当前陀螺仪测量的3个角度值
  • 67~70行使用互补滤波算法对磁力计当前测量3个角的值进行计算
  • 72~78行是稳值
  • 81行是串口发送

PS:总的来说按照arduino的代码进行照抄移植成c语言版的,当前卡在了如何较为准确的计算dt,即:两次测量的时间差(这里为了测试我给了dt一个定值0.01s,这是很不准确的做法!!!)[我采用定时器的方法总是会莫名的跑偏,我想可能是中断的影响,好吧,还是用原来实验的DMP吧,这个算法看似高大上,其实比较占MCU的资源啦,自带的DMP也存在一些缺陷,对水平方向的偏角测量较为精准,误差在1°左右,而在z轴方向上的误差较大,容易跑偏,所以还要用磁力计进行纠正Z轴的测量值~]

 

PS:相关链接

 

 

 

 
 

你可能感兴趣的:(stm32)