mpu6050互补滤波算法角度串口输出stm32

通过stm32对mpu6050用互补滤波算法算出角度用串口输出，数据和dmp一样稳定

#include “imu.h“
#include “mpu6050.h“
#include “math.h“

#define RTA		57.295780	//弧度到角度需要乘的系数（180/3.14）
#define	ATR		0.0174533	//角度到弧度需要乘的系数（3.14/180）

#define Gyro_A	0.0610351f	//陀螺仪初始化时量程是陀螺仪角速度+-2000度每秒转化成我们所认知的度每秒，所以1 / （65536 / 4000） = 0.06103515625f 
#define	Gyro_R	0.0010653f	//转化成弧度每秒  即上式*ATR

#define Prepare_Period	0.001	//执行Prepare_Data函数的周期 这里设置为1ms
#define	Filter_Num	20		//滤波数组长度


/*----浮点型角度和加速度值得定义，用于存放解算后的值----*/
S_float_XYZ Angle_Valu = {000};
S_float_XYZ GYRO_Valu = {000};
S_short_XYZ ACC_Avg;		//平均值滤波后的ACC
S_float_XYZ	GYRO_I;		//陀螺仪积分
S_float_XYZ	EXP_Angled;		//目标角度
S_float_XYZ	DIF_Angle;		//目标角度与实际角度的偏差
S_float_XYZ	Euler_Angle;		//四元数计算出的欧拉角

int16_t	ACC_X_Buffer[Filter_Num]ACC_Y_Buffer[Filter_Num]ACC_Z_Buferr[Filter_Num];		//加速度滑动窗口滤波数组

void Prepare_Data（void）
{
	static uint8_t filter_cnt = 0;
	int32_t temp1 = 0temp2 = 0temp3 = 0;
	uint8_t i;
	
/*-------6050最近一次得到的值-------*/
	MPU6050_ReadValu（）;
/*-------6050最近一次得到的值装进滤波数组-------*/	
	ACC_X_Buffer[filter_cnt] = ACC_Valu_First.X;
	ACC_Y_Buffer[filter_cnt] = ACC_Valu_First.Y;
	ACC_Z_Buferr[filter_cnt] = ACC_Valu_First.Z;

/*-------软件一阶惯性滤波-------*/	
	for（i=0;i	{
		temp1 += ACC_X_Buffer[i];
		temp2 += ACC_Y_Buffer[i];
		temp3 += ACC_Z_Buferr[i];
	}
	ACC_Avg.X = temp1 / Filter_Num;
	ACC_Avg.Y = temp2 / Filter_Num;
	ACC_Avg.Z = temp3 / Filter_Num;
	filter_cnt++;
	if（filter_cnt==Filter_Num）	filter_cnt=0;	
/*------角速度的积分值计算  角速度乘以周期  所得的和 即为 积分--------*/
// 	GYRO_I.X += GYRO_Valu_First.X*Gyro_A*Prepare_Period;
// 	GYRO_I.Y += GYRO_Valu_First.Y*Gyro_A*Prepare_Period;
	GYRO_I.Z += GYRO_Valu_First.Z*Gyro_A*Prepare_Period;
}

/*------获取姿态-------*/
void Get_Attitude（void）
{
	IMUupdate（GYRO_Valu_First.X * Gyro_R
				GYRO_Valu_First.Y * Gyro_R	
				GYRO_Valu_First.Z * Gyro_R
				ACC_Avg.XACC_Avg.YACC_Avg.Z
	）;
}


#define kp 10.0f		//比例增益控制加速度计的收敛速度
#define ki	0.008f		//积分增益控制陀螺仪偏差的收敛速度
#define halfT 0.001f	//周期的一半

