/*----------------------------------------------------------------------------------------------------
#	THIS FILE IS PART OF XEROLYSKINNER'S PROJECT
#	Quaternion - The core part of the project
#
#	THIS PROGRAM IS FREE SOFTWARE
#
#	Copyright (c) 2021 XerolySkinner
#	E-mail:ZABBCCCBBAZ@163.COM
#	QQ:2715099320
#
#	All rights reserved.
*/

/*----------------------------------------------------------------------------------------------------
	名字:Quaternion
	时间:20211229-2153
	程序员:梁源康
	说明:四元素换算
*/

//////////////////////////////////////////////////////////////////////////////////////////////////////
//----------------------------------------------------------------------------------------------------
//	头文件
#include "Quaternion.h"
#include "math.h"

#define		Kp 100.0f//比例增益支配率收敛到加速度计/磁强计
//#define	Ki 0.002f//积分增益支配率的陀螺仪偏见的衔接
#define		Ki 0.020f//积分增益支配率的陀螺仪偏见的衔接
#define		halfT 0.005f//采样周期的一半

float 		q0=0.1,q1=0.1,q2=0.1,q3=0.1;			//四元数的元素,代表估计方向
float 		exInt=0.1,eyInt=0.1,ezInt=0.1;			//按比例缩小积分误差
float 		Yaw=0,Pitch=0,Roll=0;					//偏航角,俯仰角,翻滚角
float		KalmanPitch;
//////////////////////////////////////////////////////////////////////////////////////////////////////
//----------------------------------------------------------------------------------------------------
//	程序
//	
float IMUupdate(float gx,float gy,float gz,float ax,float ay,float az){
	float norm;
	float vx,vy,vz;
	float ex,ey,ez;
	//测量正常化
	norm=sqrt(ax*ax+ay*ay+az*az);
	ax=ax/norm;//单位化
	ay=ay/norm;
	az=az/norm;
	//估计方向的重力
	vx=2*(q1*q3-q0*q2);
	vy=2*(q0*q1+q2*q3);
	vz=q0*q0-q1*q1-q2*q2+q3*q3;
	//错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
	ex=(ay*vz-az*vy);
	ey=(az*vx-ax*vz);
	ez=(ax*vy-ay*vx);
	//积分误差比例积分增益
	exInt=exInt+ex*Ki;
	eyInt=eyInt+ey*Ki;
	ezInt=ezInt+ez*Ki;
	//调整后的陀螺仪测量
	gx=gx+Kp*ex+exInt;
	gy=gy+Kp*ey+eyInt;
	gz=gz+Kp*ez+ezInt;
	//整合四元数率和正常化
	q0=q0+(-q1*gx-q2*gy-q3*gz)*halfT;
	q1=q1+(q0*gx+q2*gz-q3*gy)*halfT;
	q2=q2+(q0*gy-q1*gz+q3*gx)*halfT;
	q3=q3+(q0*gz+q1*gy-q2*gx)*halfT;
	//正常化四元
	norm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
	q0=q0/norm;
	q1=q1/norm;
	q2=q2/norm;
	q3=q3/norm;
	Pitch=asin(-2*q1*q3+2*q0*q2)*57.3;//pitch,转换为度数
	//Roll=atan2(2*q2*q3+2*q0*q1,-2*q1*q1-2*q2*q2+1)*57.3;//rollv
	//Yaw=atan2(2*(q1*q2+q0*q3),q0*q0+q1*q1-q2*q2-q3*q3)*57.3;//此处没有价值,注掉
	return Pitch;}