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]);
}
}
}
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) {
}
}