我要发帖 回复

论坛元老

332

主题

6818

积分

0

专家分

兴趣点(最多三项):

Unity
UDK|Unreal
CryEngine

私信
发表时间 : 2016-9-14 19:20:49 | 浏览 : 2974    评论 : 1


本代码基于 ARM芯片 stmf10x系列开发,各位看客可以根据自己的芯片适当调整算法,最早的版本其实也是基于arduino开发的,后面也给链接了。

这是算法的大致思路:
5e7ef6e1fc4a55c988de35b66d9101dc.png


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

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

3、MPU6050的驱动代码[updataMPU6050中获取数据为什么一正一负不是很清楚,是按照GitHub上arduino版本改的]
MPU6050.c
  1. #define    SlaveAddressMPU   0x68      //定义器件5883在IIC总线中的从地址
  2. typedef unsigned char  uchar;
  3. extern int accX, accY, accZ;
  4. extern int gyroX, gyroY, gyroZ;
  5. extern uchar    SlaveAddress;       //IIC写入时的地址字节数据,+1为读取
  6. extern uchar Single_ReadI2C(uchar REG_Address);                        //读取I2C数据
  7. extern void  Single_WriteI2C(uchar REG_Address,uchar REG_data);        //向I2C写入数据
  8. //****************************************
  9. // 定义MPU6050内部地址
  10. //****************************************
  11. #define    SMPLRT_DIV        0x19    //陀螺仪采样率,典型值:0x07(125Hz)
  12. #define    CONFIG            0x1A    //低通滤波频率,典型值:0x06(5Hz)
  13. #define    GYRO_CONFIG        0x1B    //陀螺仪自检及测量范围,典型值:0x18(不自检,2000deg/s)
  14. #define    ACCEL_CONFIG    0x1C    //加速计自检、测量范围及高通滤波频率,典型值:0x01(不自检,2G,5Hz)
  15. #define    ACCEL_XOUT_H    0x3B
  16. #define    ACCEL_XOUT_L    0x3C
  17. #define    ACCEL_YOUT_H    0x3D
  18. #define    ACCEL_YOUT_L    0x3E
  19. #define    ACCEL_ZOUT_H    0x3F
  20. #define    ACCEL_ZOUT_L    0x40
  21. #define    TEMP_OUT_H        0x41
  22. #define    TEMP_OUT_L        0x42
  23. #define    GYRO_XOUT_H        0x43
  24. #define    GYRO_XOUT_L        0x44   
  25. #define    GYRO_YOUT_H        0x45
  26. #define    GYRO_YOUT_L        0x46
  27. #define    GYRO_ZOUT_H        0x47
  28. #define    GYRO_ZOUT_L        0x48
  29. #define    PWR_MGMT_1        0x6B    //电源管理,典型值:0x00(正常启用)
  30. #define    WHO_AM_I        0x75    //IIC地址寄存器(默认数值0x68,只读)
  31. #define    MPU6050_Addr    0xD0    //IIC写入时的地址字节数据,+1为读取
  32. //**************************************
  33. //初始化MPU6050
  34. //**************************************
  35. void InitMPU6050()
  36. {
  37.     SlaveAddress=MPU6050_Addr;
  38.     Single_WriteI2C(PWR_MGMT_1, 0x00);    //解除休眠状态
  39.     Single_WriteI2C(SMPLRT_DIV, 0x07);// Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  40.     Single_WriteI2C(CONFIG, 0x00);// Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  41.     Single_WriteI2C(GYRO_CONFIG, 0x00);// Set Gyro Full Scale Range to ±250deg/s
  42.     Single_WriteI2C(ACCEL_CONFIG, 0x00);// Set Accelerometer Full Scale Range to ±2g
  43.     Single_WriteI2C(PWR_MGMT_1, 0x01);// PLL with X axis gyroscope reference and disable sleep mode
  44. }
  45. //**************************************
  46. //// Get accelerometer and gyroscope values
  47. //**************************************
  48. void updateMPU6050()
  49. {
  50.     SlaveAddress=MPU6050_Addr;// Get accelerometer and gyroscope values
  51.     accX=((Single_ReadI2C(ACCEL_XOUT_H)<<8)+Single_ReadI2C(ACCEL_XOUT_L));
  52.     accY=-((Single_ReadI2C(ACCEL_YOUT_H)<<8)+Single_ReadI2C(ACCEL_YOUT_L));
  53.     accZ=((Single_ReadI2C(ACCEL_ZOUT_H)<<8)+Single_ReadI2C(ACCEL_ZOUT_L));
  54.    
  55.     gyroX=-((Single_ReadI2C(GYRO_XOUT_H)<<8)+Single_ReadI2C(GYRO_XOUT_L));
  56.     gyroY=((Single_ReadI2C(GYRO_YOUT_H)<<8)+Single_ReadI2C(GYRO_YOUT_L));
  57.     gyroZ=-((Single_ReadI2C(GYRO_ZOUT_H)<<8)+Single_ReadI2C(GYRO_ZOUT_L));   
  58. }
