diff --git a/constants.go b/constants.go new file mode 100644 index 0000000..f1c1f2c --- /dev/null +++ b/constants.go @@ -0,0 +1,5 @@ +package psn + +var PACKET_HEADER_SIZE = 16 +var MAX_UDP_PACKET_SIZE = 1500 +var CHUNK_HEADER_SIZE = 4 diff --git a/decoder.go b/decoder.go new file mode 100644 index 0000000..aa28bef --- /dev/null +++ b/decoder.go @@ -0,0 +1,85 @@ +package psn + +import "github.com/jwetzell/psn-go/internal/decoders" + +type Decoder struct { + lastInfoPacketHeader *decoders.PacketHeaderChunk + lastDataPacketHeader *decoders.PacketHeaderChunk + infoPacketFrames map[uint8][]decoders.InfoPacketChunk + dataPacketFrames map[uint8][]decoders.DataPacketChunk + Trackers map[uint16]*Tracker + SystemName string +} + +func NewDecoder() *Decoder { + var decoder Decoder + decoder.infoPacketFrames = map[uint8][]decoders.InfoPacketChunk{} + decoder.dataPacketFrames = map[uint8][]decoders.DataPacketChunk{} + decoder.Trackers = map[uint16]*Tracker{} + return &decoder +} + +func (d *Decoder) updateInfo(framePackets []decoders.InfoPacketChunk) { + for _, framePacket := range framePackets { + d.SystemName = framePacket.Data.SystemName.Data.SystemName + for _, infoTrackerChunk := range framePacket.Data.TrackerList.Data.Trackers { + tracker, ok := d.Trackers[infoTrackerChunk.Chunk.Header.Id] + + if ok { + tracker.UpdateInfo(infoTrackerChunk) + } else { + d.Trackers[infoTrackerChunk.Chunk.Header.Id] = TrackerFromInfo(infoTrackerChunk) + } + } + } +} + +func (d *Decoder) updateData(framePackets []decoders.DataPacketChunk) { + for _, framePacket := range framePackets { + for _, dataTrackerChunk := range framePacket.Data.TrackerList.Data.Trackers { + tracker, ok := d.Trackers[dataTrackerChunk.Chunk.Header.Id] + + if ok { + tracker.UpdateData(dataTrackerChunk) + } else { + d.Trackers[dataTrackerChunk.Chunk.Header.Id] = TrackerFromData(dataTrackerChunk) + } + } + } +} + +func (d *Decoder) Decode(bytes []byte) { + chunk := decoders.DecodeChunk(bytes) + + if chunk.Header.Id == 0x6756 { + infoPacket := decoders.DecodeInfoPacketChunk(bytes) + currentInfoPacketHeader := infoPacket.Data.PacketHeader + + _, ok := d.infoPacketFrames[currentInfoPacketHeader.Data.FrameId] + + if !ok { + d.infoPacketFrames[currentInfoPacketHeader.Data.FrameId] = []decoders.InfoPacketChunk{} + } + d.infoPacketFrames[currentInfoPacketHeader.Data.FrameId] = append(d.infoPacketFrames[currentInfoPacketHeader.Data.FrameId], infoPacket) + + if len(d.infoPacketFrames[currentInfoPacketHeader.Data.FrameId]) == int(currentInfoPacketHeader.Data.FramePacketCount) { + d.updateInfo(d.infoPacketFrames[currentInfoPacketHeader.Data.FrameId]) + delete(d.infoPacketFrames, currentInfoPacketHeader.Data.FrameId) + } + } else if chunk.Header.Id == 0x6755 { + dataPacket := decoders.DecodeDataPacketChunk(bytes) + currentInfoPacketHeader := dataPacket.Data.PacketHeader + + _, ok := d.dataPacketFrames[currentInfoPacketHeader.Data.FrameId] + + if !ok { + d.dataPacketFrames[currentInfoPacketHeader.Data.FrameId] = []decoders.DataPacketChunk{} + } + d.dataPacketFrames[currentInfoPacketHeader.Data.FrameId] = append(d.dataPacketFrames[currentInfoPacketHeader.Data.FrameId], dataPacket) + + if len(d.dataPacketFrames[currentInfoPacketHeader.Data.FrameId]) == int(currentInfoPacketHeader.Data.FramePacketCount) { + d.updateData(d.dataPacketFrames[currentInfoPacketHeader.Data.FrameId]) + delete(d.dataPacketFrames, currentInfoPacketHeader.Data.FrameId) + } + } +} diff --git a/encoder.go b/encoder.go new file mode 100644 index 0000000..adfe656 --- /dev/null +++ b/encoder.go @@ -0,0 +1,99 @@ +package psn + +import "github.com/jwetzell/psn-go/internal/encoders" + +type Encoder struct { + SystemName string + VersionHigh uint8 + VersionLow uint8 + dataFrameId uint8 + infoFrameId uint8 +} + +func (e *Encoder) ResetDataFrameId() { + e.dataFrameId = 1 +} + +func (e *Encoder) ResetInfoFrameId() { + e.infoFrameId = 1 +} + +func (e *Encoder) GetInfoPackets(timestamp uint64, trackers []Tracker) [][]byte { + systemNameChunk := encoders.EncodeInfoSystemNameChunk(e.SystemName) + + trackerChunks := [][]byte{} + + for _, tracker := range trackers { + trackerChunks = append(trackerChunks, tracker.GetInfoChunk()) + } + + infoPackets := [][]byte{} + + trackerChunksLists := [][][]byte{} + currentTrackerList := [][]byte{} + + currentInfoPacketSize := PACKET_HEADER_SIZE + len(systemNameChunk) + CHUNK_HEADER_SIZE + + for _, trackerChunk := range trackerChunks { + if (currentInfoPacketSize + len(trackerChunk)) > MAX_UDP_PACKET_SIZE { + trackerChunksLists = append(trackerChunksLists, currentTrackerList) + currentTrackerList = [][]byte{} + currentInfoPacketSize = 0 + } + currentTrackerList = append(currentTrackerList, trackerChunk) + currentInfoPacketSize += len(trackerChunk) + } + trackerChunksLists = append(trackerChunksLists, currentTrackerList) + header := encoders.EncodePacketHeaderChunk(timestamp, e.VersionHigh, e.VersionLow, e.infoFrameId, uint8(len(trackerChunksLists))) + + for _, trackerChunkList := range trackerChunksLists { + infoPackets = append(infoPackets, encoders.EncodeInfoPacketChunk(header, systemNameChunk, encoders.EncodeInfoTrackerListChunk(trackerChunkList))) + } + + if e.infoFrameId == 255 { + e.infoFrameId = 0 + } else { + e.infoFrameId += 1 + } + return infoPackets +} + +func (e *Encoder) GetDataPackets(timestamp uint64, trackers []Tracker) [][]byte { + + trackerChunks := [][]byte{} + + for _, tracker := range trackers { + trackerChunks = append(trackerChunks, tracker.GetDataChunk()) + } + + dataPackets := [][]byte{} + + trackerChunksLists := [][][]byte{} + currentTrackerList := [][]byte{} + + currentDataPacketSize := PACKET_HEADER_SIZE + CHUNK_HEADER_SIZE + + for _, trackerChunk := range trackerChunks { + if (currentDataPacketSize + len(trackerChunk)) > MAX_UDP_PACKET_SIZE { + trackerChunksLists = append(trackerChunksLists, currentTrackerList) + currentTrackerList = [][]byte{} + currentDataPacketSize = 0 + } + currentTrackerList = append(currentTrackerList, trackerChunk) + currentDataPacketSize += len(trackerChunk) + } + trackerChunksLists = append(trackerChunksLists, currentTrackerList) + header := encoders.EncodePacketHeaderChunk(timestamp, e.VersionHigh, e.VersionLow, e.dataFrameId, uint8(len(trackerChunksLists))) + + for _, trackerChunkList := range trackerChunksLists { + dataPackets = append(dataPackets, encoders.EncodeDataPacketChunk(header, encoders.EncodeDataTrackerListChunk(trackerChunkList))) + } + + if e.dataFrameId == 255 { + e.dataFrameId = 0 + } else { + e.dataFrameId += 1 + } + + return dataPackets +} diff --git a/tracker.go b/tracker.go new file mode 100644 index 0000000..6c4168f --- /dev/null +++ b/tracker.go @@ -0,0 +1,186 @@ +package psn + +import ( + "github.com/jwetzell/psn-go/internal/decoders" + "github.com/jwetzell/psn-go/internal/encoders" +) + +type XYZData struct { + X float32 `json:"x"` + Y float32 `json:"y"` + Z float32 `json:"z"` +} + +type Tracker struct { + Id uint16 + Name string + Pos *XYZData + Speed *XYZData + Ori *XYZData + Validity *float32 + Accel *XYZData + TrgtPos *XYZData + Timestamp *uint64 +} + +func (t *Tracker) SetPos(x float32, y float32, z float32) { + if t.Pos == nil { + t.Pos = &XYZData{ + X: x, + Y: y, + Z: z, + } + } else { + t.Pos.X = x + t.Pos.Y = y + t.Pos.Z = z + } +} + +func (t *Tracker) SetSpeed(x float32, y float32, z float32) { + if t.Speed == nil { + t.Speed = &XYZData{ + X: x, + Y: y, + Z: z, + } + } else { + t.Speed.X = x + t.Speed.Y = y + t.Speed.Z = z + } +} + +func (t *Tracker) SetOri(x float32, y float32, z float32) { + if t.Ori == nil { + t.Ori = &XYZData{ + X: x, + Y: y, + Z: z, + } + } else { + t.Ori.X = x + t.Ori.Y = y + t.Ori.Z = z + } +} + +func (t *Tracker) SetStatus(validity float32) { + t.Validity = &validity +} + +func (t *Tracker) SetAccel(x float32, y float32, z float32) { + if t.Accel == nil { + t.Accel = &XYZData{ + X: x, + Y: y, + Z: z, + } + } else { + t.Accel.X = x + t.Accel.Y = y + t.Accel.Z = z + } +} + +func (t *Tracker) SetTrgtPos(x float32, y float32, z float32) { + if t.TrgtPos == nil { + t.TrgtPos = &XYZData{ + X: x, + Y: y, + Z: z, + } + } else { + t.TrgtPos.X = x + t.TrgtPos.Y = y + t.TrgtPos.Z = z + } +} + +func (t *Tracker) SetTimestamp(timestamp uint64) { + t.Timestamp = ×tamp +} + +func (t *Tracker) GetDataChunk() []byte { + fieldChunks := [][]byte{} + if t.Pos != nil { + fieldChunks = append(fieldChunks, encoders.EncodeDataTrackerPosChunk(t.Pos.X, t.Pos.Y, t.Pos.Z)) + } + + if t.Speed != nil { + fieldChunks = append(fieldChunks, encoders.EncodeDataTrackerSpeedChunk(t.Speed.X, t.Speed.Y, t.Speed.Z)) + } + + if t.Ori != nil { + fieldChunks = append(fieldChunks, encoders.EncodeDataTrackerOriChunk(t.Ori.X, t.Ori.Y, t.Ori.Z)) + } + + if t.Validity != nil { + fieldChunks = append(fieldChunks, encoders.EncodeDataTrackerStatusChunk(*t.Validity)) + } + + if t.Accel != nil { + fieldChunks = append(fieldChunks, encoders.EncodeDataTrackerAccelChunk(t.Accel.X, t.Accel.Y, t.Accel.Z)) + } + + if t.TrgtPos != nil { + fieldChunks = append(fieldChunks, encoders.EncodeDataTrackerTrgtPosChunk(t.TrgtPos.X, t.TrgtPos.Y, t.TrgtPos.Z)) + } + + if t.Timestamp != nil { + fieldChunks = append(fieldChunks, encoders.EncodeDataTrackerTimestampChunk(*t.Timestamp)) + } + + return encoders.EncodeDataTrackerChunk(t.Id, fieldChunks) +} + +func (t *Tracker) GetInfoChunk() []byte { + return encoders.EncodeInfoTrackerChunk(t.Id, encoders.EncodeInfoTrackerNameChunk(t.Name)) +} + +func (t *Tracker) UpdateInfo(infoTrackerChunk decoders.InfoTrackerChunk) { + t.Id = infoTrackerChunk.Chunk.Header.Id + t.Name = infoTrackerChunk.Data.TrackerName.Data.TrackerName +} + +func (t *Tracker) UpdateData(dataTrackerChunk decoders.DataTrackerChunk) { + if dataTrackerChunk.Data.Pos != nil { + t.SetPos(dataTrackerChunk.Data.Pos.Data.X, dataTrackerChunk.Data.Pos.Data.Y, dataTrackerChunk.Data.Pos.Data.Z) + } + + if dataTrackerChunk.Data.Speed != nil { + t.SetSpeed(dataTrackerChunk.Data.Speed.Data.X, dataTrackerChunk.Data.Speed.Data.Y, dataTrackerChunk.Data.Speed.Data.Z) + } + + if dataTrackerChunk.Data.Ori != nil { + t.SetOri(dataTrackerChunk.Data.Ori.Data.X, dataTrackerChunk.Data.Ori.Data.Y, dataTrackerChunk.Data.Ori.Data.Z) + } + + if dataTrackerChunk.Data.Status != nil { + t.SetStatus(dataTrackerChunk.Data.Status.Data.Validity) + } + + if dataTrackerChunk.Data.Accel != nil { + t.SetAccel(dataTrackerChunk.Data.Accel.Data.X, dataTrackerChunk.Data.Accel.Data.Y, dataTrackerChunk.Data.Accel.Data.Z) + } + + if dataTrackerChunk.Data.TrgtPos != nil { + t.SetTrgtPos(dataTrackerChunk.Data.TrgtPos.Data.X, dataTrackerChunk.Data.TrgtPos.Data.Y, dataTrackerChunk.Data.TrgtPos.Data.Z) + } + + if dataTrackerChunk.Data.Timestamp != nil { + t.SetTimestamp(dataTrackerChunk.Data.Timestamp.Data.Timestamp) + } +} + +func TrackerFromInfo(infoTrackerChunk decoders.InfoTrackerChunk) *Tracker { + var tracker Tracker + tracker.UpdateInfo(infoTrackerChunk) + return &tracker +} + +func TrackerFromData(dataTrackerChunk decoders.DataTrackerChunk) *Tracker { + var tracker Tracker + tracker.UpdateData(dataTrackerChunk) + return &tracker +}