#include "windows.h"
#include "SensorImpl.h"
#include "stdio.h"
#include "quaternion.h"
#include "math.h"
#include "UsbHid_CAPI.h"
#include "MyCls.h"
extern "C" FILE* fpdata;
Quaternion GlobalQ;
BOOL GlobalQ_Updating;
float  LocX,LocY,LocZ;
float Yaw,Pitch,Roll;
extern FILE* Getfpdata();
extern int global_calibration_flag;
extern int global_Show;
UINT8 global_key[4] = { 0 };
float gxc=0, gyc=0, gzc=0;
int	gyro_cali_count = 0;
#define  GYRO_CALIBRATION_SAMPLE_NUMBER 3000
float gx0=0, gy0=0, gz0=0;
BOOL GlobalMag_Updating = false;
Vector3 GlobalMag = {0};
static bool HmdStandByState;

void MagFilter(float *mx,float *my,float *mz,float sita);
void GyroComputeDrift(float *gx, float *gy, float *gz);


void LowPassFilterYaw(float *z, float alpha);

bool IsHmdIntoStandBy(float *Pitch, float *Roll, float dTime);


void UnpackSensor(const UCHAR* buffer, long* x, long* y, long* z)
{
 
    struct {INT32 x:21;} s;

    *x = s.x = (buffer[0] << 13) | (buffer[1] << 5) | ((buffer[2] & 0xF8) >> 3);
    *y = s.x = ((buffer[2] & 0x07) << 18) | (buffer[3] << 10) | (buffer[4] << 2) |
                    ((buffer[5] & 0xC0) >> 6);
    *z = s.x = ((buffer[5] & 0x3F) << 15) | (buffer[6] << 7) | (buffer[7] >> 1);
}
Vector3 DM_GetMag()
{
	Vector3  v = { 0 };
	static Vector3 VLast = { 0 };
	 	if (!GlobalMag_Updating)
	 	{	
				VLast = GlobalMag;
	 	}
	v = VLast;
	return v;
}
void GetControllerSensor(unsigned char usb_input_buffer[])
{
	static UINT8 updatecnt = 0;
	INT16 MagX,MagY,MagZ;
	INT16 MagX1, MagY1, MagZ1;
	long GX,GY,GZ;
	long AX,AY,AZ;
	UINT NowTime;
	float ax, ay, az, gx, gy, gz, mx, my, mz, mx1, my1, mz1;;
	static float mxlast=0,mylast=0,mzlast=0;
	static int Dtime = 0;
	static UINT LastTime = 0;
	float q0,q1,q2,q3;



	NowTime = *(UINT16* )(usb_input_buffer+2);
	if(LastTime != 0)
	{   
		if(NowTime >= LastTime)
		{
			Dtime = NowTime - LastTime;
			LastTime = NowTime;
		}else
		{
			Dtime = (65535 - LastTime) + NowTime;
			//printf("NowTime = %d  LastTime = %d Dtime = %d\n",NowTime,LastTime,Dtime);
			LastTime = NowTime;
		}

	}else
	{
		Dtime = 1;
		LastTime = NowTime;
	}
	if (global_Show==5)
	{
		//clrscr();
		back2zero();
		printf("Hex数据\n\n");
		for (int i = 0; i < 64; i++)
		{
			if ((i%16)==0)
			{
				printf("\n");
			}
			//if (global_debug) 
				printf("%2x ", usb_input_buffer[i]);

		}
		printf("\n");
	}
	//if(Dtime>3)printf("NowTime = %d  LastTime = %d Dtime = %d\n",NowTime,LastTime,Dtime);
// 	MagX1 = *(INT16 *)(usb_input_buffer + 46);
// 	MagY1 = *(INT16 *)(usb_input_buffer + 48);
// 	MagZ1 = *(INT16 *)(usb_input_buffer + 50);

	MagX = *(INT16 *)(usb_input_buffer + 56);
	MagY = *(INT16 *)(usb_input_buffer + 58);
	MagZ = *(INT16 *)(usb_input_buffer + 60);

	UnpackSensor(usb_input_buffer+8, &AX,&AY,&AZ);
	UnpackSensor(usb_input_buffer+16, &GX,&GY,&GZ);


	ax = (float)0.0001*AX;
	ay = (float)0.0001*AY;
	az = (float)0.0001*AZ;

	gx = (float)0.0001*GX;
	gy = (float)0.0001*GY;
	gz = (float)0.0001*GZ;
	float x = 16.4 * 180 / 3.14159265358979323846F;

	if (global_Show == 1)
	{
		printf("gx = %f,gy = %f gz =%f\n", gx*x, gy*x, gz*x);
	}
	GyroComputeDrift(&gx, &gy, &gz);
	if (global_calibration_flag == 1)
	{
		if (gyro_cali_count > GYRO_CALIBRATION_SAMPLE_NUMBER)
		{
			gyro_cali_count = 0;
			gxc = gyc = gzc = 0;
		}
		gyro_cali_count++;
		gxc += gx*x;
		gyc += gy*x;
		gzc += gz*x;
		if (gyro_cali_count == GYRO_CALIBRATION_SAMPLE_NUMBER)
		{
			gx0 = gxc / gyro_cali_count;
			gy0 = gyc / gyro_cali_count;
			gz0 = gzc / gyro_cali_count;
			gyro_cali_count++;
			global_calibration_flag = 0;
			printf("%f %f %f\n", gx0, gy0, gz0);
		}
	}

// 	q0 = *(float *)(usb_input_buffer+24);
// 	q1 = *(float *)(usb_input_buffer+28);
// 	q2 = *(float *)(usb_input_buffer+32);
	// 	q3 = *(float *)(usb_input_buffer+36);
// 	q0 = *(float *)(usb_input_buffer + 32);
// 	q1 = *(float *)(usb_input_buffer + 36);
// 	q2 = *(float *)(usb_input_buffer + 40);
// 	q3 = *(float *)(usb_input_buffer + 44);
	global_key[0] = usb_input_buffer[24];
	//global_key[0] = usb_input_buffer[25];
	//global_key[0] = usb_input_buffer[26];
	global_key[3] = usb_input_buffer[27];
// 	GlobalQ_Updating = TRUE;
// 	GlobalQ.w = q0;
// 	GlobalQ.x = q2;
// 	GlobalQ.y = q3;
// 	GlobalQ.z = q1;
// 	GlobalQ_Updating = FALSE;
// 	Pitch = asin(-2.0f*(q3*q1 - q0*q2))* (180.0f / 3.141592f);							    /*轴进行四元数到欧拉角的转换                                            */
// 	Yaw = atan2(q2*q1 + q0*q3, 0.5f - q2*q2 - q3*q3)* (180.0f / 3.141592f);
// 	Roll = atan2(q2*q3 + q0*q1, 0.5f - q2*q2 - q1*q1)* (180.0f / 3.141592f);
// 
// 	if (global_Show == 4)
// 		printf("Yaw = %f,Pitch = %f Roll =%f\n", Yaw, Pitch, Roll);

	mx = (float)MagX / 16;
	my = (float)MagY / 16;
	mz = (float)MagZ / 16;
	if (global_calibration_flag==3)
	{
		GlobalMag_Updating = true;
		mx = (float)MagX;
		my = (float)MagY;
		mz = (float)MagZ;
// 		mx1 = (float)MagX1;
// 		my1 = (float)MagY1;
// 		mz1 = (float)MagZ1;
		GlobalMag.x = (float)MagX;
		GlobalMag.y = (float)MagY;
		GlobalMag.z = (float)MagZ;
		GlobalMag_Updating = false;	
	}
// 	mx = (float)MagX;
// 	my = (float)MagY;
// 	mz = (float)MagZ;


	//Pitch = asin(-2.0f*(q3*q1-q0*q2))* (180.0f /3.141592f);							    /*轴进行四元数到欧拉角的转换                                            */
	//Yaw   = atan2(q2*q1 + q0*q3,0.5f - q2*q2 - q3*q3)* (180.0f /3.141592f);
	//Roll  = atan2(q2*q3 + q0*q1,0.5f - q2*q2 - q1*q1)* (180.0f /3.141592f);
	//

	//printf("Yaw = %f,Pitch = %f Roll =%f\n",Yaw,Pitch,Roll);

if(mxlast == 0&&mylast == 0&&mylast == 0)
	{
		mxlast = mx;
		mylast = my;
		mzlast = mz;

		MagFilter(&mx, &my, &mz, 0.95f);
		for (int i = 0; i < 200; i++)
			AHRSupdate(0, 0, 0, az, ax, ay, mz, mx, my, 1, Dtime * 400);
	}
	else
	{
		updatecnt++;
		if (updatecnt > 10)
			updatecnt = 0;
		if (mx == mxlast&&my == my&&mz == mzlast && updatecnt < 10)
		{
			AHRSupdate(gz,gx,gy,az,ax,ay,mz,mx,my,1,Dtime);
			//printf("数据完全相同...\n");
		}
		else
		{	
			mxlast = mx;
			mylast = my;
			mzlast = mz;
// 			if (global_Show == 3)
// 				printf("mx = %f  my= %f  mz= %f\n", mx, my, mz);
// 			if (global_calibration_flag == 3)
// 			{
// 				
// 				if (Getfpdata() != NULL)
// 				{
// 					fprintf(Getfpdata(), "%f %f %f\n", mx, my, mz);
// 					Sleep(5);
// 				}
// 			}
			MagFilter(&mx,&my,&mz,0.95f);

// 			printf("mx = %f  my= %f  mz= %f\n", mx, my, mz);
// 			if (Getfpdata()!=NULL)
// 			{
// 				fprintf(Getfpdata(), "%f %f %f\n", mx, my, mz);
// 			}		
			AHRSupdate(gz,gx,gy,az,ax,ay,mz,mx,my,1,Dtime);


		}
	}

	//printf("mx = %f  my= %f  mz= %f\n",mx,my,mz);



}


