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]);
}
}
}
public object[] MapMessage(object target, DatasetSource ds, CarTimestamp ts, BigEndianBinaryReader reader)
{
// reader will be positioned at start of payload
int nextra = 0;
if (methodHasTimestamp)
nextra++;
if (methodHasMsgType)
nextra++;
object[] vals = new object[fields.Count];
object[] param = null;
if (method != null) {
if (methodIsObjectArray) {
param = new object[1+nextra];
}
else {
param = new object[fields.Count + nextra];
}
if (methodHasTimestamp) {
param[0] = ts;
if (methodHasMsgType)
param[1] = id;
}
else if (methodHasMsgType) {
param[0] = id;
}
}
for (int i = 0; i < fields.Count; i++) {
object val = fields[i].MapField(reader, ds, ts);
if (param != null && !methodIsObjectArray)
param[i + nextra] = val;
vals[i] = val;
}
if (methodIsObjectArray && param != null) {
param[nextra] = vals;
}
if (method != null) {
method.Invoke(target, param);
}
return vals;
}
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);
}
}
}
}
public object MapField(BigEndianBinaryReader reader, DatasetSource ds, CarTimestamp ct)
{
object val = MapData(ReadData(reader));
if (!string.IsNullOrEmpty(dsitem) && ds != null) {
IDataItemSource di;
if (!ds.TryGetValue(dsitem, out di)) {
Type digType = typeof(DataItemSource<>);
Type diType = digType.MakeGenericType(outtype);
DataItemDescriptor desc = new DataItemDescriptor(dsitem, outtype, null, null, 250);
di = (IDataItemSource)Activator.CreateInstance(diType, desc);
ds.Add(dsitem, di);
}
di.Add(val, ct);
}
return val;
}
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) {
}
}
