/********************?（C）?COPYRIGHT?2014?ANO?Tech?********************************
??*?作者???：匿名科創(chuàng)
?*?文件名??：imu.c
?*?描述????：姿態(tài)解算
?*?官網(wǎng)????：www.anotc.com
?*?淘寶????：anotc.taobao.com
?*?技術(shù)Q群?：190169595
**********************************************************************************/

#include?“imu.h“
#include?“include.h“
#include?“ak8975.h“
#include?“mymath.h“

#define?Kp?0.6f????????????????	//?proportional?gain?governs?rate?of?convergence?to?accelerometer/magnetometer
#define?Ki?0.1f????????????????	//?0.001??integral?gain?governs?rate?of?convergence?of?gyroscope?biases

#define?IMU_INTEGRAL_LIM??（?2.0f?*ANGLE_TO_RADIAN?）
#define?NORM_ACC_LPF_HZ?10??		//（Hz）
#define?REF_ERR_LPF_HZ??1				//（Hz）

xyz_f_t?reference_v;
ref_t?	ref;

xyz_f_t?Gravity_Vec;??				//解算的重力向量
	
float?RollPitchYaw;????				//姿態(tài)角

float?ref_q[4]?=?{1000};
float?norm_accnorm_q;
float?norm_acc_lpf;

extern?u8?fly_ready;
void?IMUupdate（float?half_Tfloat?gx?float?gy?float?gz?float?ax?float?ay?float?azfloat?*rolfloat?*pitfloat?*yaw）?
{		
	float?ref_err_lpf_hz;
	static?float?mag_norm?mag_tmp_xmag_tmp_yyaw_mag;

	mag_norm?=?my_sqrt（ak8975.Mag_Val.x?*?ak8975.Mag_Val.x?+?ak8975.Mag_Val.y?*?ak8975.Mag_Val.y）;
	
	if（ak8975.Mag_Val.x?!=?0?&&?ak8975.Mag_Val.y?!=?0?）
	{
		mag_tmp_x?=?（float）ak8975.Mag_Val.x?/mag_norm;
		mag_tmp_y?=?（float）ak8975.Mag_Val.y?/mag_norm;	
		yaw_mag?=?fast_atan2（mag_tmp_ymag_tmp_x）?*57.3f;
	}
	//=============================================================================
	//?計(jì)算等效重力向量
	reference_v.x?=?2*（ref_q[1]*ref_q[3]?-?ref_q[0]*ref_q[2]）;
	reference_v.y?=?2*（ref_q[0]*ref_q[1]?+?ref_q[2]*ref_q[3]）;
	reference_v.z?=?1?-?2*（ref_q[1]*ref_q[1]?+?ref_q[2]*ref_q[2]）;//ref_q[0]*ref_q[0]?-?ref_q[1]*ref_q[1]?-?ref_q[2]*ref_q[2]?+?ref_q[3]*ref_q[3];

	
	//這是把四元數(shù)換算成《方向余弦矩陣》中的第三列的三個(gè)元素。
	//根據(jù)余弦矩陣和歐拉角的定義，地理坐標(biāo)系的重力向量，轉(zhuǎn)到機(jī)體坐標(biāo)系，正好是這三個(gè)元素。
	//所以這里的vx\y\z，其實(shí)就是當(dāng)前的歐拉角（即四元數(shù)）的機(jī)體坐標(biāo)參照系上，換算出來的重力單位向量。???????
	//=============================================================================


	//?計(jì)算加速度向量的模
	norm_acc?=?my_sqrt（ax*ax?+?ay*ay?+?az*az）;???
	norm_acc_lpf?+=??NORM_ACC_LPF_HZ?*（6.28f?*half_T）?*（norm_acc?-?norm_acc_lpf）;??//10hz?*3.14?*?2*0.001


	if（ABS（ax）<4400?&&?ABS（ay）<4400?&&?ABS（az）<4400?）
	{	
		//把加計(jì)的三維向量轉(zhuǎn)成單位向量。
		ax?=?ax?/?norm_acc;//4096.0f;
		ay?=?ay?/?norm_acc;//4096.0f;
		az?=?az?/?norm_acc;//4096.0f;?
		
		if（?3800?		{
			/*?叉乘得到誤差?*/
			ref.err_tmp.x?=?ay*reference_v.z?-?az*reference_v.y;
			ref.err_tmp.y?=?az*reference_