//=============================================================================================
// MahonyAHRS.c
//=============================================================================================
//
// Madgwick's implementation of Mayhony's AHRS algorithm.
// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
//
// From the x-io website "Open-source resources available on this website are
// provided under the GNU General Public Licence unless an alternative licence
// is provided in source."
//
// Date                        Author                        Notes
// 29/09/2011        SOH Madgwick    Initial release
// 02/10/2011        SOH Madgwick        Optimised for reduced CPU load
//
// Algorithm paper:
// http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=4608934&url=http%3A%2F%2Fieeexplore.ieee.org%2Fstamp%2Fstamp.jsp%3Ftp%3D%26arnumber%3D4608934
//
//=============================================================================================

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

#include "MahonyAHRS.h"
#include "bll_imu.h"
#include <math.h>
#include "system.h"

//-------------------------------------------------------------------------------------------
// Definitions

float twoKi;                // 2 * integral gain (Ki)
float q0, q1, q2, q3;        // quaternion of sensor frame relative to auxiliary frame
float integralFBx, integralFBy, integralFBz;  // integral error terms scaled by Ki
float invSampleFreq;
float roll, pitch, yaw;
char anglesComputed;

//#define twoKpDef        (5.0f * 0.5f)        // 2 * proportional gain
//#define twoKiDef        (0.5f * 1.0f)        // 2 * integral gain

#define twoKpDef        (10.0f * 0.5f)        // 2 * proportional gain
#define twoKiDef        (0.0f * 1.0f)        // 2 * integral gain
void Mahony_Init(float sampleFrequency)
{
		twoKi = twoKiDef;        // 2 * integral gain (Ki)
		q0 = 1.0f;
		q1 = 0.0f;
		q2 = 0.0f;
		q3 = 0.0f;
		integralFBx = 0.0f;
		integralFBy = 0.0f;
		integralFBz = 0.0f;
		anglesComputed = 0;
		invSampleFreq = 1.0f / sampleFrequency;
}
float Mahony_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));
		y = y * (1.5f - (halfx * y * y));
		return y;
}

