void AddPosSample(Position3D p)
{
posHist.AddLast(new TimestampedPosition {
ts = DateTime.Now, pos = p
});
while (posHist.Count > 0 && (DateTime.Now - posHist.First.Value.ts) > TimeSpan.FromSeconds(5))
{
posHist.RemoveFirst();
}
if (posHist.Count > 1)
{
Position3D startPos = posHist.First.Value.pos;
Position3D endPos = posHist.Last.Value.pos;
double seconds = (posHist.Last.Value.ts - posHist.First.Value.ts).TotalSeconds;
Distance d = startPos.DistanceFromOnEarth(endPos);
Debug.WriteLine("Seconds elapsed=" + seconds + ", distance=" + d.ToImperialUnitType().ToString());
Speed5sec = new Speed(d.Value / seconds, SpeedUnit.KilometersPerSecond);
}
}
