MPU6050姿态解算STM32源码卡尔曼滤波

本人亲测好用的MPU6050姿态解算STM32源码（卡尔曼滤波）算法，希望帮助大家

#include “control.h“	
#include “filter.h“	
  /**************************************************************************
作者：平衡小车之家
我的淘宝小店：http://shop114407458.taobao.com/
**************************************************************************/
int TIM1_UP_IRQHandler（void）  
{    
	if（TIM1->SR&0X0001）//5ms定时中断
	{   
		  TIM1->SR&=~（1<<0）;                                       //===清除定时器1中断标志位		 

			Get_Angle（Way_Angle）;                                    //===更新姿态	
		
	}       	
	 return 0;	  
} 

/**************************************************************************
函数功能：获取角度
入口参数：获取角度的算法 1：无  2：卡尔曼 3：互补滤波
返回  值：无
**************************************************************************/
void Get_Angle（u8 way）
{ 
	    float Accel_YAccel_XAccel_ZGyro_Y;
	    if（way==1）                                      //DMP没有涉及到严格的时序问题，在主函数读取
			{	
			}			
      else
      {
			Gyro_Y=（I2C_ReadOneByte（devAddrMPU6050_RA_GYRO_YOUT_H）<<8）+I2C_ReadOneByte（devAddrMPU6050_RA_GYRO_YOUT_L）;    //读取Y轴陀螺仪
			Accel_Z=（I2C_ReadOneByte（devAddrMPU6050_RA_ACCEL_ZOUT_H）<<8）+I2C_ReadOneByte（devAddrMPU6050_RA_ACCEL_ZOUT_L）;    //读取Z轴陀螺仪
		  Accel_X=（I2C_ReadOneByte（devAddrMPU6050_RA_ACCEL_XOUT_H）<<8）+I2C_ReadOneByte（devAddrMPU6050_RA_ACCEL_XOUT_L）; //读取X轴加速度记
	 		if（Gyro_Y>32768）  Gyro_Y-=65536;     //数据类型转换  也可通过short强制类型转换
			if（Accel_Z>32768）  Accel_Z-=65536;     //数据类型转换
	  	if（Accel_X>32768） Accel_X-=65536;    //数据类型转换
	   	Accel_Y=atan2（Accel_XAccel_Z）*180/PI;                 //计算与地面的夹角	
		  Gyro_Y=Gyro_Y/16.4;                                    //陀螺仪量程转换	
      if（Way_Angle==2）		  	Kalman_Filter（Accel_Y-Gyro_Y）;//卡尔曼滤波	
			else if（Way_Angle==3）   Yijielvbo（Accel_Y-Gyro_Y）;    //互补滤波
	    Angle_Balance=angle;                                   //更新平衡倾角
	  	}
}

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2017-09-27 17:05  MPU6050姿态解算STM32源码（卡尔曼滤波）\
     目录           0  2017-09-18 15:52  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\
     目录           0  2017-07-05 14:45  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\CONTROL\
     文件        1997  2015-08-14 22:03  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\CONTROL\control.c
     文件         375  2015-08-14 21:35  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\CONTROL\control.h
     目录           0  2017-07-05 14:45  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\DMP\
     文件       19342  2012-12-14 11:16  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\DMP\dmpKey.h
     文件        6765  2012-12-14 11:16  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\DMP\dmpmap.h
     文件       84104  2015-07-09 14:05  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\DMP\inv_mpu.c
     文件        4335  2015-07-09 14:05  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\DMP\inv_mpu.h
     文件       57839  2015-07-09 14:05  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\DMP\inv_mpu_dmp_motion_driver.c
     文件        3386  2015-07-09 14:05  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\DMP\inv_mpu_dmp_motion_driver.h
     目录           0  2017-07-05 14:45  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\DataScope_DP\
     文件        2891  2015-07-09 14:05  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\DataScope_DP\DataScope_DP.C
     文件         586  2015-07-09 14:05  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\DataScope_DP\DataScope_DP.h
     目录           0  2017-07-05 14:45  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\MPU6050\
     文件       11370  2015-08-14 21:55  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\MPU6050\MPU6050.c
     文件       14792  2015-08-14 21:48  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\MPU6050\mpu6050.h
     目录           0  2017-07-05 14:45  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\filter\
     文件        2108  2015-07-09 14:05  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\filter\filter.c
     文件         399  2015-07-09 14:05  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\filter\filter.h
     目录           0  2017-07-05 14:45  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\show\
     文件        3809  2015-07-14 12:11  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\show\show.c
     文件         332  2015-07-09 14:05  MPU6050姿态解算STM32源码（卡尔曼滤波）\BALANCE\show\show.h
     目录           0  2017-07-05 14:45  MPU6050姿态解算STM32源码（卡尔曼滤波）\HARDWARE\
     目录           0  2017-07-05 14:45  MPU6050姿态解算STM32源码（卡尔曼滤波）\HARDWARE\IIC\
     文件       10557  2015-07-09 14:05  MPU6050姿态解算STM32源码（卡尔曼滤波）\HARDWARE\IIC\IOI2C.c
     文件        3119  2015-07-09 14:05  MPU6050姿态解算STM32源码（卡尔曼滤波）\HARDWARE\IIC\IOI2C.h
     目录           0  2017-07-05 14:45  MPU6050姿态解算STM32源码（卡尔曼滤波）\HARDWARE\TIMER\
     文件         608  2015-08-14 21:38  MPU6050姿态解算STM32源码（卡尔曼滤波）\HARDWARE\TIMER\timer.c
     文件         114  2015-06-25 10:44  MPU6050姿态解算STM32源码（卡尔曼滤波）\HARDWARE\TIMER\timer.h
............此处省略115个文件信息