void mySerialPort_DataReceived(Object sender, SerialDataReceivedEventArgs e)
{
while (mySerialPort.BytesToRead > 0)
{
byte b = (byte)mySerialPort.ReadByte();
currentFrame.WriteByte(b);
if (!FrameIsValid && currentFrame.Length >= 2) //still looking for "PA"
{
currentFrame.Seek(-2, SeekOrigin.Current); //see if last two bytes indicate header origin
int FrameOrigin = (int)currentFrame.Position;
FrameIsValid = (currentFrame.ReadByte() == 0x50) & (currentFrame.ReadByte() == 0x41); //PA
if (FrameIsValid && FrameOrigin != 0) //Found data outside of valid frame
{
hd = new Head(); //Create "pseudo"-response with earlier data
hd.ResponseSize = (short)FrameOrigin; //Make new response in header
currentFrame.SetLength(FrameOrigin);
currentFrame.CopyTo(hd.response); // and put all of data before PA into it
frames.Add(hd); //and add to queue
currentFrame.SetLength(0); //Start new valid frame with "PA"
currentFrame.WriteByte(0x50);
currentFrame.WriteByte(0x41);
}
}
else if (!HeadIsDone && currentFrame.Length == 8) //then we should have a complete header
{
HeadIsDone = true;
currentFrame.Position = 0;
BinaryReader t = new BinaryReader(currentFrame);
hd = new Head();
hd.FrameTag = new string(t.ReadChars(2)); //PA
hd.StationNumber = t.ReadByte();
hd.InitiatingCommand = t.ReadByte();
hd.ErrorIndicator = t.ReadByte(); //0 for no error
t.ReadByte(); //unassigned
hd.ResponseSize = t.ReadInt16(); //response size
}
else if (HeadIsDone) //then we have header and are accumulating response data
{
hd.response.WriteByte(b); //Put current byte into response
if (hd.response.Length == hd.ResponseSize) //completed response field
{
frames.Add(hd); //Add header to queue
currentFrame.SetLength(0); // and reset for next frame
FrameIsValid = false;
HeadIsDone = false;
}
}
}
}
Head readHeader()
{
try
{
Head h = new Head();
h.FrameTag = new string(reader.ReadChars(2));
h.StationNumber = reader.ReadByte();
h.InitiatingCommand = reader.ReadByte();
h.ErrorIndicator = reader.ReadByte();
reader.ReadByte();
h.ResponseSize = reader.ReadInt16();
if (h.ErrorIndicator != (byte)0)
throw new Exception("Error code returned 0x" + h.ErrorIndicator.ToString("X2"));
reader.ReadBytes(h.ResponseSize);
return h;
}
catch (Exception e)
{
throw new Exception("Patriot.readHeader: " + e.Message);
}
}
public Point3 ExtractPoint3(Head h)
{
try
{
BinaryReader b = new BinaryReader(h.response);
Point3 p = new Point3();
p.X = b.ReadSingle();
p.Y = b.ReadSingle();
p.Z = b.ReadSingle();
h = tryReadHead();
if (h == null)
throw new Exception("No second sensor data recieved");
b = new BinaryReader(h.response);
p.X -= b.ReadSingle();
p.Y -= b.ReadSingle();
p.Z -= b.ReadSingle();
return p;
}
catch(Exception e)
{
throw new Exception("ExtractPoint3: " + e.Message);
}
}