/**********************************************************************************************************
* Function Name  : AHRSupdate
* 输入           : 三轴加速度计、陀螺仪、磁力计的值
* 描述           ：加速度计、陀螺仪、磁力计融合得到的欧拉角

                   1）这个算法基于标准笛卡尔右手坐标系，z轴朝上，x轴朝人，y轴朝右
				   2）传感器的方向需要变换到和改算法定义的坐标轴相同。

* 修改           ：2016/11/5 : 根据磁力计采样率比较低的特点，设置了2种模式：1、融合磁力计 2、不融合磁力计
                   2016/11/6 ：改进了四元数更新算法，原来的四元数更新算法会导致累计误差。
				   2016/11/9 : 规定了本算法运行的坐标轴，重写了四元数到欧拉角变换的代码。
***********************************************************************************************************/


float   q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float exInt = 0, eyInt = 0, ezInt = 0;                                                 /*缩放积分误差	                             */
#define Kp 2.5f                                                                        /*加速度计和磁力计对陀螺仪的比例修正参数		 */
#define Ki 0.005f                                                                      /*加速度计和磁力计对陀螺仪的积分修正参数		 */
#define  Kd   0.0f                                                                    
static int AHRS_Init = 0;
void ResetAHRS(){ AHRS_Init = 0; q0 = 1; q1 = 0; q2 = 0; q3 = 0; }
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, char useMag, int Dtime)
{
	/*原算法的定义*/
	float norm, halfT;
	float hx, hy, hz, bz, by;
	float vx, vy, vz, wx, wy, wz;
	float ex, ey, ez;
	float qw, qx, qy, qz;
	float AccelAjust, MagAjust;
	/*方便之后的程序使用，减少计算时间*/
	float q0q0 = q0*q0;    float q0q1 = q0*q1;
	float q0q2 = q0*q2;    float q0q3 = q0*q3;
	float q1q1 = q1*q1;    float q1q2 = q1*q2;
	float q1q3 = q1*q3;    float q2q2 = q2*q2;
	float q2q3 = q2*q3;    float q3q3 = q3*q3;
	if (AHRS_Init > 200)
	{
		halfT = 0.0005f*Dtime;		                                                        /*得到每次姿态更新的周期的一半t = (1/SampleFrq) * 0.5		           */
	}
	else
	{
		if (useMag == 1)
		{
			halfT = 0.1;
			gx = gy = gz = 0;
			AHRS_Init++;
		}
		else
		{
			return;
		}
	}

	norm = invSqrt(ax*ax + ay*ay + az*az);       								       /*把加速度计向量转换为单位向量*/

	if ((1.0f / norm) > 1.6 || (1.0f / norm) < 0.4){
		AccelAjust = 0;
		MagAjust = 1.2f;

	}
	else{
		AccelAjust = 1.0f;
		MagAjust = 1.0f;
	}
	//		printf("加速度过大...\n");
	//		getchar();

	ax = ax * norm;
	ay = ay * norm;
	az = az * norm;																	   /*把磁力计向量转换为单位向量  */
	norm = invSqrt(mx*mx + my*my + mz*mz);
	mx = mx * norm;
	my = my * norm;
	mz = mz * norm;


	hx = 2.0f*(mx*(0.5f - q2q2 - q3q3) + my*(q1q2 - q0q3) + mz*(q1q3 + q0q2));		   /*把(mx,my,mz)转换到地理坐标系的(hx,hy,hz),利用H = Q^-1 * M            */
	hy = 2.0f*(mx*(q1q2 + q0q3) + my*(0.5f - q1q1 - q3q3) + mz*(q2q3 - q0q1));
	hz = 2.0f*(mx*(q1q3 - q0q2) + my*(q2q3 + q0q1) + mz*(0.5f - q1q1 - q2q2));

	/*计的数值为(bx,by,0),所以我们不关注hx的数值                           */
	// bx = sqrtf((hx*hx) + (hy*hy));												   /*使磁力计正交化*/
	by = sqrtf((hx*hx) + (hy*hy));
	bz = hz;

	/*v代表的是把地理坐标系的加速度(0,0,1g)转换到机体坐标系的加            */
	vx = 2.0f*(q1q3 - q0q2);														   /*速度计(ax,ay,az),其实就是用R*(0,0,1),R为旋转矩阵，此矩阵可			  */
	vy = 2 * (q0q1 + q2q3);															   /*由四元数转换得到													  */
	vz = q0q0 - q1q1 - q2q2 + q3q3;

	wx = 2.0f*(by*(q1q2 + q0q3) + bz*(q1q3 - q0q2));							    /*把正交化的H即B从地理坐标系转换到飞行器坐标系，利用W = Q * B          */
	wy = 2.0f*(by*(0.5f - q1q1 - q3q3) + bz*(q0q1 + q2q3));								/*这里认为bx = 0                                                   */
	wz = 2.0f*(by*(q2q3 - q0q1) + bz*(0.5f - q1q1 - q2q2));



	/*如果使用磁力计，则融合磁力计*/
	if (useMag != 0)
	{																				    /*用当前姿态向量和加速度的姿态向量做叉乘，乘积越小说明两个向量方向越相同*/
		ex = AccelAjust*(ay*vz - az*vy) + MagAjust*(my*wz - mz*wy);
		ey = AccelAjust*(az*vx - ax*vz) + MagAjust*(mz*wx - mx*wz);
		ez = AccelAjust*(ax*vy - ay*vx) + MagAjust*(mx*wy - my*wx);
	}
	else
	{

		ex = AccelAjust*(ay*vz - az*vy);
		ey = AccelAjust*(az*vx - ax*vz);											     /*因为加速度计没有校准，ez不靠谱*/
		ez = AccelAjust*(ax*vy - ay*vx);
		ez = 0;
	}

	if (ex != 0.0f && ey != 0.0f && ez != 0.0f)
	{
		/*这里使用了PID调节的方式，对角速度修正,Ki 是积分修正，Kp是直接修正    */
		exInt = exInt + ex*Ki * halfT;			      									/*对误差进行积分                                                       */
		eyInt = eyInt + ey*Ki * halfT;
		ezInt = ezInt + ez*Ki * halfT;
		gx = gx + Kp*(1 + Kd*fabsf(ex))*ex + exInt;														/*使用比例和积分综合对陀螺仪进行修正，由于存在积分修正，所以才能确保回 */
		gy = gy + Kp*(1 + Kd*fabsf(ey))*ey + eyInt;														/*到期望回到的位置                                                     */
		gz = gz + Kp*(1 + Kd*fabsf(ez))*ez + ezInt;
	}


	qw = (-q1*gx - q2*gy - q3*gz)*halfT;											    /*利用一阶龙格库塔法对四元数进行更新*/
	qx = (q0*gx + q2*gz - q3*gy)*halfT;
	qy = (q0*gy - q1*gz + q3*gx)*halfT;
	qz = (q0*gz + q1*gy - q2*gx)*halfT;

	q0 = q0 + qw;
	q1 = q1 + qx;
	q2 = q2 + qy;
	q3 = q3 + qz;

	//QuaternionFilter(&q1,&q2,&q3,0.98f);

	//LowPassFilterYaw(&q3,0.98);

	norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);										/*对四元数进行规范化，即四元数模为1*/

	q0 = q0 * norm;       //w
	q1 = q1 * norm;       //x
	q2 = q2 * norm;       //y
	q3 = q3 * norm;       //z
	/*对原变换公式进行重写，避免了以前坐标轴混乱的问题，按照标准数学右手坐标*/