float q0 = 1q1 = 0q2 = 0q3 = 0;		//四元数元素代表估计取向，四元素初始值
float exInt = 0 eyInt = 0ezInt = 0;	//比例积分误差，初始值误差
/*------四元数解析-----*/
/*
*	axayaz为在XYZ轴上的加速度
*	gxgygz为在XYZ轴上的陀螺仪
*/
void IMUupdate（float gx float gy float gz float ax float ay float az）
{
	float norm;				//归一化变量
	
	float vxvyvz;
	float exeyez;
	
/*---计算平方项，节省运算时间- 其实鄙人觉得这一步并没有什么卵用，但由于参考别人的四元数算法，鄙人也没怎么搞太懂---*/
	float q0q0 = q0 * q0;
	float q0q1 = q0 * q1;
	float q0q2 = q0 * q2;
	
	float q1q1 = q1 * q1;
	float q1q3 = q1 * q3;
	
	float q2q2 = q2 * q2;
	float q2q3 = q2 * q3;
	
	float q3q3 = q3 * q3;
	
	if（ax*ay*az == 0）
			return;
	
	norm = sqrt（（ax * ax） + （ay * ay） + （az *az））;		//加速度数据归一化
/*----计算 规范后的加速度-----*/
	ax = ax / norm;			 
	ay = ay / norm;
	az = az / norm;
	
/*-----估计重力方向和流量/变迁----*/
	vx = 2*（q1q3 - q0q2）;		////四元素中xyz的表示			
	vy = 2*（q0q1 + q2q3）;
	vz = q0q0 - q1q1 - q2q2 + q3q3 ;

	ex = （ay*vz - az*vy） ;            //向量外积在相减得到差分就是误差
	ey = （az*vx - ax*vz） 

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       4419  2016-07-06 09:58  MPU6050 互补滤波算法算出欧拉角\APP\IMU\imu.c

     文件        579  2016-07-05 21:01  MPU6050 互补滤波算法算出欧拉角\APP\IMU\imu.h

     文件      17273  2010-06-07 10:25  MPU6050 互补滤波算法算出欧拉角\CORE\core_cm3.c

     文件      85714  2011-02-09 14:59  MPU6050 互补滤波算法算出欧拉角\CORE\core_cm3.h

     文件      15503  2011-03-10 10:52  MPU6050 互补滤波算法算出欧拉角\CORE\startup_stm32f10x_hd.s

     文件       1687  2016-07-06 09:26  MPU6050 互补滤波算法算出欧拉角\HARDWARE\IIC\iic.c

     文件        855  2016-07-06 09:58  MPU6050 互补滤波算法算出欧拉角\HARDWARE\IIC\iic.h

     文件        291  2016-07-05 21:49  MPU6050 互补滤波算法算出欧拉角\HARDWARE\LED\led.c

     文件        211  2016-07-05 21:06  MPU6050 互补滤波算法算出欧拉角\HARDWARE\LED\led.h

     文件       3635  2019-04-02 15:33  MPU6050 互补滤波算法算出欧拉角\HARDWARE\MPU6050\mpu6050.c

     文件       1387  2019-04-02 15:34  MPU6050 互补滤波算法算出欧拉角\HARDWARE\MPU6050\mpu6050.h

     文件       1423  2019-04-02 15:33  MPU6050 互补滤波算法算出欧拉角\HARDWARE\TIM3\timet.c

     文件        136  2016-07-06 09:58  MPU6050 互补滤波算法算出欧拉角\HARDWARE\TIM3\timet.h

     文件        399  2011-04-23 10:24  MPU6050 互补滤波算法算出欧拉角\keilkilll.bat

     文件       3942  2019-04-02 11:21  MPU6050 互补滤波算法算出欧拉角\OBJ\core_cm3.crf

     文件        104  2019-04-02 11:21  MPU6050 互补滤波算法算出欧拉角\OBJ\core_cm3.d

     文件      11328  2019-04-02 11:21  MPU6050 互补滤波算法算出欧拉角\OBJ\core_cm3.o

     文件     349839  2019-04-02 11:22  MPU6050 互补滤波算法算出欧拉角\OBJ\delay.crf

     文件       1710  2019-04-02 11:22  MPU6050 互补滤波算法算出欧拉角\OBJ\delay.d

     文件     384412  2019-04-02 11:22  MPU6050 互补滤波算法算出欧拉角\OBJ\delay.o

     文件     401976  2019-04-02 15:33  MPU6050 互补滤波算法算出欧拉角\OBJ\DEMO.axf

     文件       2147  2019-04-02 15:33  MPU6050 互补滤波算法算出欧拉角\OBJ\DEMO.build_log.htm

     文件      65563  2019-04-02 15:33  MPU6050 互补滤波算法算出欧拉角\OBJ\DEMO.hex

     文件     117687  2019-04-02 15:33  MPU6050 互补滤波算法算出欧拉角\OBJ\DEMO.htm

     文件       1182  2019-04-02 15:33  MPU6050 互补滤波算法算出欧拉角\OBJ\DEMO.lnp

     文件       3657  2016-07-06 09:58  MPU6050 互补滤波算法算出欧拉角\OBJ\DEMO.tra

     文件      74461  2019-04-02 15:33  MPU6050 互补滤波算法算出欧拉角\OBJ\DEMO_DEMO.dep

     文件         19  2016-07-06 09:49  MPU6050 互补滤波算法算出欧拉角\OBJ\ExtDll.iex

     文件     352010  2019-04-02 11:22  MPU6050 互补滤波算法算出欧拉角\OBJ\iic.crf

     文件       1601  2019-04-02 11:22  MPU6050 互补滤波算法算出欧拉角\OBJ\iic.d

............此处省略206个文件信息