private void serialPort_dataReceived(object sender, SerialDataReceivedEventArgs e) {
//Console.WriteLine("Compass received data!");
if (myCompass.BytesToRead >= 4)
{
byte[] buf = new byte[4];
myCompass.Read(buf, 0, 4);
// make this more good later
// if (buf[0] != (byte)motor.MotorCommands.COMMAND_START || buf[3] != (byte)motor.MotorCommands.COMMAND_STOP)
if (buf[0] != (byte)254 || buf[3] != (byte)233)
{
// something went wrong
Console.WriteLine("Error in compass serial port handler, received: " + buf.ToString());
return;
}
int angle = ((int)buf[1]) + buf[2];
if (!(0 <= angle && angle <= 360))
{
Console.WriteLine("Error in compass serial port handler, impossible angle received");
return;
}
CompassData dat = new CompassData();
dat.angle = angle;
CompassNotification not = new CompassNotification(dat);
_mainPort.Post(not);
}
}
public virtual IEnumerator<ITask> CompassNotificationHandler(CompassNotification c)
{
_state.currentAngle = c.Body;
SendNotification(_subMgrPort, c);
yield break;
}