void Mahony_update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
		float recipNorm;
		float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
		float hx, hy, bx, bz;
		float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
		float halfex, halfey, halfez;
		float qa, qb, qc;

		// Compute feedback only if accelerometer measurement valid
		// (avoids NaN in accelerometer normalisation)
		if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

						// Normalise accelerometer measurement
						recipNorm = Mahony_invSqrt(ax * ax + ay * ay + az * az);
						ax *= recipNorm;
						ay *= recipNorm;
						az *= recipNorm;

						// Normalise magnetometer measurement
						recipNorm = Mahony_invSqrt(mx * mx + my * my + mz * mz);
						mx *= recipNorm;
						my *= recipNorm;
						mz *= recipNorm;

						// Auxiliary variables to avoid repeated arithmetic
						q0q0 = q0 * q0;
						q0q1 = q0 * q1;
						q0q2 = q0 * q2;
						q0q3 = q0 * q3;
						q1q1 = q1 * q1;
						q1q2 = q1 * q2;
						q1q3 = q1 * q3;
						q2q2 = q2 * q2;
						q2q3 = q2 * q3;
						q3q3 = q3 * q3;

						// Reference direction of Earth's magnetic field
						hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
						hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
						bx = sqrtf(hx * hx + hy * hy);
						bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));

						// Estimated direction of gravity and magnetic field
						halfvx = q1q3 - q0q2;
						halfvy = q0q1 + q2q3;
						halfvz = q0q0 - 0.5f + q3q3;
						halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
						halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
						halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);

						// Error is sum of cross product between estimated direction
						// and measured direction of field vectors
						halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
						halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
						halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);

						// Compute and apply integral feedback if enabled
						if(twoKi > 0.0f) {
										// integral error scaled by Ki
										integralFBx += twoKi * halfex * invSampleFreq;
										integralFBy += twoKi * halfey * invSampleFreq;
										integralFBz += twoKi * halfez * invSampleFreq;
										gx += integralFBx;        // apply integral feedback
										gy += integralFBy;
										gz += integralFBz;
						} else {
										integralFBx = 0.0f;        // prevent integral windup
										integralFBy = 0.0f;
										integralFBz = 0.0f;
						}

						// Apply proportional feedback
						gx += twoKpDef * halfex;
						gy += twoKpDef * halfey;
						gz += twoKpDef * halfez;
		}

		// Integrate rate of change of quaternion
		gx *= (0.5f * invSampleFreq);                // pre-multiply common factors
		gy *= (0.5f * invSampleFreq);
		gz *= (0.5f * invSampleFreq);
		qa = q0;
		qb = q1;
		qc = q2;
		q0 += (-qb * gx - qc * gy - q3 * gz);
		q1 += (qa * gx + qc * gz - q3 * gy);
		q2 += (qa * gy - qb * gz + q3 * gx);
		q3 += (qa * gz + qb * gy - qc * gx);

		// Normalise quaternion
		recipNorm = Mahony_invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
		q0 *= recipNorm;
		q1 *= recipNorm;
		q2 *= recipNorm;
		q3 *= recipNorm;
		anglesComputed = 0;
}
void Mahony_computeAngles()
{
        roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
        pitch = asinf(-2.0f * (q1*q3 - q0*q2));
        yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
        anglesComputed = 1;
}
float getRoll() {
        if (!anglesComputed) Mahony_computeAngles();
        return roll * 57.29578f;
}
float getPitch() {
        if (!anglesComputed) Mahony_computeAngles();
        return pitch * 57.29578f;
}
float getYaw() {
        if (!anglesComputed) Mahony_computeAngles();
        return yaw * 57.29578f;
}

static float acc_x,acc_y,acc_z;
static float groy_x,groy_y,groy_z;
static float mag_x,mag_y,mag_z;
extern unsigned int send_bytes_client(unsigned char *bytes, uint16_t len);
void Mahony_send_ANO(uint8_t fun,uint8_t* p,int len)
{
        uint8_t buf[256];
        int L=0;
        uint8_t ver = 0;
       
        buf[L] = 0xAA; ver += buf[L++];
        buf[L] = 0x05; ver += buf[L++];
        buf[L] = 0xAF; ver += buf[L++];
        buf[L] = fun;  ver += buf[L++];
        buf[L] = len;  ver += buf[L++];
        for(int i=0;i<len;i++){
                buf[L] = p[i]; ver += buf[L++];
        }
        buf[L++] = ver;
		send_bytes_client(buf,L);
	
//	unsigned char buf[255];
//	unsigned char ver=0; 
//	unsigned char i,L=0,Len=len+5;  
//	
//	buf[L]=0xAA;	ver+=buf[L++];		// 头
//	buf[L]=Len;		ver+=buf[L++]; 		//1位长度
//	buf[L]=~Len;	ver+=buf[L++];	 	// 1位长度反码
//	buf[L]=fun; 	ver+=buf[L++];    	//1命令
//	for(i=0;i<len; i++){ buf[L]=p[i];ver+=buf[L++];} //数据
//	buf[L++]=ver;      //效验
//	#if DEBUG_BLE_Client
//	SEGGER_RTT_printf(0,"Tx_BLE_Client:"); for(int i=0;i<L;i++){SEGGER_RTT_printf(0,"%02X ",buf[i]);} SEGGER_RTT_printf(0,"\r\n");
//	#endif
//	send_bytes_client(buf,L);   //压入发送缓存
}

