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

#define?RTA		57.295780	//弧度到角度需要乘的系數(shù)（180/3.14）
#define	ATR		0.0174533	//角度到弧度需要乘的系數(shù)（3.14/180）

#define?Gyro_A	0.0610351f	//陀螺儀初始化時(shí)量程是陀螺儀角速度+-2000度每秒轉(zhuǎn)化成我們所認(rèn)知的度每秒，所以1?/?（65536?/?4000）?=?0.06103515625f?
#define	Gyro_R	0.0010653f	//轉(zhuǎn)化成弧度每秒??即上式*ATR

#define?Prepare_Period	0.001	//執(zhí)行Prepare_Data函數(shù)的周期?這里設(shè)置為1ms
#define	Filter_Num	20		//濾波數(shù)組長(zhǎng)度


/*----浮點(diǎn)型角度和加速度值得定義，用于存放解算后的值----*/
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;		//目標(biāo)角度
S_float_XYZ	DIF_Angle;		//目標(biāo)角度與實(shí)際角度的偏差
S_float_XYZ	Euler_Angle;		//四元數(shù)計(jì)算出的歐拉角

int16_t	ACC_X_Buffer[Filter_Num]ACC_Y_Buffer[Filter_Num]ACC_Z_Buferr[Filter_Num];		//加速度滑動(dòng)窗口濾波數(shù)組

void?Prepare_Data（void）
{
	static?uint8_t?filter_cnt?=?0;
	int32_t?temp1?=?0temp2?=?0temp3?=?0;
	uint8_t?i;
	
/*-------6050最近一次得到的值-------*/
	MPU6050_ReadValu（）;
/*-------6050最近一次得到的值裝進(jìn)濾波數(shù)組-------*/	
	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;	
/*------角速度的積分值計(jì)算??角速度乘以周期??所得的和?即為?積分--------*/
//?	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;
}

/*------獲取姿態(tài)-------*/
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		//比例增益控制加速度計(jì)的收斂速度
#define?ki	0.008f		//積分增益控制陀螺儀偏差的收斂速度
#define?halfT?0.001f	//周期的一半

