void link_PacketReceived(object sender, PacketReceivedEventArgs e)
{
switch (e.ID)
{
case (byte)Glo.IDs.AssertMessage:
GloObject <GloAssertMessage> assertMessage = new GloObject <GloAssertMessage>(Glo.IDs.AssertMessage);
assertMessage.data = GloLink.ByteArrayToStructure <GloAssertMessage>(e.Data);
logAndDisplayMessage(assertMessage.data.text, MessageSource.RobotAssert);
break;
case (byte)Glo.IDs.DebugMessage:
GloObject <GloDebugMessage> debugMessage = new GloObject <GloDebugMessage>(Glo.IDs.DebugMessage);
debugMessage.data = GloLink.ByteArrayToStructure <GloDebugMessage>(e.Data);
logAndDisplayMessage(debugMessage.data.text, MessageSource.RobotDebug);
break;
case (byte)Glo.IDs.CaptureData:
if (capturingData)
{
GloObject <GloCaptureData> testData = new GloObject <GloCaptureData>(Glo.IDs.CaptureData);
testData.data = GloLink.ByteArrayToStructure <GloCaptureData>(e.Data);
captureData.Add(new float[] { (float)testData.data.time, (float)(testData.data.tiltAngle * 180.0 / Math.PI) });
}
break;
case (byte)Glo.IDs.StatusData:
GloObject <GloStatusData> statusData = new GloObject <GloStatusData>(Glo.IDs.StatusData);
statusData.data = GloLink.ByteArrayToStructure <GloStatusData>(e.Data);
view.TiltAngle = statusData.data.tiltAngle * 180.0 / Math.PI;
break;
default:
logAndDisplayMessage("Received unhandled packet with ID: " + e.ID, MessageSource.UI);
break;
}
}
public void parseData(byte[] dataBytes)
{
// set to true if we received a new complete packet
bool packetPending = false;
NumberBytesReceived += dataBytes.Length;
foreach (byte inByte in dataBytes)
{
switch (parseState)
{
case -1: // looking for first frame byte
if (inByte == messageFrame[0])
{
advanceParse();
}
else
{
resetParse();
}
break;
case 0: // looking for 2nd frame byte
if (inByte == messageFrame[1])
{
advanceParse();
}
else
{
resetParse();
}
break;
case 1: // looking for 3rd frame byte
if (inByte == messageFrame[2])
{
advanceParse();
}
else
{
resetParse(); throw new Exception("Invalid frame: " + inByte.ToString());
}
break;
case 2: // pulling out id
objectID = inByte;
advanceParse();
break;
case 3: // pulling out byte one of instance
instance = inByte;
advanceParse();
break;
case 4: // pulling out byte two of instance
instance += (ushort)(((ushort)inByte) << 8);
advanceParse();
break;
case 5: // pulling out length of data
numberBodyBytes = inByte;
advanceParse();
break;
case 6: // pulling out body
gloObjectData[bodyIndex] = inByte;
bodyIndex++;
// if this is last byte then advance parsing state
if (bodyIndex >= numberBodyBytes)
{
advanceParse();
}
break;
case 7: // looking for CR (0x0D) -- should add CRC check for serial
if (inByte == 0x0D)
{
advanceParse();
}
else
{
resetParse();
}
break;
case 8: // looking for LF (0x0A)
if (inByte == 0x0A)
{
packetPending = true;
}
resetParse();
break;
default: // safety reset
resetParse();
break;
}
if (packetPending)
{
packetPending = false;
NumberPacketsReceived++;
if (PacketReceived != null)
{
PacketReceivedEventArgs args = new PacketReceivedEventArgs();
args.ID = objectID;
args.Instance = instance;
args.Data = gloObjectData;
PacketReceived(this, args);
}
}
}
}