add high level library for decoding and encoding

2026-08-17 05:13:36 +00:00 · 2024-12-19 18:30:19 -06:00
parent df3a3de47c
commit e8422106ca
4 changed files with 375 additions and 0 deletions
@@ -0,0 +1,5 @@
+package psn
+
+var PACKET_HEADER_SIZE = 16
+var MAX_UDP_PACKET_SIZE = 1500
+var CHUNK_HEADER_SIZE = 4
@@ -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)
+		}
+	}
+}
@@ -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
+}
@@ -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 = &timestamp
+}
+
+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
+}