float?q0?=?1q1?=?0q2?=?0q3?=?0;		//四元數(shù)元素代表估計(jì)取向，四元素初始值
float?exInt?=?0?eyInt?=?0ezInt?=?0;	//比例積分誤差，初始值誤差
/*------四元數(shù)解析-----*/
/*
*	axayaz為在XYZ軸上的加速度
*	gxgygz為在XYZ軸上的陀螺儀
*/
void?IMUupdate（float?gx?float?gy?float?gz?float?ax?float?ay?float?az）
{
	float?norm;				//歸一化變量
	
	float?vxvyvz;
	float?exeyez;
	
/*---計(jì)算平方項(xiàng)，節(jié)省運(yùn)算時(shí)間-?其實(shí)鄙人覺得這一步并沒有什么卵用，但由于參考別人的四元數(shù)算法，鄙人也沒怎么搞太懂---*/
	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））;		//加速度數(shù)據(jù)歸一化
/*----計(jì)算?規(guī)范后的加速度-----*/
	ax?=?ax?/?norm;			?
	ay?=?ay?/?norm;
	az?=?az?/?norm;
	
/*-----估計(jì)重力方向和流量/變遷----*/
	vx?=?2*（q1q3?-?q0q2）;		////四元素中xyz的表示			
	vy?=?2*（q0q1?+?q2q3）;
	vz?=?q0q0?-?q1q1?-?q2q2?+?q3q3?;

	ex?=?（ay*vz?-?az*vy）?;????????????//向量外積在相減得到差分就是誤差
	ey?=?（az*vx?-?ax*vz）?

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----

?????文件???????4419??2016-07-06?09:58??MPU6050?互補(bǔ)濾波算法算出歐拉角\APP\IMU\imu.c

?????文件????????579??2016-07-05?21:01??MPU6050?互補(bǔ)濾波算法算出歐拉角\APP\IMU\imu.h

?????文件??????17273??2010-06-07?10:25??MPU6050?互補(bǔ)濾波算法算出歐拉角\CORE\core_cm3.c

?????文件??????85714??2011-02-09?14:59??MPU6050?互補(bǔ)濾波算法算出歐拉角\CORE\core_cm3.h

?????文件??????15503??2011-03-10?10:52??MPU6050?互補(bǔ)濾波算法算出歐拉角\CORE\startup_stm32f10x_hd.s

?????文件???????1687??2016-07-06?09:26??MPU6050?互補(bǔ)濾波算法算出歐拉角\HARDWARE\IIC\iic.c

?????文件????????855??2016-07-06?09:58??MPU6050?互補(bǔ)濾波算法算出歐拉角\HARDWARE\IIC\iic.h

?????文件????????291??2016-07-05?21:49??MPU6050?互補(bǔ)濾波算法算出歐拉角\HARDWARE\LED\led.c

?????文件????????211??2016-07-05?21:06??MPU6050?互補(bǔ)濾波算法算出歐拉角\HARDWARE\LED\led.h

?????文件???????3635??2019-04-02?15:33??MPU6050?互補(bǔ)濾波算法算出歐拉角\HARDWARE\MPU6050\mpu6050.c

?????文件???????1387??2019-04-02?15:34??MPU6050?互補(bǔ)濾波算法算出歐拉角\HARDWARE\MPU6050\mpu6050.h

?????文件???????1423??2019-04-02?15:33??MPU6050?互補(bǔ)濾波算法算出歐拉角\HARDWARE\TIM3\timet.c

?????文件????????136??2016-07-06?09:58??MPU6050?互補(bǔ)濾波算法算出歐拉角\HARDWARE\TIM3\timet.h

?????文件????????399??2011-04-23?10:24??MPU6050?互補(bǔ)濾波算法算出歐拉角\keilkilll.bat

?????文件???????3942??2019-04-02?11:21??MPU6050?互補(bǔ)濾波算法算出歐拉角\OBJ\core_cm3.crf

?????文件????????104??2019-04-02?11:21??MPU6050?互補(bǔ)濾波算法算出歐拉角\OBJ\core_cm3.d

?????文件??????11328??2019-04-02?11:21??MPU6050?互補(bǔ)濾波算法算出歐拉角\OBJ\core_cm3.o

?????文件?????349839??2019-04-02?11:22??MPU6050?互補(bǔ)濾波算法算出歐拉角\OBJ\delay.crf

?????文件???????1710??2019-04-02?11:22??MPU6050?互補(bǔ)濾波算法算出歐拉角\OBJ\delay.d

?????文件?????384412??2019-04-02?11:22??MPU6050?互補(bǔ)濾波算法算出歐拉角\OBJ\delay.o

?????文件?????401976??2019-04-02?15:33??MPU6050?互補(bǔ)濾波算法算出歐拉角\OBJ\DEMO.axf

?????文件???????2147??2019-04-02?15:33??MPU6050?互補(bǔ)濾波算法算出歐拉角\OBJ\DEMO.build_log.htm

?????文件??????65563??2019-04-02?15:33??MPU6050?互補(bǔ)濾波算法算出歐拉角\OBJ\DEMO.hex

?????文件?????117687??2019-04-02?15:33??MPU6050?互補(bǔ)濾波算法算出歐拉角\OBJ\DEMO.htm

?????文件???????1182??2019-04-02?15:33??MPU6050?互補(bǔ)濾波算法算出歐拉角\OBJ\DEMO.lnp

?????文件???????3657??2016-07-06?09:58??MPU6050?互補(bǔ)濾波算法算出歐拉角\OBJ\DEMO.tra

?????文件??????74461??2019-04-02?15:33??MPU6050?互補(bǔ)濾波算法算出歐拉角\OBJ\DEMO_DEMO.dep

?????文件?????????19??2016-07-06?09:49??MPU6050?互補(bǔ)濾波算法算出歐拉角\OBJ\ExtDll.iex

?????文件?????352010??2019-04-02?11:22??MPU6050?互補(bǔ)濾波算法算出歐拉角\OBJ\iic.crf

?????文件???????1601??2019-04-02?11:22??MPU6050?互補(bǔ)濾波算法算出歐拉角\OBJ\iic.d

............此處省略206個(gè)文件信息