MPU6050 HMC5883 Kalman 融合算法移植-EDA365

单片机 > 单片机程序设计 > 详情

MPU6050 HMC5883 Kalman 融合算法移植

发布时间：

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

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

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")->

10 Contact information

11 -------------------

13 Kristian Lauszus, TKJ Electronics

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

15 e-mail : kristianl@tkjelectronics->com

16 */

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

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

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 };

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;

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

43 klm->bias = 0; // Reset bias

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 }

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

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;

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;

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;

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;

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];

92 return klm->angle;

93 }

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

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

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

1 #include "stm32f10x.h"

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

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

16 void I2C_GPIO_Config(void)

17 {

18 GPIO_InitTypeDef GPIO_InitStructure;

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);

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 }

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

32 void I2C_delay(void)

33 {

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

35 while(i)

36 {

37 i--;

38 }

39 }

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

42 void delay5ms(void)

43 {

44 int i=5000;

45 while(i)

46 {

47 i--;

48 }

49 }

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 }

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 }

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 }

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 }

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

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

3 typedef unsigned char uchar;

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写入数据

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

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));

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 }

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

1 #define uchar unsigned char

2 #define uint unsigned int

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

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

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

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

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);

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

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

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

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

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

47 }

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

1 #include "stm32f10x.h"

3 void USART1_Configuration(void);

4 void USART1_SendData(u8 SendData);

5 extern void Delayms(vu32 m);

7 void USART1_Configuration()

8 {

9 GPIO_InitTypeDef GPIO_InitStructure;

10 USART_InitTypeDef USART_InitStructure;

11 USART_ClockInitTypeDef USART_ClockInitStructure;

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 );

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端口

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端口

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); // 时钟参数初始化设置

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; // 硬件流控制失能

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

42 /* Configure USART1 basic and asynchronous paramters */

43 USART_Init(USART1, &USART_InitStructure);

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 }

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

1 #include "stm32f10x.h"

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 }

7、main函数文件

1 #include "stm32f10x.h"

2 #include "Kalman.h"

3 #include

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

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

8 /* IMU Data MPU6050 AND HMC5883 Data*/

9 int accX, accY, accZ;

10 int gyroX, gyroY, gyroZ;

11 int magX, magY, magZ;

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

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计算角度

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

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

23 #define MAG0MAX 603

24 #define MAG0MIN -578

26 #define MAG1MAX 542

27 #define MAG1MIN -701

29 #define MAG2MAX 547

30 #define MAG2MIN -556

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

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

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

36 double magGain[3];

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 }

程序说明：

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

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

10 gyroXangle = roll;

11 compAngleX = roll;

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

14 gyroYangle = pitch;

15 compAngleY = pitch;

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

18 gyroZangle = yaw;

19 compAngleZ = yaw;

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

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

9 // timer = micros;

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

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

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

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

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

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

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;

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;

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;

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轴的测量值~]

『本文转载自网络,版权归原作者所有,如有侵权请联系删除』

上一篇:MSC1212在绝热材料导热系数测定系统中的应用下一篇:mini2440硬件篇之Nor Flash

热门文章更多

8051单片机的函数发生器的设计

MPU6050 HMC5883 Kalman 融合算法移植

相关推荐