/// <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); }