void Mahony_send_ANO_STATUS(void)
{
        uint8_t buf[256];
        uint8_t L=0;
       
        int16_t rol  = (int16_t)(getRoll()*100);
        int16_t pitch = (int16_t)(getPitch()*100);
        int16_t yaw   = (int16_t)(getYaw()*100);
//        int32_t dis = IMU_GetDistand();

		//SEGGER_RTT_printf(0,"rol=%d,pitch=%d,yaw=%d\n",rol,pitch,yaw);
        buf[L++] = (uint8_t)(rol>>8);
        buf[L++] = (uint8_t)(rol>>0);
        buf[L++] = (uint8_t)(pitch>>8);
        buf[L++] = (uint8_t)(pitch>>0);
        buf[L++] = (uint8_t)(yaw>>8);
        buf[L++] = (uint8_t)(yaw>>0);
//        buf[L++] = (uint8_t)(dis>>24);
//        buf[L++] = (uint8_t)(dis>>16);
//        buf[L++] = (uint8_t)(dis>>8);
//        buf[L++] = (uint8_t)(dis>>0);
//		if(dis<500&&pitch>-4000) buf[L++] = 1;
//		else buf[L++] = 0;
//        buf[L++] = 0;
//        buf[L++] = 0;
        buf[L++] = 0;
        buf[L++] = 0;
				buf[L++] = 0;
				buf[L++] = 0;
        buf[L++] = 0; 
				buf[L++] = 0;
        buf[L++] = 0;
        Mahony_send_ANO(0x01,buf,L);
}

void Mahony_send_ANO_SENSER(int16_t gx, int16_t gy, int16_t gz, int16_t ax, int16_t ay, int16_t az, int16_t mx, int16_t my, int16_t mz)
{
        uint8_t buf[256];
        uint8_t L=0;
       
        buf[L++] = (uint8_t)(ax>>8);
        buf[L++] = (uint8_t)(ax>>0);
        buf[L++] = (uint8_t)(ay>>8);
        buf[L++] = (uint8_t)(ay>>0);
        buf[L++] = (uint8_t)(az>>8);
        buf[L++] = (uint8_t)(az>>0);
       
        buf[L++] = (uint8_t)(gx>>8);
        buf[L++] = (uint8_t)(gx>>0);
        buf[L++] = (uint8_t)(gy>>8);
        buf[L++] = (uint8_t)(gy>>0);
        buf[L++] = (uint8_t)(gz>>8);
        buf[L++] = (uint8_t)(gz>>0);
       
        buf[L++] = (uint8_t)(mx>>8);
        buf[L++] = (uint8_t)(mx>>0);
        buf[L++] = (uint8_t)(my>>8);
        buf[L++] = (uint8_t)(my>>0);
        buf[L++] = (uint8_t)(mz>>8);
        buf[L++] = (uint8_t)(mz>>0);

        Mahony_send_ANO(0x02,buf,L);
}

