public async void Read(Object state)
{
try
{
/* Ask the Compass to Perform a Reading */
var outBuffer = new byte[2];
outBuffer[0] = HMC5983_MODE;
outBuffer[1] = 0x01;
_compassSensor.Write(outBuffer);
}
catch (FileNotFoundException)
{
if (IsOnline)
{
_timer.Change(2500, 2500);
}
RawX.IsOnline = false;
RawY.IsOnline = false;
IsOnline = false;
return;
}
if (IsOnline)
{
_timer.Change(500, 500);
}
try
{
/* Wait for Conversion complete, 10ms should be plenty */
await Task.Delay(20);
/* Move the read buffer to register #3 which is where the compass data starts */
/* Read Compass Values */
var inBuffer = new byte[6];
_compassSensor.WriteRead(new byte[1] {
HMC5983_OUT_X_MSB
}, inBuffer);
var hX = (Int16)(inBuffer[0] << 8 | inBuffer[1]);
var hZ = (Int16)(inBuffer[2] << 8 | inBuffer[3]);
var hY = (Int16)(inBuffer[4] << 8 | inBuffer[5]);
if (_isCalibrating)
{
if (hX < _minX)
{
_minX = hX;
}
if (hX > _maxX)
{
_maxX = hX;
}
if (hY < _minY)
{
_minY = hY;
}
if (hY > _minY)
{
_maxY = hY;
}
}
_medianFilter.Add(new Point2D <int>(hX, hY));
var radians = Math.Atan2(_medianFilter.Filtered.X, _medianFilter.Filtered.Y);
var angle = radians * (180 / Math.PI);
Value = angle.ToString();
if (hY > 0)
{
Value = (90 - angle).ToString();
}
else if (hY < 0)
{
Value = (270 - angle).ToString();
}
IsOnline = true;
RawX.Value = hX.ToString();
RawX.IsOnline = true;
RawY.Value = hY.ToString();
RawY.IsOnline = true;
}
catch (Exception ex)
{
RawX.IsOnline = false;
RawY.IsOnline = false;
IsOnline = false;
}
}
public async void Read(Object state)
{
try
{
/* Ask the Compass to Perform a Reading */
var outBuffer = new byte[2];
outBuffer[0] = HMC5983_MODE;
outBuffer[1] = 0x01;
_compassSensor.Write(outBuffer);
/* Wait for Conversion complete, 10ms should be plenty */
await Task.Delay(20);
/* Move the read buffer to register #3 which is where the compass data starts */
/* Read Compass Values */
var inBuffer = new byte[6];
_compassSensor.WriteRead(new byte[1] {
HMC5983_OUT_X_MSB
}, inBuffer);
// var hX = BitConverter.ToInt16(inBuffer, 0);
// var hZ = BitConverter.ToInt16(inBuffer, 2);
// var hY = BitConverter.ToInt16(inBuffer, 4);
var hX = (Int16)(inBuffer[0] << 8 | inBuffer[1]);
var hZ = (Int16)(inBuffer[2] << 8 | inBuffer[3]);
var hY = (Int16)(inBuffer[4] << 8 | inBuffer[5]);
_medianFilter.Add(new Point2D <int>(hX, hY));
var radians = Math.Atan2(_medianFilter.Filtered.X, _medianFilter.Filtered.Y);
var angle = radians * (180 / Math.PI);
Value = angle.ToString();
if (hY > 0)
{
Value = (90 - angle).ToString();
}
else if (hY < 0)
{
Value = (270 - angle).ToString();
}
IsOnline = true;
RawX.Value = hX.ToString();
RawX.IsOnline = true;
RawY.Value = hY.ToString();
RawY.IsOnline = true;
Debug.WriteLine($"Angle={Value}");
}
catch (Exception ex)
{
Debug.WriteLine(ex.Message);
RawX.IsOnline = false;
RawY.IsOnline = false;
Debug.WriteLine("Compass Offline: " + Value.ToString());
IsOnline = false;
}
}