//=====================================================================================================
//?AHRS.c
//?S.O.H.?Madgwick
//?25th?August?2010
//=================================================================================================//?Description:
//
//?四元的“DCM的過濾器”的實施[Mayhony等]。采用了磁性失真
//?我的過濾器[Madgwick]省去了補償算法

引用
//?通量（BX?BZ）的方向，是預定義的和限制的磁場效應

扭曲到偏航
//?axis?only.
//
//?User?must?define?‘halfT‘?as?the?（sample?period?/?2）?and?the?filter?gains?‘Kp‘?and?‘Ki‘.
//
//?Global?variables?‘q0‘?‘q1‘?‘q2‘?‘q3‘?are?the?quaternion?elements?representing?the?estimated
//?方向.??見我概述使用這個應用程序中的四元報告。
//
//?用戶必須調用?‘AHRSupdate（）‘?校準每個采樣周期和解析?gyroscope?（‘gx‘?‘gy‘?‘gz‘）
//?加速度計?（‘ax‘?‘ay‘?‘ay‘）?and?磁力儀（‘mx‘?‘my‘?‘mz‘）?data.??Gyroscope?units?are
//?弧度/秒，加速度計和磁強計單位歸為載體無關，是歸為載體。
//
//=====================================================================================================

//----------------------------------------------------------------------------------------------------
//?Header?files

#include?“AHRS.h“
#include?

//----------------------------------------------------------------------------------------------------
//?Definitions

#define?Kp?2.0f			//?比例增益支配收斂率accelerometer/magnetometer
#define?Ki?0.005f		//?積分增益執政速率陀螺儀的銜接gyroscopeases
#define?halfT?0.5f		//?采樣周期的一半

//---------------------------------------------------------------------------------------------------
//?Variable?definitions

float?q0?=?1?q1?=?0?q2?=?0?q3?=?0;	//?四元數的元素，代表估計方向
float?exInt?=?0?eyInt?=?0?ezInt?=?0;	//?按比例縮小積分誤差

//====================================================================================================
//?Function
//====================================================================================================

void?AHRSupdate（float?gx?float?gy?float?gz?float?ax?float?ay?float?az?float?mx?float?my?float?mz）?{
	float?norm;
	float?hx?hy?hz?bx?bz;
	float?vx?vy?vz?