// 	Pitch = asin(-2.0f*(q3*q1 - q0*q2))* (180.0f / 3.141592f);							    /*轴进行四元数到欧拉角的转换                                            */
// 	Yaw = atan2(q2*q1 + q0*q3, 0.5f - q2*q2 - q3*q3)* (180.0f / 3.141592f);
// 	Roll = atan2(q2*q3 + q0*q1, 0.5f - q2*q2 - q1*q1)* (180.0f / 3.141592f);

// 	/*对原变换公式进行重写，避免了以前坐标轴混乱的问题，按照标准y上x右z人*/
// 	Pitch = asin(-2.0f*(q2*q3 - q0*q1))* (180.0f / 3.141592f);							    /*轴进行四元数到欧拉角的转换                                            */
// 	Yaw = atan2(q1*q3 + q0*q2, 0.5f - q1*q1 - q2*q2)* (180.0f / 3.141592f);
// 	Roll = atan2(q1*q2 + q0*q3, 0.5f - q1*q1 - q3*q3)* (180.0f / 3.141592f);

	/*对原变换公式进行重写，避免了以前坐标轴混乱的问题，按照标准y上z右x前*/
	Pitch = asin(-2.0f*(q2*q1 - q0*q3))* (180.0f / 3.141592f);							    /*轴进行四元数到欧拉角的转换                                            */
	Yaw = atan2(q3*q1 + q0*q2, 0.5f - q3*q3 - q2*q2)* (180.0f / 3.141592f);
	Roll = atan2(q3*q2 + q0*q1, 0.5f - q3*q3 - q1*q1)* (180.0f / 3.141592f);

	LowPassFilterYaw(&Yaw, 0.9f);

