ahrs

void ahrs(void)
{
	// quaternion base process 
	float Norm;
	float ax = AccelGyro[0];
	float ay = AccelGyro[1];
	float az = AccelGyro[2];
	float gx =((AccelGyro[3]-gx_diff)/ GYROSCOPE_SENSITIVITY )*M_PI/180 ;
	float gy =((AccelGyro[4]-gy_diff)/ GYROSCOPE_SENSITIVITY )*M_PI/180 ;
	float gz =((AccelGyro[5]-gz_diff)/ GYROSCOPE_SENSITIVITY )*M_PI/180 ;
	
	float q1_dot = 0.5 * (-q2 * gx - q3 * gy - q4 * gz);
	float q2_dot = 0.5 * ( q1 * gx + q3 * gz - q4 * gy);
	float q3_dot = 0.5 * ( q1 * gy - q2 * gz + q4 * gx);
	float q4_dot = 0.5 * ( q1 * gz + q2 * gy - q3 * gx);

	if(!((ax == 0) && (ay == 0) && (az == 0))) {
		// Normalise 
		Norm = sqrtf(ax * ax + ay * ay + az * az);
		ax /= Norm;
		ay /= Norm;
		az /= Norm;   

		float _2q1 = 2 * q1;
		float _2q2 = 2 * q2;
		float _2q3 = 2 * q3;
		float _2q4 = 2 * q4;
		float _4q1 = 4 * q1;
		float _4q2 = 4 * q2;
		float _4q3 = 4 * q3;
		float _8q2 = 8 * q2;
		float _8q3 = 8 * q3;
		float q1q1 = q1 * q1;
		float q2q2 = q2 * q2;
		float q3q3 = q3 * q3;
		float q4q4 = q4 * q4;
		// Gradient decent 
		float s1 = _4q1 * q3q3 + _2q4 * ax + _4q1 * q2q2 - _2q2 * ay;
		float s2 = _4q2 * q4q4 - _2q4 * ax + 4 * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az;
		float s3 = 4 * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az;
		float s4 = 4 * q2q2 * q4 - _2q2 * ax + 4 * q3q3 * q4 - _2q3 * ay;
		// Normalise 
		Norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
		s1 /= Norm;
		s2 /= Norm;
		s3 /= Norm;
		s4 /= Norm;
		// compensate acc
		q1_dot -= (beta * s1);
		q2_dot -= (beta * s2);
		q3_dot -= (beta * s3);
		q4_dot -= (beta * s4);
	}
	// Integrate 
	q1 += q1_dot / sampleFreq;
	q2 += q2_dot / sampleFreq;
	q3 += q3_dot / sampleFreq;
	q4 += q4_dot / sampleFreq;
	// Normalise
	Norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4 );
	q1 /= Norm;
	q2 /= Norm;
	q3 /= Norm;
	q4 /= Norm;
	// convert to euler
	y_pitch =  2*(q3*q4 + q1*q2);
    
	float x =  2*(0.5 - q2*q2 - q3*q3);

			q_pitch = atan2 (y_pitch,x)* -180.0f / M_PI;
            t_compensate  = T_center * ((y_pitch-y0_pitch)/sqrtf(((y_pitch*y_pitch))+(x*x))); // pitch angle compensate            test  =((y_pitch-y0_pitch)/sqrtf(((y_pitch*y_pitch))+(x*x))); // pitch angle compensate

			y_roll = -2*(q2*q4 - q1*q3)	;
            q_roll = asinf(y_roll)* -180.0f / M_PI;
            y_roll = y_roll - y0_roll ;
            if(y_roll < 0) y_roll = -y_roll ;
            t_compensate *= (1 + (y_roll));       // roll angle compensate            

            if(t_compensate < 0) t_compensate = -t_compensate ; // +value
 
            q_yaw   += gz/sampleFreq * 180.0f / M_PI ;
}