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(); } }
private void OnReceive(IAsyncResult ar) { UdpClient udp = (UdpClient)ar.AsyncState; try { IPEndPoint endpoint = new IPEndPoint(IPAddress.Any, 0); byte[] data = udp.EndReceive(ar, ref endpoint); // check if we have any pending operations PendingOp op = null; lock (pendingOps) { pendingOps.TryGetValue(endpoint.Address, out op); pendingOps[endpoint.Address] = null; } if (op != null && !op.cancelled) { // figure out what this message is if (op.messageCode == MicroMessageCodes.GetTimestamp && data.Length == 8) { // we have a value message, parse BigEndianBinaryReader reader = new BigEndianBinaryReader(data); reader.ReadUInt16(); ushort secs = reader.ReadUInt16(); int ticks = reader.ReadInt32(); RaiseTimestampEvent(endpoint.Address, new MicroTimestamp(secs, ticks), true); } else if (op.messageCode == MicroMessageCodes.TimingMode && data.Length == 3) { bool server = data[2] == 1; RaiseBoolEvent(endpoint.Address, server, true, ModeReceived); } else if (op.messageCode == MicroMessageCodes.TimingPulse && data.Length == 3) { bool result = data[2] == 1; RaiseBoolEvent(endpoint.Address, result, true, PulseReceived); } else if (op.messageCode == MicroMessageCodes.TimingResync && data.Length == 3) { bool result = data[2] == 0; RaiseBoolEvent(endpoint.Address, result, true, ResyncAcknowledged); } else if (op.messageCode == MicroMessageCodes.TimingSync && data.Length == 3) { bool result = data[2] == 1; RaiseBoolEvent(endpoint.Address, result, true, SyncReceived); } } udp.BeginReceive(OnReceive, udp); } catch (ObjectDisposedException) { } }