// 	if (global_Show == 4)
// 		printf("Yaw = %f,Pitch = %f Roll =%f\n", Yaw, Pitch, Roll);
	if (AHRS_Init > 200)
	{
		GlobalQ_Updating = TRUE;
		GlobalQ.w = q0;
		GlobalQ.x = q2;
		GlobalQ.y = q3;
		GlobalQ.z = q1;
		GlobalQ_Updating = FALSE;
	}


	IsHmdIntoStandBy(&Pitch, &Roll, 1.0f);
}


void MagFilter(float *mx, float *my, float *mz, float sita)
{
	static float xlast = 0, ylast = 0, zlast = 0;
	if (xlast == 0 && ylast == 0 && ylast == 0)
	{
		xlast = *mx;
		ylast = *my;
		zlast = *mz;
	}
	else
	{
		xlast = sita*xlast + (1 - sita)*(*mx);
		ylast = sita*ylast + (1 - sita)*(*my);
		zlast = sita*zlast + (1 - sita)*(*mz);
	}
}


/*******************************************************************************
* Function Name  : invSqrt
* Description    : 快速计算 1/Sqrt(x)，源自雷神3，神奇的0x5f3759df
* 输入           : x
* 输出           ：1/sqrt(x)
*******************************************************************************/
float invSqrt(float x)
{
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long*)&y;
	i = 0x5f3759df - (i >> 1);
	y = *(float*)&i;
	y = y * (1.5f - (halfx * y * y));
	return y;
}


