Пример #1
0
 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;
             }
         }
     }
 }
Пример #2
0
 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);
     }
 }
Пример #3
0
 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);
     }
 }