public void Reset()
{
lock (lockobj) {
rollingQueue.Clear();
recentTimestamp = double.NaN;
}
}
public void PushAbsolutePose(AbsolutePose pose)
{
lock (lockobj) {
queue.Add(pose);
if (!queue.VerifySort(delegate(AbsolutePose l, AbsolutePose r) { return(l.timestamp.CompareTo(r.timestamp)); }))
{
OperationalTrace.WriteError("absolute sort is donzoed, flushing queue");
queue.Clear();
}
}
}
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));
}
}
}
