void TrackedDistance_DataValueAdded(object sender, SourceDataValueAddedEventArgs<double> e)
{
// check if we received something before
if (double.IsNaN(lastTime.ts)) {
// we haven't received anything, initialize starting time
lastTime = e.Time;
}
else if (e.Time < lastTime) {
//OperationalTrace.WriteWarning("resetting tracked distanace: event time {0}, last time {1}", e.Time, lastTime);
// timestamp rollover/reset
// clear everything out
lock (lockobj) {
queue.Clear();
lastTime = e.Time;
lastDist = 0;
}
}
else {
// calculate dt
double dt = e.Time.ts - lastTime.ts;
// calculate delta distance
double dx = Math.Abs(e.Value)*dt;
// get the lock
lock (lockobj) {
lastDist += dx;
lastTime = e.Time;
//OperationalTrace.WriteVerbose("adding dist {0}, time {1}", lastDist, lastTime);
queue.Add(new TrackedDistanceItem(lastTime, lastDist));
}
}
}
internal void OnDataItemValueAdded(SourceDataValueAddedEventArgs e)
{
if (!doSendDataValues)
{
return;
}
MemoryStream ms = null;
try {
// construct the message
ms = GetSendBuffer(e.AbstractDataItem.TypeCode);
BinaryWriter writer = new BinaryWriter(ms, Encoding.UTF8);
// write the message type -- for now we only send data value messages
writer.Write(DataItemComm.DataValueMsgID);
// write the sequence number and increment
writer.Write(seqNo);
Interlocked.Increment(ref seqNo);
// write the data item name
writer.Write(e.AbstractDataItem.Name);
// write the time as a double (seconds in car time)
writer.Write(e.Time.ts);
// figure out the type
DataTypeCode dtc = e.AbstractDataItem.TypeCode;
// write out the type code
writer.Write((int)dtc);
// write the data depending on the type
switch (dtc)
{
case DataTypeCode.Double:
writer.Write(((SourceDataValueAddedEventArgs <double>)e).Value);
break;
case DataTypeCode.Single:
writer.Write(((SourceDataValueAddedEventArgs <float>)e).Value);
break;
case DataTypeCode.Int8:
writer.Write(((SourceDataValueAddedEventArgs <sbyte>)e).Value);
break;
case DataTypeCode.Int16:
writer.Write(((SourceDataValueAddedEventArgs <Int16>)e).Value);
break;
case DataTypeCode.Int32:
writer.Write(((SourceDataValueAddedEventArgs <Int32>)e).Value);
break;
case DataTypeCode.Int64:
writer.Write(((SourceDataValueAddedEventArgs <Int64>)e).Value);
break;
case DataTypeCode.UInt8:
writer.Write(((SourceDataValueAddedEventArgs <byte>)e).Value);
break;
case DataTypeCode.UInt16:
writer.Write(((SourceDataValueAddedEventArgs <UInt16>)e).Value);
break;
case DataTypeCode.UInt32:
writer.Write(((SourceDataValueAddedEventArgs <UInt32>)e).Value);
break;
case DataTypeCode.UInt64:
writer.Write(((SourceDataValueAddedEventArgs <UInt64>)e).Value);
break;
case DataTypeCode.Boolean:
writer.Write(((SourceDataValueAddedEventArgs <bool>)e).Value);
break;
case DataTypeCode.DateTime:
writer.Write(((SourceDataValueAddedEventArgs <DateTime>)e).Value.ToBinary());
break;
case DataTypeCode.TimeSpan:
writer.Write(((SourceDataValueAddedEventArgs <TimeSpan>)e).Value.Ticks);
break;
case DataTypeCode.Coordinates: {
Coordinates v = ((SourceDataValueAddedEventArgs <Coordinates>)e).Value;
WriteCoordinate(v, writer);
}
break;
case DataTypeCode.Circle: {
Circle c = ((SourceDataValueAddedEventArgs <Circle>)e).Value;
writer.Write(c.r);
WriteCoordinate(c.center, writer);
}
break;
case DataTypeCode.Line: {
Line l = ((SourceDataValueAddedEventArgs <Line>)e).Value;
WriteCoordinate(l.P0, writer);
WriteCoordinate(l.P1, writer);
}
break;
case DataTypeCode.LineSegment: {
LineSegment ls = ((SourceDataValueAddedEventArgs <LineSegment>)e).Value;
WriteCoordinate(ls.P0, writer);
WriteCoordinate(ls.P1, writer);
}
break;
case DataTypeCode.Polygon: {
Polygon pg = ((SourceDataValueAddedEventArgs <Polygon>)e).Value;
writer.Write((int)pg.CoordinateMode);
writer.Write(pg.Count);
foreach (Coordinates pg_pt in pg)
{
WriteCoordinate(pg_pt, writer);
}
}
break;
case DataTypeCode.LineList: {
LineList ll = ((SourceDataValueAddedEventArgs <LineList>)e).Value;
writer.Write(ll.Count);
foreach (Coordinates ll_pt in ll)
{
WriteCoordinate(ll_pt, writer);
}
}
break;
case DataTypeCode.Bezier: {
CubicBezier cb = ((SourceDataValueAddedEventArgs <CubicBezier>)e).Value;
WriteCoordinate(cb.P0, writer);
WriteCoordinate(cb.P1, writer);
WriteCoordinate(cb.P2, writer);
WriteCoordinate(cb.P3, writer);
}
break;
case DataTypeCode.CoordinatesArray: {
Coordinates[] pts = ((SourceDataValueAddedEventArgs <Coordinates[]>)e).Value;
writer.Write(pts.Length);
for (int i = 0; i < pts.Length; i++)
{
WriteCoordinate(pts[i], writer);
}
}
break;
case DataTypeCode.LineListArray: {
LineList[] lineLists = ((SourceDataValueAddedEventArgs <LineList[]>)e).Value;
writer.Write(lineLists.Length);
for (int i = 0; i < lineLists.Length; i++)
{
LineList list = lineLists[i];
writer.Write(list.Count);
for (int j = 0; j < list.Count; j++)
{
WriteCoordinate(list[i], writer);
}
}
}
break;
case DataTypeCode.PolygonArray: {
Polygon[] polys = ((SourceDataValueAddedEventArgs <Polygon[]>)e).Value;
writer.Write(polys.Length);
for (int i = 0; i < polys.Length; i++)
{
Polygon poly = polys[i];
writer.Write(poly.Count);
for (int j = 0; j < poly.Count; j++)
{
WriteReducedCoord(poly[j], writer);
}
}
}
break;
case DataTypeCode.ObstacleArray: {
OperationalObstacle[] obs = ((SourceDataValueAddedEventArgs <OperationalObstacle[]>)e).Value;
writer.Write(obs.Length);
for (int i = 0; i < obs.Length; i++)
{
writer.Write(obs[i].age);
writer.Write((int)obs[i].obstacleClass);
writer.Write(obs[i].ignored);
writer.Write(obs[i].headingValid);
writer.Write(obs[i].heading);
writer.Write(obs[i].poly.Count);
for (int j = 0; j < obs[i].poly.Count; j++)
{
WriteReducedCoord(obs[i].poly[j], writer);
}
}
}
break;
case DataTypeCode.BinarySerialized:
BinarySerializeData(ms, e.ObjectValue);
break;
}
// we've constructed the message, queue it to send to the listeners
lock (sendQueue) {
sendQueue.Enqueue(new SendQueueEntry(ms.GetBuffer(), (int)ms.Position));
// pulse the send event to indicate that there's stuff to send
Monitor.Pulse(sendQueue);
}
}
catch (Exception) {
// don't try to recover
if (ms != null)
{
FreeBuffer(ms);
}
}
}
internal void OnDataItemValueAdded(SourceDataValueAddedEventArgs e) {
if (!doSendDataValues)
return;
MemoryStream ms = null;
try {
// construct the message
ms = GetSendBuffer(e.AbstractDataItem.TypeCode);
BinaryWriter writer = new BinaryWriter(ms, Encoding.UTF8);
// write the message type -- for now we only send data value messages
writer.Write(DataItemComm.DataValueMsgID);
// write the sequence number and increment
writer.Write(seqNo);
Interlocked.Increment(ref seqNo);
// write the data item name
writer.Write(e.AbstractDataItem.Name);
// write the time as a double (seconds in car time)
writer.Write(e.Time.ts);
// figure out the type
DataTypeCode dtc = e.AbstractDataItem.TypeCode;
// write out the type code
writer.Write((int)dtc);
// write the data depending on the type
switch (dtc) {
case DataTypeCode.Double:
writer.Write(((SourceDataValueAddedEventArgs<double>)e).Value);
break;
case DataTypeCode.Single:
writer.Write(((SourceDataValueAddedEventArgs<float>)e).Value);
break;
case DataTypeCode.Int8:
writer.Write(((SourceDataValueAddedEventArgs<sbyte>)e).Value);
break;
case DataTypeCode.Int16:
writer.Write(((SourceDataValueAddedEventArgs<Int16>)e).Value);
break;
case DataTypeCode.Int32:
writer.Write(((SourceDataValueAddedEventArgs<Int32>)e).Value);
break;
case DataTypeCode.Int64:
writer.Write(((SourceDataValueAddedEventArgs<Int64>)e).Value);
break;
case DataTypeCode.UInt8:
writer.Write(((SourceDataValueAddedEventArgs<byte>)e).Value);
break;
case DataTypeCode.UInt16:
writer.Write(((SourceDataValueAddedEventArgs<UInt16>)e).Value);
break;
case DataTypeCode.UInt32:
writer.Write(((SourceDataValueAddedEventArgs<UInt32>)e).Value);
break;
case DataTypeCode.UInt64:
writer.Write(((SourceDataValueAddedEventArgs<UInt64>)e).Value);
break;
case DataTypeCode.Boolean:
writer.Write(((SourceDataValueAddedEventArgs<bool>)e).Value);
break;
case DataTypeCode.DateTime:
writer.Write(((SourceDataValueAddedEventArgs<DateTime>)e).Value.ToBinary());
break;
case DataTypeCode.TimeSpan:
writer.Write(((SourceDataValueAddedEventArgs<TimeSpan>)e).Value.Ticks);
break;
case DataTypeCode.Coordinates: {
Coordinates v = ((SourceDataValueAddedEventArgs<Coordinates>)e).Value;
WriteCoordinate(v, writer);
}
break;
case DataTypeCode.Circle: {
Circle c = ((SourceDataValueAddedEventArgs<Circle>)e).Value;
writer.Write(c.r);
WriteCoordinate(c.center, writer);
}
break;
case DataTypeCode.Line: {
Line l = ((SourceDataValueAddedEventArgs<Line>)e).Value;
WriteCoordinate(l.P0, writer);
WriteCoordinate(l.P1, writer);
}
break;
case DataTypeCode.LineSegment: {
LineSegment ls = ((SourceDataValueAddedEventArgs<LineSegment>)e).Value;
WriteCoordinate(ls.P0, writer);
WriteCoordinate(ls.P1, writer);
}
break;
case DataTypeCode.Polygon: {
Polygon pg = ((SourceDataValueAddedEventArgs<Polygon>)e).Value;
writer.Write((int)pg.CoordinateMode);
writer.Write(pg.Count);
foreach (Coordinates pg_pt in pg) {
WriteCoordinate(pg_pt, writer);
}
}
break;
case DataTypeCode.LineList: {
LineList ll = ((SourceDataValueAddedEventArgs<LineList>)e).Value;
writer.Write(ll.Count);
foreach (Coordinates ll_pt in ll) {
WriteCoordinate(ll_pt, writer);
}
}
break;
case DataTypeCode.Bezier: {
CubicBezier cb = ((SourceDataValueAddedEventArgs<CubicBezier>)e).Value;
WriteCoordinate(cb.P0, writer);
WriteCoordinate(cb.P1, writer);
WriteCoordinate(cb.P2, writer);
WriteCoordinate(cb.P3, writer);
}
break;
case DataTypeCode.CoordinatesArray: {
Coordinates[] pts = ((SourceDataValueAddedEventArgs<Coordinates[]>)e).Value;
writer.Write(pts.Length);
for (int i = 0; i < pts.Length; i++) {
WriteCoordinate(pts[i], writer);
}
}
break;
case DataTypeCode.LineListArray: {
LineList[] lineLists = ((SourceDataValueAddedEventArgs<LineList[]>)e).Value;
writer.Write(lineLists.Length);
for (int i = 0; i < lineLists.Length; i++) {
LineList list = lineLists[i];
writer.Write(list.Count);
for (int j = 0; j < list.Count; j++) {
WriteCoordinate(list[i], writer);
}
}
}
break;
case DataTypeCode.PolygonArray: {
Polygon[] polys = ((SourceDataValueAddedEventArgs<Polygon[]>)e).Value;
writer.Write(polys.Length);
for (int i = 0; i < polys.Length; i++) {
Polygon poly = polys[i];
writer.Write(poly.Count);
for (int j = 0; j < poly.Count; j++) {
WriteReducedCoord(poly[j], writer);
}
}
}
break;
case DataTypeCode.ObstacleArray: {
OperationalObstacle[] obs = ((SourceDataValueAddedEventArgs<OperationalObstacle[]>)e).Value;
writer.Write(obs.Length);
for (int i = 0; i < obs.Length; i++) {
writer.Write(obs[i].age);
writer.Write((int)obs[i].obstacleClass);
writer.Write(obs[i].ignored);
writer.Write(obs[i].headingValid);
writer.Write(obs[i].heading);
writer.Write(obs[i].poly.Count);
for (int j = 0; j < obs[i].poly.Count; j++) {
WriteReducedCoord(obs[i].poly[j], writer);
}
}
}
break;
case DataTypeCode.BinarySerialized:
BinarySerializeData(ms, e.ObjectValue);
break;
}
// we've constructed the message, queue it to send to the listeners
lock (sendQueue) {
sendQueue.Enqueue(new SendQueueEntry(ms.GetBuffer(), (int)ms.Position));
// pulse the send event to indicate that there's stuff to send
Monitor.Pulse(sendQueue);
}
}
catch (Exception) {
// don't try to recover
if (ms != null) {
FreeBuffer(ms);
}
}
}
internal void OnDataItemValueAdded(SourceDataValueAddedEventArgs e)
{
sender.OnDataItemValueAdded(e);
}
