/// <summary>
/// Override to provide new behavior for receipt of a PosiStageNet data packet
/// </summary>
protected virtual void OnDataPacketReceived(PsnDataPacketChunk dataPacket)
{
if (dataPacket.SubChunks.Count(c => c.ChunkId == PsnDataPacketChunkId.PsnDataHeader) > 1)
{
InvalidPacketReceived?.Invoke(this,
new InvalidPacketsReceivedEventArgs(dataPacket, false,
"Packet contains multiple data packet header chunks"));
return;
}
var headerChunk =
(PsnDataHeaderChunk)
dataPacket.SubChunks.FirstOrDefault(c => c.ChunkId == PsnDataPacketChunkId.PsnDataHeader);
if (headerChunk == null)
{
InvalidPacketReceived?.Invoke(this,
new InvalidPacketsReceivedEventArgs(dataPacket, false, "Packet missing data packet header chunk"));
return;
}
if (_currentDataPacketChunks.Any())
{
if (headerChunk.TimeStamp != _lastDataPacketHeader.TimeStamp ||
headerChunk.FramePacketCount != _lastDataPacketHeader.FramePacketCount)
{
InvalidPacketReceived?.Invoke(this,
new InvalidPacketsReceivedEventArgs(_currentDataPacketChunks, false,
"Incomplete packet chunk discarded, did not receive all packets for data frame"));
_currentDataPacketChunks.Clear();
}
}
_lastDataPacketHeader = headerChunk;
_currentDataPacketChunks.Add(dataPacket);
if (_currentDataPacketChunks.Count == _lastDataPacketHeader.FramePacketCount)
{
OnCompleteDataFrameReceived(_lastDataPacketHeader, _currentDataPacketChunks);
_currentDataPacketChunks.Clear();
}
}
/// <summary>
/// Override to provide new behavior for a receipt of a complete PosiStageNet data frame
/// </summary>
/// <param name="header"></param>
/// <param name="dataPackets"></param>
protected virtual void OnCompleteDataFrameReceived(PsnDataHeaderChunk header,
IReadOnlyCollection <PsnDataPacketChunk> dataPackets)
{
var dataTrackerChunks = dataPackets.SelectMany(p =>
((PsnDataTrackerListChunk)p.SubChunks.Single(c => c.ChunkId == PsnDataPacketChunkId.PsnDataTrackerList))
.SubChunks)
.ToList();
if (dataTrackerChunks.Select(c => c.TrackerId).Distinct().Count() != dataTrackerChunks.Count)
{
InvalidPacketReceived?.Invoke(this,
new InvalidPacketsReceivedEventArgs(dataPackets, false, "Duplicate tracker IDs in frame"));
return;
}
foreach (var chunk in dataTrackerChunks)
{
if (!_trackers.TryGetValue(chunk.TrackerId, out var tracker))
{
tracker = new PsnTracker(chunk.TrackerId);
}
Tuple <float, float, float> position = null;
Tuple <float, float, float> speed = null;
Tuple <float, float, float> orientation = null;
Tuple <float, float, float> acceleration = null;
Tuple <float, float, float> targetPosition = null;
float?validity = null;
foreach (var subchunk in chunk.SubChunks)
{
switch (subchunk.ChunkId)
{
case PsnDataTrackerChunkId.PsnDataTrackerPos:
position = ((PsnDataTrackerPosChunk)subchunk).Vector;
break;
case PsnDataTrackerChunkId.PsnDataTrackerSpeed:
speed = ((PsnDataTrackerSpeedChunk)subchunk).Vector;
break;
case PsnDataTrackerChunkId.PsnDataTrackerOri:
orientation = ((PsnDataTrackerOriChunk)subchunk).Vector;
break;
case PsnDataTrackerChunkId.PsnDataTrackerStatus:
validity = ((PsnDataTrackerStatusChunk)subchunk).Validity;
break;
case PsnDataTrackerChunkId.PsnDataTrackerAccel:
acceleration = ((PsnDataTrackerAccelChunk)subchunk).Vector;
break;
case PsnDataTrackerChunkId.PsnDataTrackerTrgtPos:
targetPosition = ((PsnDataTrackerTrgtPosChunk)subchunk).Vector;
break;
default:
throw new ArgumentOutOfRangeException(nameof(subchunk.ChunkId));
}
}
tracker = PsnTracker.CloneInternal(tracker, dataTimeStamp: header.TimeStamp,
position: position, clearPosition: position == null,
speed: speed, clearSpeed: speed == null,
orientation: orientation, clearOrientation: orientation == null,
acceleration: acceleration, clearAcceleration: acceleration == null,
targetposition: targetPosition, clearTargetPosition: targetPosition == null,
validity: validity, clearValidity: validity == null);
_trackers[chunk.TrackerId] = tracker;
}
TrackersUpdated?.Invoke(this, Trackers);
}