复制代码
4、HMC5883的驱动代码[updataHMC5883直接获取源数据,并未做大的处理]
HMC5883.c
  1. #define   uchar unsigned char
  2. #define   uint unsigned int   
  3. //定义器件在IIC总线中的从地址,根据ALT  ADDRESS地址引脚不同修改
  4. #define    HMC5883_Addr   0x3C    //磁场传感器器件地址
  5. unsigned char BUF[8];                         //接收数据缓存区                  
  6. extern uchar    SlaveAddress;               //IIC写入时的地址字节数据,+1为读取
  7. extern int magX, magY, magZ;    //hmc最原始数据
  8. extern uchar SlaveAddress;       //IIC写入时的地址字节数据,+1为读取
  9. extern uchar Single_ReadI2C(uchar REG_Address);                        //读取I2C数据
  10. extern void  Single_WriteI2C(uchar REG_Address,uchar REG_data);        //向I2C写入数据
  11. //**************************************
  12. //初始化HMC5883,根据需要请参考pdf进行修改
  13. //**************************************
  14. void InitHMC5883()
  15. {
  16.     SlaveAddress=HMC5883_Addr;
  17.     Single_WriteI2C(0x02,0x00);  //
  18.     Single_WriteI2C(0x01,0xE0);  //
  19. }
  20. //**************************************
  21. //从HMC5883连续读取6个数据放在BUF中
  22. //**************************************
  23. void updateHMC5883()
  24. {
  25.     SlaveAddress=HMC5883_Addr;
  26.     Single_WriteI2C(0x00,0x14);
  27.     Single_WriteI2C(0x02,0x00);
  28. //    Delayms(10);
  29.    
  30.     BUF[1]=Single_ReadI2C(0x03);//OUT_X_L_A
  31.     BUF[2]=Single_ReadI2C(0x04);//OUT_X_H_A
  32.     BUF[3]=Single_ReadI2C(0x07);//OUT_Y_L_A
  33.     BUF[4]=Single_ReadI2C(0x08);//OUT_Y_H_A
  34.     BUF[5]=Single_ReadI2C(0x05);//OUT_Z_L_A
  35.     BUF[6]=Single_ReadI2C(0x06);//OUT_Y_H_A
  36.    
  37.     magX=(BUF[1] << 8) | BUF[2]; //Combine MSB and LSB of X Data output register
  38.     magY=(BUF[3] << 8) | BUF[4]; //Combine MSB and LSB of Y Data output register
  39.     magZ=(BUF[5] << 8) | BUF[6]; //Combine MSB and LSB of Z Data output register
  40. //    if(magX>0x7fff)magX-=0xffff;//补码表示滴~所以要转化一下      
  41. //    if(magY>0x7fff)magY-=0xffff;   
  42. //     if(magZ>0x7fff)magZ-=0xffff;
  43. }
