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; }