void QuatUpdate(void)
{
	//FUNCTION: QuatUpdate
	//--------------------------------------------------------------
	// Convert gyro rates to radians, add in the previous cycle error corrections
	//--------------------------------------------------------------
	float rx = Float(gx) * const_GyroScale + errCorrX;
	float ry = Float(gy) * const_GyroScale + errCorrY;
	float rz = Float(gz) * const_GyroScale + errCorrZ;


	//--------------------------------------------------------------
	// Update the orientation quaternion
	//--------------------------------------------------------------
	float rmag = Sqrt(rx*rx + ry*ry + rz*rz + const_Epsilon) * 0.5f;


	float sinr, cosr;
	cosr = SinCos(rmag, sinr);
	sinr /= rmag;


	float qdw = -(rx*qx + ry*qy + rz*qz) * 0.5f;		// Could neg first, then shift, but will take an instruction
	float qdx = (rx*qw + rz*qy + ry*qz) * 0.5f;
	float qdy = (ry*qw - rz*qx + rx*qz) * 0.5f;
	float qdz = (rz*qw + ry*qx - rx*qy) * 0.5f;


	qw = cosr * qw + sinr * qdw;
	qx = cosr * qx + sinr * qdx;
	qy = cosr * qy + sinr * qdy;
	qz = cosr * qz + sinr * qdz;


	rmag = Sqrt(qx*qx + qy*qy + qz*qz + qw*qw + const_Epsilon);
	qw /= rmag;
	qx /= rmag;
	qy /= rmag;
	qz /= rmag;


	//--------------------------------------------------------------
	// Convert the updated quaternion to a rotation matrix
	//--------------------------------------------------------------
	float fx2 = qx*qx;
	float fy2 = qy*qy;
	float fz2 = qz*qz;


	float fwx = qw*qx;
	float fwy = qw*qy;
	float fwz = qw*qz;


	float fxy = qx*qy;
	float fxz = qx*qz;
	float fyz = qy*qz;


	m00 = 1.0f - 2.0f * (fy2 + fz2);
	m01 =        2.0f * (fxy - fwz);
	m02 =        2.0f * (fxz + fwy);


	m10 =        2.0f * (fxy + fwz);
	m11 = 1.0f - 2.0f * (fx2 + fz2);
	m12 =        2.0f * (fyz - fwx);


	m20 =        2.0f * (fxz - fwy);
	m21 =        2.0f * (fyz + fwx);
	m22 = 1.0f - 2.0f * (fx2 + fy2);




	//--------------------------------------------------------------
	// Grab the accelerometer values as floats
	//--------------------------------------------------------------
	float fax = -Float(ax);
	float fay = Float(az);
	float faz = Float(ay);


	//--------------------------------------------------------------
	// Rotate accelerometer vector by the level correction angles
	//--------------------------------------------------------------
	float axRot = (fax * accRollCorrCos) - (fay * accRollCorrSin);
	float ayRot = (fax * accRollCorrSin) + (fay * accRollCorrCos);
	fax = axRot;
	fay = ayRot;


	axRot = (faz * accPitchCorrCos) - (fay * accPitchCorrSin);
	ayRot = (fax * accPitchCorrSin) + (fay * accPitchCorrCos);
	faz = axRot;
	fay = ayRot;


	//--------------------------------------------------------------
	// Compute length of the accelerometer vector and normalize it.
	// Use the computed length to decide weighting, IE how likely is
	// it a good reading to use to correct our rotation estimate.
	// If it's too long/short, weight it less.
	//--------------------------------------------------------------


	//ENDFUNCTION
}