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();
}
}