public void HandleMessage(byte[] message, object target, DatasetSource ds) { BigEndianBinaryReader reader = new BigEndianBinaryReader(message); // read in the current time CarTimestamp time = new CarTimestamp(reader.ReadUInt16(), reader.ReadInt32()); // read the feedback type byte feedbackType = reader.ReadByte(); // read the length ushort len = reader.ReadUInt16(); // dispatch if we can FeedbackType type; if (types.TryGetValue((int)feedbackType, out type)) { object[] vals = type.MapMessage(target, ds, time, reader); if (logWriter != null) { logWriter.Write(time); logWriter.Write(","); logWriter.Write(feedbackType); logWriter.Write(","); for (int i = 0; i < vals.Length - 1; i++) { logWriter.Write(vals[i]); logWriter.Write(","); } logWriter.WriteLine(vals[vals.Length - 1]); } } }
private void ListenerProc() { while (true) { IPEndPoint ep = new IPEndPoint(IPAddress.Any, 0); byte[] data = client.Receive(ref ep); // get the current time DateTime now = HighResDateTime.Now; if (data.Length == 5) { // parse the data BigEndianBinaryReader reader = new BigEndianBinaryReader(data); // check that the first byte is a 0 if (reader.ReadByte() == 0) { // read the seconds ushort secs = reader.ReadUInt16(); // read the ticks ushort ticks = reader.ReadUInt16(); // get the car timestamp CarTimestamp ct = new CarTimestamp(secs, ticks); // get the elapsed seconds since start TimeSpan elapsed = now-start; double localElapsedSecs = elapsed.TotalSeconds; // update the diff double curDiff = ct.ts-localElapsedSecs; double newDiff; // check if we haven't initialized the offset or the offset deviates from the just-measured offset by more than a second if (double.IsNaN(offset) || Math.Abs(offset-curDiff) >= 1) { newDiff = curDiff; } else { // smooth out the offset values using an exponential filter newDiff = curDiff*alpha + offset*(1-alpha); } // swap in the new value // probably don't need an interlocked operation for this, but using it so that // we swap the entire double at once and don't get any weird problems Interlocked.Exchange(ref offset, newDiff); } } } }
protected int ReadData(BigEndianBinaryReader reader) { switch (intype) { case InType.U8: return reader.ReadByte(); case InType.S8: return reader.ReadSByte(); case InType.U16: return reader.ReadUInt16(); case InType.S16: return reader.ReadInt16(); case InType.U32: return (int)reader.ReadUInt32(); case InType.S32: return reader.ReadInt32(); default: throw new InvalidOperationException(); } }