碼上歡樂
相關內容    簡體    繁體

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

本文轉載自   查看原文   2014-08-15 22:29   12037    stm32/ kalman/ hmc5883/ MPU6050/ 硬件_stm32F10x

 

一、卡爾曼濾波九軸融合算法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:相關鏈接

 

 

 

 
 


免責聲明！

本站轉載的文章為個人學習借鑒使用，本站對版權不負任何法律責任。如果侵犯了您的隱私權益，請聯系本站郵箱yoyou2525@163.com刪除。



猜您在找 STM32 磁場傳感器HMC5883 MPU6050工作原理及STM32控制MPU6050 mpu6050姿勢融合（歐拉角計算） 第44章 MPU6050傳感器—姿態檢測—零死角玩轉STM32-F429系列 MPU6050濾波、姿態融合（一階互補、卡爾曼） HMC5883 3軸磁阻測方位角 玩轉X-CTR100 l STM32F4 l MPU6050加速度陀螺儀傳感器 MPU6050驅動及如何讀取參數值 iic通訊 FPGA實現 mpu6050為例 基於I2C總線的MPU6050學習筆記
 
粵ICP備18138465號   © 2018-2025 CODEPRJ.COM