void GyroComputeDrift(float *gx, float *gy, float *gz)
{
	static float GxOffect = 0.0f, GyOffect = 0.0f, GzOffect = 0.0f;
	static float GxOffectTmp = 0.0f, GyOffectTmp = 0.0f, GzOffectTmp = 0.0f;
	static float GxTmp = 0.0f, GyTmp = 0.0f, GzTmp = 0.0f;
	static int count = 0;

	*gx = *gx - GxOffect;
	*gy = *gy - GyOffect;
	*gz = *gz - GzOffect;

	if (HmdStandByState == true){
		GxTmp = GxTmp + *gx;
		GyTmp = GyTmp + *gy;
		GzTmp = GzTmp + *gz;
		count++;
		if (count == 3000){
			//printf("陀螺仪校准完成\n");

			GxOffectTmp = GxTmp / count;
			GyOffectTmp = GyTmp / count;
			GzOffectTmp = GzTmp / count;

			GxOffect = GxOffect + GxOffectTmp;
			GyOffect = GyOffect + GyOffectTmp;
			GzOffect = GzOffect + GzOffectTmp;

			GxTmp = 0.0f;
			GyTmp = 0.0f;
			GzTmp = 0.0f;
			count = 0;
		}
	}
	else{
		count = 0;
		GxTmp = 0.0f;
		GyTmp = 0.0f;
		GzTmp = 0.0f;
	}

}