void Mahony_send_My_SENSER(int16_t gx, int16_t gy, int16_t gz, int16_t ax, int16_t ay, int16_t az, int16_t mx, int16_t my, int16_t mz)
{
	uint8_t buf[256];
	uint8_t L=0;
	
	int16_t _q0 = (int16_t)(q0*100);
	int16_t _q1 = (int16_t)(q1*100);
	int16_t _q2 = (int16_t)(q2*100);
	int16_t _q3 = (int16_t)(q3*100);

	int16_t rol  	= (int16_t)(getRoll()*100);
	int16_t pit 	= (int16_t)(getPitch()*100);
	int16_t yaw   	= (int16_t)(getYaw()*100);

	int16_t ds = 3210;
	int16_t pf 	= 12;
	int16_t pb 	= 34;
	int16_t ptx 	= 56;
	int16_t pty 	= 78;
	int16_t ptz 	= 90;
	
   
	buf[L++] = (uint8_t)(ax>>8);
	buf[L++] = (uint8_t)(ax>>0);
	buf[L++] = (uint8_t)(ay>>8);
	buf[L++] = (uint8_t)(ay>>0);
	buf[L++] = (uint8_t)(az>>8);
	buf[L++] = (uint8_t)(az>>0);
   
	buf[L++] = (uint8_t)(gx>>8);
	buf[L++] = (uint8_t)(gx>>0);
	buf[L++] = (uint8_t)(gy>>8);
	buf[L++] = (uint8_t)(gy>>0);
	buf[L++] = (uint8_t)(gz>>8);
	buf[L++] = (uint8_t)(gz>>0);
   
	buf[L++] = (uint8_t)(mx>>8);
	buf[L++] = (uint8_t)(mx>>0);
	buf[L++] = (uint8_t)(my>>8);
	buf[L++] = (uint8_t)(my>>0);
	buf[L++] = (uint8_t)(mz>>8);
	buf[L++] = (uint8_t)(mz>>0);
	
	buf[L++] = (uint8_t)(pf>>8);
	buf[L++] = (uint8_t)(pf>>0);
	
	buf[L++] = (uint8_t)(pb>>8);
	buf[L++] = (uint8_t)(pb>>0);
	
	buf[L++] = (uint8_t)(ds>>8);
	buf[L++] = (uint8_t)(ds>>0);
	
	buf[L++] = (uint8_t)(rol>>8);
	buf[L++] = (uint8_t)(rol>>0);
	
	buf[L++] = (uint8_t)(pit>>8);
	buf[L++] = (uint8_t)(pit>>0);
	
	buf[L++] = (uint8_t)(yaw>>8);
	buf[L++] = (uint8_t)(yaw>>0);
	
	buf[L++] = (uint8_t)(ptx>>8);
	buf[L++] = (uint8_t)(ptx>>0);
	
	buf[L++] = (uint8_t)(pty>>8);
	buf[L++] = (uint8_t)(pty>>0);
	
	buf[L++] = (uint8_t)(ptz>>8);
	buf[L++] = (uint8_t)(ptz>>0);
	
	buf[L++] = (uint8_t)(_q0>>8);
	buf[L++] = (uint8_t)(_q0>>0);
	
	buf[L++] = (uint8_t)(_q1>>8);
	buf[L++] = (uint8_t)(_q1>>0);
	
	buf[L++] = (uint8_t)(_q2>>8);
	buf[L++] = (uint8_t)(_q2>>0);
	
	buf[L++] = (uint8_t)(_q3>>8);
	buf[L++] = (uint8_t)(_q3>>0);

	Mahony_send_ANO(0xEE,buf,L);
}

//void Mahony_process(int16_t gx, int16_t gy, int16_t gz, int16_t ax, int16_t ay, int16_t az, int16_t mx, int16_t my, int16_t mz)
//{
//	 static uint8_t tt = 0;
//	 acc_x = ax/4096.0f;
//	 acc_y = ay/4096.0f;
//	 acc_z = az/4096.0f;
//	
//   int16_t rol  = (int16_t)(getRoll()*100);
//	
//	 Mahony_update(groy_x,groy_y,groy_z,acc_x,acc_y,acc_z,mag_x,mag_y,mag_z);
////	 DEBUG_LOG("rol :%d\r\n", rol);    
//	 if(++tt>=10){tt = 0;
//		Mahony_send_ANO_STATUS();
//		Mahony_send_ANO_SENSER(gx,gy,gz,ax,ay,az,mx,my,mz);
////			Mahony_send_My_SENSER(gx,gy,gz,ax,ay,az,mx,my,mz);
//	 }
//}

void Mahony_process(int16_t gx, int16_t gy, int16_t gz, int16_t ax, int16_t ay, int16_t az, int16_t mx, int16_t my, int16_t mz)
{
	 acc_x = ax/4096.0f;
	 acc_y = ay/4096.0f;
	 acc_z = az/4096.0f;
	 Mahony_update(groy_x,groy_y,groy_z,acc_x,acc_y,acc_z,mag_x,mag_y,mag_z);
}


//============================================================================================
// END OF CODE
//============================================================================================