复制代码
5、USART简单的单字节发送的串口驱动文件
USART.c
  1. #include "stm32f10x.h"
  2. void USART1_Configuration(void);
  3. void USART1_SendData(u8 SendData);
  4. extern void Delayms(vu32 m);
  5. void USART1_Configuration()
  6. {
  7.     GPIO_InitTypeDef GPIO_InitStructure;
  8.     USART_InitTypeDef USART_InitStructure;
  9.     USART_ClockInitTypeDef  USART_ClockInitStructure;
  10.     RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB ,ENABLE );//| RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOD, ENABLE  );
  11.     RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1 |RCC_APB2Periph_USART1, ENABLE  );
  12.     /* Configure USART1 Tx (PA.09) as alternate function push-pull */
  13.     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;                 //    选中管脚9
  14.     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;         // 复用推挽输出
  15.     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;         // 最高输出速率50MHz
  16.     GPIO_Init(GPIOA, &GPIO_InitStructure);                 // 选择A端口
  17.    
  18.     /* Configure USART1 Rx (PA.10) as input floating */
  19.     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;              //选中管脚10
  20.     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;      //浮空输入
  21.     GPIO_Init(GPIOA, &GPIO_InitStructure);                  //选择A端口
  22.     USART_ClockInitStructure.USART_Clock = USART_Clock_Disable;            // 时钟低电平活动
  23.     USART_ClockInitStructure.USART_CPOL = USART_CPOL_Low;                // 时钟低电平
  24.     USART_ClockInitStructure.USART_CPHA = USART_CPHA_2Edge;                // 时钟第二个边沿进行数据捕获
  25.     USART_ClockInitStructure.USART_LastBit = USART_LastBit_Disable;        // 最后一位数据的时钟脉冲不从SCLK输出
  26.     /* Configure the USART1 synchronous paramters */
  27.     USART_ClockInit(USART1, &USART_ClockInitStructure);                    // 时钟参数初始化设置
  28.    
  29.     USART_InitStructure.USART_BaudRate = 9600;                          // 波特率为:115200
  30.     USART_InitStructure.USART_WordLength = USART_WordLength_8b;              // 8位数据
  31.     USART_InitStructure.USART_StopBits = USART_StopBits_1;                  // 在帧结尾传输1个停止位
  32.     USART_InitStructure.USART_Parity = USART_Parity_No ;                  // 奇偶失能
  33.     USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;    // 硬件流控制失能
  34.    
  35.     USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;          // 发送使能+接收使能
  36.     /* Configure USART1 basic and asynchronous paramters */
  37.     USART_Init(USART1, &USART_InitStructure);
  38.    
  39.     /* Enable USART1 */
  40.     USART_ClearFlag(USART1, USART_IT_RXNE);             //清中断,以免一启用中断后立即产生中断
  41.     USART_ITConfig(USART1,USART_IT_RXNE, ENABLE);        //使能USART1中断源
  42.     USART_Cmd(USART1, ENABLE);                            //USART1总开关:开启
  43. }
  44. void  USART1_SendData(u8 SendData)
  45. {
  46.     USART_SendData(USART1, SendData);
  47.     while(USART_GetFlagStatus(USART1, USART_FLAG_TC)==RESET);
  48. }
复制代码
6、非精确延时函数集[其他文件所需的一些延时放在这里]
DELAY.c
  1. #include "stm32f10x.h"
  2. void Delay(vu32 nCount)
  3. {
  4.     for(; nCount != 0; nCount--);
  5. }
  6. void Delayms(vu32 m)
  7. {
  8.     u32 i;   
  9.     for(; m != 0; m--)   
  10.         for (i=0; i<50000; i++);
  11. }