void LowPassFilterYaw(float *z, float alpha)
{
	static float lastz = 0;
	if (lastz == 0)
	{
		lastz = *z;
	}
	else
	{
		*z = alpha*lastz + (1 - alpha)*(*z);
		lastz = *z;
	}

}



bool IsHmdIntoStandBy(float *Pitch, float *Roll, float dTime)
{
	static float LastPitch = 0.0f, LastRoll = 0.0f;
	static float LastGyroX = 0.0f, LastGyroY = 0.0f;
	static int count = 0;
	float GyroX = 0.0f, GyroY = 0.0f, alpha = 0.98f;


	/* 初始化静态变量 */
	if (LastPitch == 0.0f&&LastRoll == 0.0f){
		LastPitch = *Pitch;
		LastRoll = *Roll;
	}
	/*对Pitch和Roll微分，求Gx和Gy */
	GyroX = fabsf(*Roll - LastRoll) / dTime;
	GyroY = fabsf(*Pitch - LastPitch) / dTime;
	/*初始化静态变量*/
	if (LastGyroX == 0.0f&&LastGyroY == 0.0f){
		LastGyroX = GyroX;
		LastGyroY = GyroY;
	}
	/*进行一个滑动窗口滤波降低偶然误差*/
	GyroX = alpha*LastGyroX + (1 - alpha)*GyroX;
	GyroY = alpha*LastGyroY + (1 - alpha)*GyroY;

	if (GyroX < 0.0015f&&GyroY < 0.0015f){
		count++;
		if (count == 3000){
			count = 0;
			HmdStandByState = true;
			//printf("头显进入休眠\n");
		}
	}
	else{
		count = 0;
		HmdStandByState = false;
	}
	/* 更新静态变量 */
	LastRoll = *Roll;
	LastPitch = *Pitch;
	LastGyroX = GyroX;
	LastGyroY = GyroY;

	return HmdStandByState;
}

float DM_GetYaw(void)
{
	return Yaw;
}

float DM_GetPitch(void)
{
	return Pitch;
}
float DM_GetRoll(void)
{
	return Roll;
}
Quaternion CQ_DM = { 1, 0, 0, 0 };
bool DM_Corrected = false;
Quaternion CorrectQuaterinonYaw(Quaternion q)
{
	Quaternion qcorr;
	qcorr.w = q.w; qcorr.x = 0.0f;
	qcorr.y = q.y; qcorr.z = 0.0f;
	qcorr = QuaternionInversion(qcorr);
	float mod = QuaternionMod(qcorr);
	qcorr.w = qcorr.w / mod;
	qcorr.x = qcorr.x / mod;
	qcorr.y = qcorr.y / mod;
	qcorr.z = qcorr.z / mod;
	return qcorr;
}
int DM_Correct(void)
{
	Quaternion q = { GlobalQ.w, 0, GlobalQ.y, 0 };
	CQ_DM = CorrectQuaterinonYaw(q);
	DM_Corrected = true;
	return DM_Corrected ? 1 : 0;
}

Quaternion DM_GetPoseQuat(void)
{
	static Quaternion QLast = { 1.0f, 0.0f, 0.0f, 0.0f };
	if (!GlobalQ_Updating)
	{
		QLast.w = GlobalQ.w;
		QLast.x = GlobalQ.x;
		QLast.y = GlobalQ.y;
		QLast.z = GlobalQ.z;
	}
	if (DM_Corrected)
	{
		return QuaternionMulti(QLast, CQ_DM);
	}
	else
		return QLast;
}