[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
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 }
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 }
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 }
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 }
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 }
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 }
程序说明：
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:相关链接
- GitHub上面的基于arduino的工程：https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter.git
- 3轴加速计网页pdf版使用详细资料（公式，计算）：http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
- 加速计和磁力计倾斜补偿算法网页pdf资料：http://www.freescale.com/files/sensors/doc/app_note/AN4248.pdf
- 上述工程代码（你得自己解决dt问题）：http://pan.baidu.com/s/1gdlATFH
- MPU6050寄存器中文版：http://pan.baidu.com/s/1gdIKUK7
- MPU6050中文资料：http://pan.baidu.com/s/1bnkxjhP
- MPU6050数据轻松分析（基于arduino的kalman滤波讲解含代码）：http://pan.baidu.com/s/1eQvMtX4
- pitch yaw roll 相关知识(1)：http://blog.163.com/vipwdp@126/blog/static/150224366201281935518196/
- pitch yaw roll 相关知识(2)：http://www.cnblogs.com/wqj1212/archive/2010/11/21/1883033.html
- pitch yaw roll 相关知识(3)：http://www.cnblogs.com/tclikang/archive/2012/11/09/2761988.html
- 四元数与欧拉角知识：http://www.cnblogs.com/wqj1212/archive/2010/11/21/1883033.html