protected void NotifyHeadingChanged(double heading, double headingCorrection)
{
var args = new HeadingChangedEventArgs()
{
Heading = heading, HeadingCorrection = headingCorrection
};
HeadingChanged?.Invoke(this, args);
}
public void Interpret(byte [] data)
{
int currentIndex = 0;
X.raw = BitConverter.ToInt16(data, currentIndex);
currentIndex += sizeof(Int16);
Y.raw = BitConverter.ToInt16(data, currentIndex);
currentIndex += sizeof(Int16);
Z.raw = BitConverter.ToInt16(data, currentIndex);
currentIndex += sizeof(Int16);
X.max = Math.Max(X.max, X.raw);
Y.max = Math.Max(Y.max, Y.raw);
Z.max = Math.Max(Z.max, Z.raw);
X.min = Math.Min(X.min, X.raw);
Y.min = Math.Min(Y.min, Y.raw);
Z.min = Math.Min(Z.min, Z.raw);
//offset XY
Int16 centerX = (Int16)((X.max + X.min) / 2);
Int16 centerY = (Int16)((Y.max + Y.min) / 2);
Int16 centerZ = (Int16)((Z.max + Z.min) / 2);
if (Math.Abs(X.raw) > MaxValueX)
{
MaxValueX = Math.Abs(X.raw);
}
if (Math.Abs(Y.raw) > MaxValueY)
{
MaxValueY = Math.Abs(Y.raw);
}
if (Math.Abs(Z.raw) > MaxValueZ)
{
MaxValueZ = Math.Abs(Z.raw);
}
GraphXY.AbsoluteMaxValue = Math.Max(MaxValueX, MaxValueY);
GraphXZ.AbsoluteMaxValue = Math.Max(MaxValueX, MaxValueZ);
GraphYZ.AbsoluteMaxValue = Math.Max(MaxValueY, MaxValueZ);
if (IsScaled)
{
X.raw = (Int16)(X.raw - centerX);
Y.raw = (Int16)(Y.raw - centerY);
Console.WriteLine($"{centerX} {centerY}");
GraphXY.AddPoint(X.raw, Y.raw);
GraphXZ.AddPoint(X.raw, Z.raw);
GraphYZ.AddPoint(Y.raw, Z.raw);
}
else
{
GraphXY.AddPoint(X.raw, Y.raw);
GraphXZ.AddPoint(X.raw, Z.raw);
GraphYZ.AddPoint(Y.raw, Z.raw);
}
double heading = Math.Atan2(Y.raw, X.raw) * 180.0 / Math.PI;
HeadingChanged?.Invoke(heading);
}
private void Compass_ReadingChanged(Compass sender, CompassReadingChangedEventArgs args)
{
DispatcherHelper.CheckBeginInvokeOnUI(() => HeadingChanged?.Invoke(args));
}
