public double GetHeading()
{
ThreeAxisScaledData sd;
double Xm, Ym, Zm;
double Xh, Yh;
double cp, sp, cr, sr; // Cos and Sin of Pitch and Roll
double result;
sd = this.ScaledData; // this ensures we are only reading once from the bus
Xm = sd.ScaledX; // swapping X=Y due to installment
Ym = sd.ScaledY;
Zm = sd.ScaledZ;
cp = ExMath.Cos(pitch);
sp = ExMath.Sin(pitch);
cr = ExMath.Cos(roll);
sr = ExMath.Sin(roll);
#if false
// Below algorithm for tilt taken from LoveElectronics and proved to be wrong:
// Roll causes heading to swing and be incorrect
Xh = Xm * cp + Zm * sp;
Yh = Xm * sr * sp + Ym * cr - Zm * sr * cp;
result = ExMath.Atan(Yh / Xh);
#else
// Below algorthing for tilt compensation taken from
// http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Technical_Article-documents/Applications_of_Magnetic_Sensors_for_Low_Cost_Compass_Systems.pdf
// Goto http://www.magneticsensors.com choose Literature and on the "Technical Articles" select the above article
// results are great!
Xh = Xm * cp + Ym * sr * sp - Zm * cr * sp;
Yh = Ym * cr + Zm * sr;
result = ExMath.Atan(Yh / Xh);
if (Xh < 0)
{
result = ExMath.PI - result;
}
else if (Xh > 0)
{
if (Yh < 0)
{
result = -result;
}
else if (Yh > 0)
{
result = 2 * ExMath.PI - result;
}
else
{
;
}
}
else // Xh == 0
if (Yh < 0)
{
result = ExMath.PI / 2; // 90 degrees
}
else
{
result = ExMath.PI + ExMath.PI / 2; // 270 degrees
}
#endif
// Add the declination value
result += declination;
return(result);
}