复制代码
7、main函数文件:
main.c
  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. struct Kalman kalmanX, kalmanY, kalmanZ; // Create the Kalman instances
  6. /* IMU Data MPU6050 AND HMC5883 Data*/
  7. int accX, accY, accZ;
  8. int gyroX, gyroY, gyroZ;
  9. int magX, magY, magZ;
  10. double roll, pitch, yaw; // Roll and pitch are calculated using the accelerometer while yaw is calculated using the magnetometer
  11. double gyroXangle, gyroYangle, gyroZangle; // Angle calculate using the gyro only 只用陀螺仪计算角度
  12. double compAngleX, compAngleY, compAngleZ; // Calculated angle using a complementary filter  用电磁计计算角度
  13. double kalAngleX, kalAngleY, kalAngleZ; // Calculated angle using a Kalman filter    用kalman计算角度
  14. //uint32_t timer,micros; //上一次时间与当前时间
  15. uint8_t i2cData[14]; // Buffer for I2C data
  16. #define MAG0MAX 603
  17. #define MAG0MIN -578
  18. #define MAG1MAX 542
  19. #define MAG1MIN -701
  20. #define MAG2MAX 547
  21. #define MAG2MIN -556
  22. #define RAD_TO_DEG 57.295779513082320876798154814105  // 弧度转角度的转换率
  23. #define DEG_TO_RAD 0.01745329251994329576923690768489 // 角度转弧度的转换率
  24. float magOffset[3] = { (MAG0MAX + MAG0MIN) / 2, (MAG1MAX + MAG1MIN) / 2, (MAG2MAX + MAG2MIN) / 2 };
  25. double magGain[3];
  26. void  SYSTICK_Configuration(void);                                //系统滴答中断配置
  27. void  RCC_Configuration(void);
  28. void  updatePitchRoll(void);                                    //根据加速计刷新Pitch和Roll数据
  29. void  updateYaw(void);                                            //根据磁力计刷新Yaw角
  30. void  InitAll(void);                                            //循环前的初始化
  31. void  func(void);                                                //循环里的内容
  32. extern void InitMPU6050(void);                                    //初始化MPU6050
  33. extern void InitHMC5883(void);                                    //初始化HMC5883
  34. extern void updateMPU6050(void);                                //Get accelerometer and gyroscope values
  35. extern void updateHMC5883(void);                                //Get magnetometer values
  36. extern void USART1_Configuration(void);                            //串口初始化
  37. extern void USART1_SendData(u8 SendData);                        //串口发送函数
  38. extern void I2C_GPIO_Config(void);                                //I2C初始化函数
  39. /****************************************************************************
  40. * 名    称:int main(void)
  41. * 功    能:主函数
  42. * 入口参数:无
  43. * 出口参数:无
  44. * 说    明:
  45. * 调用方法:无
  46. ****************************************************************************/
  47. int main(void)
  48. {
  49.       RCC_Configuration();                   //系统时钟配置   
  50.       USART1_Configuration();
  51.       I2C_GPIO_Config();
  52.       InitHMC5883();
  53.     InitMPU6050();
  54.     InitAll();   
  55. //    SYSTICK_Configuration();               
  56.      while(1)
  57.     {
  58.         func();
  59.       }
  60. }
  61. ///*
  62. //系统滴答中断配置
  63. //*/
  64. //void SYSTICK_Configuration(void)
  65. //{
  66. //    micros=0;//全局计数时间归零
  67. //     if (SysTick_Config(72000))            //时钟节拍中断时1000ms一次  用于定时
  68. //       {
  69. //        /* Capture error */
  70. ////        while (1);
  71. //       }
  72. //}
  73. ///*
  74. //当前时间++.为了防止溢出当其大于2^20时,令其归零
  75. //*/
  76. //void SysTickHandler(void)
  77. //{
  78. //     micros++;
  79. //    if(micros>(1<<20))
  80. //          micros=0;
  81. //}
  82. /****************************************************************************
  83. * 名    称:void RCC_Configuration(void)
  84. * 功    能:系统时钟配置为72MHZ
  85. * 入口参数:无
  86. * 出口参数:无
  87. * 说    明:
  88. * 调用方法:无
  89. ****************************************************************************/
  90. void RCC_Configuration(void)
  91. {   
  92.     SystemInit();
  93. }
  94. void InitAll()
  95. {
  96.     /* Set Kalman and gyro starting angle */
  97.     updateMPU6050();
  98.     updateHMC5883();
  99.     updatePitchRoll();
  100.     updateYaw();
  101.    
  102.     setAngle(&kalmanX,roll); // First set roll starting angle
  103.     gyroXangle = roll;
  104.     compAngleX = roll;
  105.    
  106.     setAngle(&kalmanY,pitch); // Then pitch
  107.     gyroYangle = pitch;
  108.     compAngleY = pitch;
  109.    
  110.     setAngle(&kalmanZ,yaw); // And finally yaw
  111.     gyroZangle = yaw;
  112.     compAngleZ = yaw;
  113.    
  114. //    timer = micros; // Initialize the timer   
  115. }
  116. void send(double xx,double yy,double zz)
  117. {
  118.     int    a[3];
  119.      u8 i,sendData[12];      
  120.     a[0]=(int)xx;a[1]=(int)yy;a[2]=(int)zz;
  121.     for(i=0;i<3;i++)
  122.     {
  123.         if(a[i]<0){
  124.             sendData[i*4]='-';
  125.             a[i]=-a[i];
  126.         }
  127.         else sendData[i*4]=' ';
  128.         sendData[i*4+1]=(u8)(a[i]%1000/100+0x30);
  129.         sendData[i*4+2]=(u8)(a[i]%100/10+0x30);
  130.         sendData[i*4+3]=(u8)(a[i]%10+0x30);
  131.     }
  132.     for(i=0;i<12;i++)
  133.     {
  134.         USART1_SendData(sendData[i]);
  135.     }
  136.     USART1_SendData(0x0D);
  137.     USART1_SendData(0x0A);
  138. }
  139. void func()
  140. {
  141.     double gyroXrate,gyroYrate,gyroZrate,dt=0.01;
  142.     /* Update all the IMU values */
  143.     updateMPU6050();
  144.     updateHMC5883();
  145.    
  146. //    dt = (double)(micros - timer) / 1000; // Calculate delta time
  147. //    timer = micros;
  148. //    if(dt<0)dt+=(1<<20);    //时间是周期性的,有可能当前时间小于上次时间,因为这个周期远大于两次积分时间,所以最多相差1<<20
  149.     /* Roll and pitch estimation */
  150.     updatePitchRoll();             //用采集的加速计的值计算roll和pitch的值
  151.     gyroXrate = gyroX / 131.0;     // Convert to deg/s    把陀螺仪的角加速度按照当初设定的量程转换为°/s
  152.     gyroYrate = gyroY / 131.0;     // Convert to deg/s
  153.    
  154.     #ifdef RESTRICT_PITCH        //如果上面有#define RESTRICT_PITCH就采用这种方法计算,防止出现-180和180之间的跳跃
  155.     // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  156.     if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
  157.         setAngle(&kalmanX,roll);
  158.         compAngleX = roll;
  159.         kalAngleX = roll;
  160.         gyroXangle = roll;
  161.     } else
  162.     kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
  163.    
  164.     if (fabs(kalAngleX) > 90)
  165.         gyroYrate = -gyroYrate; // Invert rate, so it fits the restricted accelerometer reading
  166.     kalAngleY = getAngle(&kalmanY,pitch, gyroYrate, dt);
  167.     #else
  168.     // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  169.     if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
  170.         kalmanY.setAngle(pitch);
  171.         compAngleY = pitch;
  172.         kalAngleY = pitch;
  173.         gyroYangle = pitch;
  174.     } else
  175.     kalAngleY = getAngle(&kalmanY, pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
  176.    
  177.     if (abs(kalAngleY) > 90)
  178.         gyroXrate = -gyroXrate; // Invert rate, so it fits the restricted accelerometer reading
  179.     kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
  180.     #endif
  181.    
  182.    
  183.     /* Yaw estimation */
  184.     updateYaw();
  185.     gyroZrate = gyroZ / 131.0; // Convert to deg/s
  186.     // This fixes the transition problem when the yaw angle jumps between -180 and 180 degrees
  187.     if ((yaw < -90 && kalAngleZ > 90) || (yaw > 90 && kalAngleZ < -90)) {
  188.         setAngle(&kalmanZ,yaw);
  189.         compAngleZ = yaw;
  190.         kalAngleZ = yaw;
  191.         gyroZangle = yaw;
  192.     } else
  193.     kalAngleZ = getAngle(&kalmanZ, yaw, gyroZrate, dt); // Calculate the angle using a Kalman filter
  194.    
  195.    
  196.     /* Estimate angles using gyro only */
  197.     gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
  198.     gyroYangle += gyroYrate * dt;
  199.     gyroZangle += gyroZrate * dt;
  200.     //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate from the Kalman filter
  201.     //gyroYangle += kalmanY.getRate() * dt;
  202.     //gyroZangle += kalmanZ.getRate() * dt;
  203.    
  204.     /* Estimate angles using complimentary filter */
  205.     compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
  206.     compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
  207.     compAngleZ = 0.93 * (compAngleZ + gyroZrate * dt) + 0.07 * yaw;
  208.    
  209.     // Reset the gyro angles when they has drifted too much
  210.     if (gyroXangle < -180 || gyroXangle > 180)
  211.         gyroXangle = kalAngleX;
  212.     if (gyroYangle < -180 || gyroYangle > 180)
  213.         gyroYangle = kalAngleY;
  214.     if (gyroZangle < -180 || gyroZangle > 180)
  215.         gyroZangle = kalAngleZ;
  216.    
  217.    
  218.     send(roll,pitch,yaw);
  219. //    send(gyroXangle,gyroYangle,gyroZangle);
  220. //    send(compAngleX,compAngleY,compAngleZ);
  221. //    send(kalAngleX,kalAngleY,kalAngleZ);
  222. //    send(kalAngleY,compAngleY,gyroYangle);
  223.     /* Print Data */
  224. //    //#if 1
  225. //    printf("%lf %lf %lf %lf\n",roll,gyroXangle,compAngleX,kalAngleX);
  226. //    printf("%lf %lf %lf %lf\n",pitch,gyroYangle,compAngleY,kalAngleY);
  227. //    printf("%lf %lf %lf %lf\n",yaw,gyroZangle,compAngleZ,kalAngleZ);
  228.     //#endif
  229.    
  230. //    //#if 0 // Set to 1 to print the IMU data
  231. //    printf("%lf %lf %lf\n",accX / 16384.0,accY / 16384.0,accZ / 16384.0);
  232. //    printf("%lf %lf %lf\n",gyroXrate,gyroYrate,gyroZrate);
  233. //    printf("%lf %lf %lf\n",magX,magY,magZ);
  234.     //#endif
  235.    
  236.     //#if 0 // Set to 1 to print the temperature
  237.     //Serial.print("\t");
  238.     //
  239.     //double temperature = (double)tempRaw / 340.0 + 36.53;
  240.     //Serial.print(temperature); Serial.print("\t");
  241.     //#endif
  242. //    delay(10);
  243. }
  244. //****************************************
  245. //根据加速计刷新Pitch和Roll数据
  246. //这里采用两种方法计算roll和pitch,如果最上面没有#define RESTRICT_PITCH就采用第二种计算方法
  247. //****************************************
  248. void updatePitchRoll() {
  249.     // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
  250.     // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  251.     // It is then converted from radians to degrees
  252.     #ifdef RESTRICT_PITCH // Eq. 25 and 26
  253.     roll = atan2(accY,accZ) * RAD_TO_DEG;
  254.     pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
  255.     #else // Eq. 28 and 29
  256.     roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  257.     pitch = atan2(-accX, accZ) * RAD_TO_DEG;
  258.     #endif
  259. }
  260. //****************************************
  261. //根据磁力计刷新Yaw角
  262. //****************************************
  263. void updateYaw() { // See: http://www.freescale.com/files/sensors/doc/app_note/AN4248.pdf
  264.     double rollAngle,pitchAngle,Bfy,Bfx;  
  265.    
  266.     magX *= -1; // Invert axis - this it done here, as it should be done after the calibration
  267.     magZ *= -1;
  268.    
  269.     magX *= magGain[0];
  270.     magY *= magGain[1];
  271.     magZ *= magGain[2];
  272.    
  273.     magX -= magOffset[0];
  274.     magY -= magOffset[1];
  275.     magZ -= magOffset[2];
  276.    
  277.    
  278.     rollAngle  = kalAngleX * DEG_TO_RAD;
  279.     pitchAngle = kalAngleY * DEG_TO_RAD;
  280.    
  281.     Bfy = magZ * sin(rollAngle) - magY * cos(rollAngle);
  282.     Bfx = magX * cos(pitchAngle) + magY * sin(pitchAngle) * sin(rollAngle) + magZ * sin(pitchAngle) * cos(rollAngle);
  283.     yaw = atan2(-Bfy, Bfx) * RAD_TO_DEG;
  284.    
  285.     yaw *= -1;
  286. }
