MahonyAHRS陀螺仪、加速度计、地磁计的融合代码(2)

// Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; }

//--------------------------------------------------------------------------------------------------- // Fast inverse square-root

// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root

floatinvSqrt(float x) { floathalfx = 0.5f * x; float y = x; longi = *(long*)&y; i = 0x5f3759df - (i>>1); y = *(float*)&i; y = y * (1.5f - (halfx * y * y)); return y; }

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

//====================================================================================================