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); }
public double GetHeadingRaw() { ThreeAxisScaledData sd = this.ScaledData; // this ensures we are reading data only once from the bus return(ExMath.Atan(sd.ScaledY / sd.ScaledX)); }