复制代码

程序说明:
  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.     /* Roll and pitch estimation */
  12.     updatePitchRoll();             //用采集的加速计的值计算roll和pitch的值
  13.     gyroXrate = gyroX / 131.0;     // Convert to deg/s    把陀螺仪的角加速度按照当初设定的量程转换为°/s
  14.     gyroYrate = gyroY / 131.0;     // Convert to deg/s
  15.    
  16.     #ifdef RESTRICT_PITCH        //如果上面有#define RESTRICT_PITCH就采用这种方法计算,防止出现-180和180之间的跳跃
  17.     // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  18.     if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
  19.         setAngle(&kalmanX,roll);
  20.         compAngleX = roll;
  21.         kalAngleX = roll;
  22.         gyroXangle = roll;
  23.     } else
  24.     kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
  25.    
  26.     if (fabs(kalAngleX) > 90)
  27.         gyroYrate = -gyroYrate; // Invert rate, so it fits the restricted accelerometer reading
  28.     kalAngleY = getAngle(&kalmanY,pitch, gyroYrate, dt);
  29.     #else
  30.     // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  31.     if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
  32.         kalmanY.setAngle(pitch);
  33.         compAngleY = pitch;
  34.         kalAngleY = pitch;
  35.         gyroYangle = pitch;
  36.     } else
  37.     kalAngleY = getAngle(&kalmanY, pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
  38.    
  39.     if (abs(kalAngleY) > 90)
  40.         gyroXrate = -gyroXrate; // Invert rate, so it fits the restricted accelerometer reading
  41.     kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
  42.     #endif
  43.    
  44.    
  45.     /* Yaw estimation */
  46.     updateYaw();
  47.     gyroZrate = gyroZ / 131.0; // Convert to deg/s
  48.     // This fixes the transition problem when the yaw angle jumps between -180 and 180 degrees
  49.     if ((yaw < -90 && kalAngleZ > 90) || (yaw > 90 && kalAngleZ < -90)) {
  50.         setAngle(&kalmanZ,yaw);
  51.         compAngleZ = yaw;
  52.         kalAngleZ = yaw;
  53.         gyroZangle = yaw;
  54.     } else
  55.     kalAngleZ = getAngle(&kalmanZ, yaw, gyroZrate, dt); // Calculate the angle using a Kalman filter
  56.    
  57.    
  58.     /* Estimate angles using gyro only */
  59.     gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
  60.     gyroYangle += gyroYrate * dt;
  61.     gyroZangle += gyroZrate * dt;
  62.     //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate from the Kalman filter
  63.     //gyroYangle += kalmanY.getRate() * dt;
  64.     //gyroZangle += kalmanZ.getRate() * dt;
  65.    
  66.     /* Estimate angles using complimentary filter */互补滤波算法
  67.     compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
  68.     compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
  69.     compAngleZ = 0.93 * (compAngleZ + gyroZrate * dt) + 0.07 * yaw;
  70.    
  71.     // Reset the gyro angles when they has drifted too much
  72.     if (gyroXangle < -180 || gyroXangle > 180)
  73.         gyroXangle = kalAngleX;
  74.     if (gyroYangle < -180 || gyroYangle > 180)
  75.         gyroYangle = kalAngleY;
  76.     if (gyroZangle < -180 || gyroZangle > 180)
  77.         gyroZangle = kalAngleZ;
  78.    
  79.    
  80.     send(roll,pitch,yaw);
  81. //    send(gyroXangle,gyroYangle,gyroZangle);
  82. //    send(compAngleX,compAngleY,compAngleZ);
  83. //    send(kalAngleX,kalAngleY,kalAngleZ);
  84. //    send(kalAngleY,compAngleY,gyroYangle);
  85. }
复制代码




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

u=1283604294,3826281550&amp;fm=21&amp;gp=0.jpg

PS:相关链接



最近VR访客

相关帖子

UE4   |   虚幻引擎   |   Unity VR    |    Hololens
我的领域 评论于2016-9-21 09:28:16
感谢楼主分享,这真是技术性的知识啊,要是在开发VR硬件遇到困难的时候,这个还真的提供了很大的帮助呢。

手机版|VR开发网 |网站地图 统计  津ICP备16006248号 网安备12019202000257

GMT+8, 2018-10-21 04:17 AM

返回顶部