diff --git a/decoder.js b/decoder.js new file mode 100644 index 0000000..6ed1b04 --- /dev/null +++ b/decoder.js @@ -0,0 +1,142 @@ +const PacketParser = require('./decoders/packet'); +const InfoPacket = require('./models/packets/info-packet'); +const DataPacket = require('./models/packets/data-packet'); + +class Decoder { + lastInfoPacketHeader; + + lastDataPacketHeader; + + infoPacketFrames = {}; + + dataPacketFrames = {}; + + constructor() { + this.info = { + trackers: {}, + }; + + this.data = { + trackers: {}, + }; + } + + updateInfo(framePackets) { + // console.log('info update'); + framePackets.forEach((packet) => { + packet.subChunks?.forEach((subChunk) => { + if (subChunk.id === 0x0001) { + // NOTE(jwetzell): system name subChunk + this.info.system_name = subChunk.system_name; + } else if (subChunk.id === 0x0002) { + // console.log(subChunk); + subChunk.trackers?.forEach((tracker) => { + if (this.info.trackers[tracker.id] === undefined) { + this.info.trackers[tracker.id] = {}; + } + this.info.trackers[tracker.id].name = tracker.tracker_name.tracker_name; + }); + } + }); + }); + } + + updateData(framePackets) { + // console.log('data update'); + framePackets.forEach((packet) => { + packet.subChunks?.forEach((subChunk) => { + if (subChunk.id === 0x0001) { + subChunk.trackers?.forEach((tracker) => { + if (this.data.trackers[tracker.id] === undefined) { + this.data.trackers[tracker.id] = {}; + } + tracker.fields?.forEach((field) => { + Object.entries(field).forEach(([key, value]) => { + this.data.trackers[tracker.id][key] = value; + }); + }); + }); + } + }); + }); + } + + // TODO(jwetzell): support multiple frame packets this might require some rework of how chunks are assumed + decode(packetBuf) { + const packet = PacketParser.parse(packetBuf); + // console.log(packet); + + if (packet.id === 0x6756) { + const infoPacket = new InfoPacket(packet); + if (infoPacket) { + if (infoPacket.subChunks?.length > 0) { + const currentInfoPacketHeader = infoPacket.getHeaderPacket(); + // console.log(currentInfoPacketHeader); + if (!currentInfoPacketHeader) { + // NOTE(jwetzell): not sure that info packets without a header subchunk are valid? + return; + } + + const systemSubChunk = infoPacket.getSystemPacket(); + // console.log(systemSubChunk); + if (!systemSubChunk) { + // NOTE(jwetzell): not sure that info packets without a system subchunk are valid? + return; + } + + if (this.infoPacketFrames[currentInfoPacketHeader.frame_id] === undefined) { + this.infoPacketFrames[currentInfoPacketHeader.frame_id] = []; + } + this.infoPacketFrames[currentInfoPacketHeader.frame_id].push(infoPacket); + + if ( + this.infoPacketFrames[currentInfoPacketHeader.frame_id].length === + currentInfoPacketHeader.frame_packet_count + ) { + // console.log('found complete info frame'); + this.updateInfo(this.infoPacketFrames[currentInfoPacketHeader.frame_id]); + delete this.infoPacketFrames[currentInfoPacketHeader.frame_id]; + } else { + // TODO(jwetzell): need to compute whether a frame is complete + // console.log('compute frame completion'); + } + + this.lastInfoPacketHeader = currentInfoPacketHeader; + } + } + } else if (packet.id === 0x6755) { + const dataPacket = new DataPacket(packet); + // console.log(dataPacket); + if (dataPacket) { + if (dataPacket.subChunks?.length > 0) { + const currentDataPacketHeader = dataPacket.getHeaderPacket(); + if (!currentDataPacketHeader) { + // NOTE(jwetzell): not sure that info packets without a header subchunk are valid? + console.error('data packet with no header'); + return; + } + + if (this.dataPacketFrames[currentDataPacketHeader.frame_id] === undefined) { + this.dataPacketFrames[currentDataPacketHeader.frame_id] = []; + } + this.dataPacketFrames[currentDataPacketHeader.frame_id].push(dataPacket); + + if ( + this.dataPacketFrames[currentDataPacketHeader.frame_id].length === + currentDataPacketHeader.frame_packet_count + ) { + this.updateData(this.dataPacketFrames[currentDataPacketHeader.frame_id]); + delete this.dataPacketFrames[currentDataPacketHeader.frame_id]; + } else { + // TODO(jwetzell): need to compute whether a frame is complete + // console.log('compute data frame completion'); + } + + this.lastDataPacketHeader = currentDataPacketHeader; + } + } + } + } +} + +module.exports = Decoder; diff --git a/decoders/chunk.js b/decoders/chunk.js new file mode 100644 index 0000000..dbb4ba7 --- /dev/null +++ b/decoders/chunk.js @@ -0,0 +1,20 @@ +const { Parser } = require('binary-parser'); + +module.exports = new Parser() + .uint16le('id') + .uint16le('data_len', { + formatter: (item) => { + const binary = item.toString(2).padStart(16, '0'); + return Number.parseInt(binary.substring(1), 2); + }, + }) + .seek(-2) + .uint16le('has_subchunks', { + formatter: (item) => { + const binary = item.toString(2).padStart(16, '0'); + return binary.charAt(0) === '1'; + }, + }) + .buffer('chunk_data', { + length: 'data_len', + }); diff --git a/parsers/data/index.js b/decoders/data/data-tracker-field-chunk.js similarity index 58% rename from parsers/data/index.js rename to decoders/data/data-tracker-field-chunk.js index 7f664aa..06eaabc 100644 --- a/parsers/data/index.js +++ b/decoders/data/data-tracker-field-chunk.js @@ -1,6 +1,6 @@ const Parser = require('binary-parser').Parser; -const DataTrackerFieldParser = new Parser() +module.exports = new Parser() .uint16le('id') .uint16le('data_len', { formatter: (item) => { @@ -27,29 +27,3 @@ const DataTrackerFieldParser = new Parser() 0x0006: new Parser().uint64le('tracker_timestamp'), }, }); - -const PSN_DATA_TRACKER_PARSER = new Parser() - .uint16le('id') - .uint16le('data_len', { - formatter: (item) => { - const binary = item.toString(2).padStart(16, '0'); - return Number.parseInt(binary.substring(1), 2); - }, - }) - .seek(-2) - .uint16le('has_subchunks', { - formatter: (item) => { - const binary = item.toString(2).padStart(16, '0'); - return binary.charAt(0) === '1'; - }, - }) - .buffer('data', { - length: 'data_len', - }); - -const DataTrackerListParser = new Parser().array('trackers', { type: PSN_DATA_TRACKER_PARSER, readUntil: 'eof' }); - -module.exports = { - DataTrackerFieldParser, - DataTrackerListParser, -}; diff --git a/parsers/common/index.js b/decoders/data/data-tracker-list-chunk.js similarity index 60% rename from parsers/common/index.js rename to decoders/data/data-tracker-list-chunk.js index 05e9559..6ff58e7 100644 --- a/parsers/common/index.js +++ b/decoders/data/data-tracker-list-chunk.js @@ -1,6 +1,6 @@ const Parser = require('binary-parser').Parser; -const ChunkParser = new Parser() +const dataTrackerParser = new Parser() .uint16le('id') .uint16le('data_len', { formatter: (item) => { @@ -15,19 +15,8 @@ const ChunkParser = new Parser() return binary.charAt(0) === '1'; }, }) - .buffer('chunk_data', { + .buffer('data', { length: 'data_len', }); -const HeaderParser = new Parser() - .uint64le('packet_timestamp') - .uint8('version_high') - .uint8('version_low') - .uint8('frame_id') - .uint8('frame_packet_count') - .seek(4); - -module.exports = { - ChunkParser, - HeaderParser, -}; +module.exports = new Parser().array('trackers', { type: dataTrackerParser, readUntil: 'eof' }); diff --git a/decoders/data/index.js b/decoders/data/index.js new file mode 100644 index 0000000..07a3f67 --- /dev/null +++ b/decoders/data/index.js @@ -0,0 +1,7 @@ +const TrackerListChunk = require('./data-tracker-list-chunk'); +const TrackerFieldChunk = require('./data-tracker-field-chunk'); + +module.exports = { + TrackerFieldChunk, + TrackerListChunk, +}; diff --git a/decoders/index.js b/decoders/index.js new file mode 100644 index 0000000..ed1599a --- /dev/null +++ b/decoders/index.js @@ -0,0 +1,17 @@ +const Chunk = require('./chunk'); +const PacketHeaderChunk = require('./packet-header-chunk'); +const Packet = require('./packet'); +const Info = require('./info'); +const Data = require('./data'); + +module.exports = { + Chunk, + PacketHeaderChunk, + Packet, + Info: { + ...Info, + }, + Data: { + ...Data, + }, +}; diff --git a/decoders/info/index.js b/decoders/info/index.js new file mode 100644 index 0000000..4769124 --- /dev/null +++ b/decoders/info/index.js @@ -0,0 +1,7 @@ +const SystemNameChunk = require('./info-system-name-chunk'); +const TrackerListChunk = require('./info-tracker-list-chunk'); + +module.exports = { + SystemNameChunk, + TrackerListChunk, +}; diff --git a/decoders/info/info-system-name-chunk.js b/decoders/info/info-system-name-chunk.js new file mode 100644 index 0000000..0dc1c8f --- /dev/null +++ b/decoders/info/info-system-name-chunk.js @@ -0,0 +1,3 @@ +const Parser = require('binary-parser').Parser; + +module.exports = new Parser().string('system_name', { greedy: true }); diff --git a/parsers/info/index.js b/decoders/info/info-tracker-list-chunk.js similarity index 68% rename from parsers/info/index.js rename to decoders/info/info-tracker-list-chunk.js index f224125..2db1431 100644 --- a/parsers/info/index.js +++ b/decoders/info/info-tracker-list-chunk.js @@ -1,6 +1,6 @@ const Parser = require('binary-parser').Parser; -const PSN_INFO_TRACKER_NAME_PARSER = new Parser() +const infoTrackerNameParser = new Parser() .uint16le('id') .uint16le('data_len', { formatter: (item) => { @@ -17,7 +17,7 @@ const PSN_INFO_TRACKER_NAME_PARSER = new Parser() }) .string('tracker_name', { length: 'data_len' }); -const PSN_INFO_TRACKER_PARSER = new Parser() +const infoTrackerParser = new Parser() .uint16le('id') .uint16le('data_len', { formatter: (item) => { @@ -33,14 +33,7 @@ const PSN_INFO_TRACKER_PARSER = new Parser() }, }) .nest('tracker_name', { - type: PSN_INFO_TRACKER_NAME_PARSER, + type: infoTrackerNameParser, }); -const InfoSystemNameParser = new Parser().string('system_name', { greedy: true }); - -const InfoTrackerListParser = new Parser().array('trackers', { type: PSN_INFO_TRACKER_PARSER, readUntil: 'eof' }); - -module.exports = { - InfoSystemNameParser, - InfoTrackerListParser, -}; +module.exports = new Parser().array('trackers', { type: infoTrackerParser, readUntil: 'eof' }); diff --git a/decoders/packet-header-chunk.js b/decoders/packet-header-chunk.js new file mode 100644 index 0000000..6672db3 --- /dev/null +++ b/decoders/packet-header-chunk.js @@ -0,0 +1,9 @@ +const Parser = require('binary-parser').Parser; + +module.exports = new Parser() + .uint64le('packet_timestamp') + .uint8('version_high') + .uint8('version_low') + .uint8('frame_id') + .uint8('frame_packet_count') + .seek(4); diff --git a/parsers/index.js b/decoders/packet.js similarity index 82% rename from parsers/index.js rename to decoders/packet.js index f485af9..e5d915a 100644 --- a/parsers/index.js +++ b/decoders/packet.js @@ -1,6 +1,6 @@ const Parser = require('binary-parser').Parser; -const PSN_PACKET_PARSER = new Parser() +module.exports = new Parser() .uint16le('id') .uint16le('data_len', { formatter: (item) => { @@ -16,5 +16,3 @@ const PSN_PACKET_PARSER = new Parser() }, }) .buffer('chunk_data', { readUntil: 'eof' }); - -module.exports = (buffer) => PSN_PACKET_PARSER.parse(buffer); diff --git a/encoder.js b/encoder.js new file mode 100644 index 0000000..a1d2a4b --- /dev/null +++ b/encoder.js @@ -0,0 +1,95 @@ +const dataChunk = require('./encoders/data/data-chunk'); +const dataTrackerListChunk = require('./encoders/data/data-tracker-list-chunk'); +const infoChunk = require('./encoders/info/info-chunk'); +const infoSystemNameChunk = require('./encoders/info/info-system-name-chunk'); +const infoTrackerListChunk = require('./encoders/info/info-tracker-list-chunk'); +const packetHeaderChunk = require('./encoders/packet-header-chunk'); + +const MAX_TRACKER_LIST_SIZE = 1000; + +class Encoder { + constructor(systemName, versionHigh, versionLow) { + this.systemName = systemName; + this.versionHigh = versionHigh; + this.versionLow = versionLow; + this.dataFrameId = 1; + this.infoFrameId = 1; + } + + getInfoPackets(timestamp, trackers) { + const trackerChunks = trackers.map((tracker) => tracker.getInfoChunk()); + + const infoPackets = []; + + const trackerChunksLists = []; + let currentTrackerList = []; + + let currentTrackerListSize = 0; + + trackerChunks.forEach((trackerChunk) => { + if (currentTrackerListSize + trackerChunk.length > MAX_TRACKER_LIST_SIZE) { + trackerChunksLists.push(currentTrackerList); + currentTrackerList = []; + currentTrackerListSize = 0; + } + currentTrackerList.push(trackerChunk); + currentTrackerListSize += trackerChunk.length; + }); + trackerChunksLists.push(currentTrackerList); + const header = packetHeaderChunk( + timestamp, + this.versionHigh, + this.versionLow, + this.infoFrameId, + trackerChunksLists.length + ); + trackerChunksLists.forEach((trackerChunkList) => { + infoPackets.push(infoChunk(header, infoSystemNameChunk(this.systemName), infoTrackerListChunk(trackerChunkList))); + }); + this.dataFrameId += 1; + if (this.dataFrameId > 255) { + this.dataFrameId = 0; + } + return infoPackets; + } + + getDataPackets(timestamp, trackers) { + const allTrackerChunks = trackers.map((tracker) => tracker.getDataChunk()); + + const dataPackets = []; + + const trackerChunksLists = []; + let currentTrackerList = []; + + let currentTrackerListSize = 0; + + allTrackerChunks.forEach((trackerChunk) => { + if (currentTrackerListSize + trackerChunk.length > MAX_TRACKER_LIST_SIZE) { + trackerChunksLists.push(currentTrackerList); + currentTrackerList = []; + currentTrackerListSize = 0; + } + currentTrackerList.push(trackerChunk); + currentTrackerListSize += trackerChunk.length; + }); + trackerChunksLists.push(currentTrackerList); + + const header = packetHeaderChunk( + timestamp, + this.versionHigh, + this.versionLow, + this.dataFrameId, + trackerChunksLists.length + ); + trackerChunksLists.forEach((trackerChunks) => { + dataPackets.push(dataChunk(header, dataTrackerListChunk(trackerChunks))); + }); + this.dataFrameId += 1; + if (this.dataFrameId > 255) { + this.dataFrameId = 0; + } + return dataPackets; + } +} + +module.exports = Encoder; diff --git a/encoders/chunk.js b/encoders/chunk.js new file mode 100644 index 0000000..bb67573 --- /dev/null +++ b/encoders/chunk.js @@ -0,0 +1,13 @@ +module.exports = (id, chunkData, hasSubchunks) => { + if (chunkData.length > 0x7fff) { + throw new Error('chunkData can not be greater than 32767 bytes'); + } + + const header = Buffer.alloc(4); + header.writeUInt16LE(id); + const chunkLengthBinaryString = chunkData.length.toString(2).padStart(15, '0'); + const secondByteBinary = `${hasSubchunks ? '1' : '0'}${chunkLengthBinaryString}`; + header.writeUInt16LE(parseInt(secondByteBinary, 2), 2); + + return Buffer.concat([header, chunkData]); +}; diff --git a/encoders/data/data-chunk.js b/encoders/data/data-chunk.js new file mode 100644 index 0000000..131ebe1 --- /dev/null +++ b/encoders/data/data-chunk.js @@ -0,0 +1,4 @@ +const chunk = require('../chunk'); + +module.exports = (packetHeaderChunk, trackerListChunk) => + chunk(0x6755, Buffer.concat([packetHeaderChunk, trackerListChunk]), true); diff --git a/encoders/data/data-tracker-accel-chunk.js b/encoders/data/data-tracker-accel-chunk.js new file mode 100644 index 0000000..b348e56 --- /dev/null +++ b/encoders/data/data-tracker-accel-chunk.js @@ -0,0 +1,9 @@ +const chunk = require('../chunk'); + +module.exports = (x, y, z) => { + const buf = Buffer.alloc(12); + buf.writeFloatLE(x); + buf.writeFloatLE(y, 4); + buf.writeFloatLE(z, 8); + return chunk(0x0004, buf, false); +}; diff --git a/encoders/data/data-tracker-chunk.js b/encoders/data/data-tracker-chunk.js new file mode 100644 index 0000000..f3d5985 --- /dev/null +++ b/encoders/data/data-tracker-chunk.js @@ -0,0 +1,3 @@ +const chunk = require('../chunk'); + +module.exports = (trackerId, fieldChunks) => chunk(trackerId, Buffer.concat(fieldChunks), true); diff --git a/encoders/data/data-tracker-list-chunk.js b/encoders/data/data-tracker-list-chunk.js new file mode 100644 index 0000000..b040226 --- /dev/null +++ b/encoders/data/data-tracker-list-chunk.js @@ -0,0 +1,3 @@ +const chunk = require('../chunk'); + +module.exports = (trackerChunks) => chunk(0x0001, Buffer.concat(trackerChunks), true); diff --git a/encoders/data/data-tracker-ori-chunk.js b/encoders/data/data-tracker-ori-chunk.js new file mode 100644 index 0000000..074cb47 --- /dev/null +++ b/encoders/data/data-tracker-ori-chunk.js @@ -0,0 +1,9 @@ +const chunk = require('../chunk'); + +module.exports = (x, y, z) => { + const buf = Buffer.alloc(12); + buf.writeFloatLE(x); + buf.writeFloatLE(y, 4); + buf.writeFloatLE(z, 8); + return chunk(0x0002, buf, false); +}; diff --git a/encoders/data/data-tracker-pos-chunk.js b/encoders/data/data-tracker-pos-chunk.js new file mode 100644 index 0000000..95ebdfb --- /dev/null +++ b/encoders/data/data-tracker-pos-chunk.js @@ -0,0 +1,9 @@ +const chunk = require('../chunk'); + +module.exports = (x, y, z) => { + const buf = Buffer.alloc(12); + buf.writeFloatLE(x); + buf.writeFloatLE(y, 4); + buf.writeFloatLE(z, 8); + return chunk(0x0000, buf, false); +}; diff --git a/encoders/data/data-tracker-speed-chunk.js b/encoders/data/data-tracker-speed-chunk.js new file mode 100644 index 0000000..659fded --- /dev/null +++ b/encoders/data/data-tracker-speed-chunk.js @@ -0,0 +1,9 @@ +const chunk = require('../chunk'); + +module.exports = (x, y, z) => { + const buf = Buffer.alloc(12); + buf.writeFloatLE(x); + buf.writeFloatLE(y, 4); + buf.writeFloatLE(z, 8); + return chunk(0x0001, buf, false); +}; diff --git a/encoders/data/data-tracker-status-chunk.js b/encoders/data/data-tracker-status-chunk.js new file mode 100644 index 0000000..5c7ca46 --- /dev/null +++ b/encoders/data/data-tracker-status-chunk.js @@ -0,0 +1,7 @@ +const chunk = require('../chunk'); + +module.exports = (validity) => { + const buf = Buffer.alloc(4); + buf.writeFloatLE(validity); + return chunk(0x0003, buf, false); +}; diff --git a/encoders/data/data-tracker-timestamp-chunk.js b/encoders/data/data-tracker-timestamp-chunk.js new file mode 100644 index 0000000..13d340c --- /dev/null +++ b/encoders/data/data-tracker-timestamp-chunk.js @@ -0,0 +1,10 @@ +const chunk = require('../chunk'); + +module.exports = (timestamp) => { + const buf = Buffer.alloc(8); + const timestampHigh = timestamp.toString(2).padStart(64, '0').substring(0, 32); + const timestampLow = timestamp.toString(2).padStart(64, '0').substring(32); + buf.writeUInt32LE(parseInt(timestampLow, 2)); + buf.writeUInt32LE(parseInt(timestampHigh, 2), 4); + return chunk(0x0006, buf, false); +}; diff --git a/encoders/data/data-tracker-trgtpos-chunk.js b/encoders/data/data-tracker-trgtpos-chunk.js new file mode 100644 index 0000000..b71765e --- /dev/null +++ b/encoders/data/data-tracker-trgtpos-chunk.js @@ -0,0 +1,9 @@ +const chunk = require('../chunk'); + +module.exports = (x, y, z) => { + const buf = Buffer.alloc(12); + buf.writeFloatLE(x); + buf.writeFloatLE(y, 4); + buf.writeFloatLE(z, 8); + return chunk(0x0005, buf, false); +}; diff --git a/encoders/info/info-chunk.js b/encoders/info/info-chunk.js new file mode 100644 index 0000000..34e32ca --- /dev/null +++ b/encoders/info/info-chunk.js @@ -0,0 +1,4 @@ +const chunk = require('../chunk'); + +module.exports = (packetHeaderChunk, systemNameChunk, trackerListChunk) => + chunk(0x6756, Buffer.concat([packetHeaderChunk, systemNameChunk, trackerListChunk]), true); diff --git a/encoders/info/info-system-name-chunk.js b/encoders/info/info-system-name-chunk.js new file mode 100644 index 0000000..f41f165 --- /dev/null +++ b/encoders/info/info-system-name-chunk.js @@ -0,0 +1,3 @@ +const chunk = require('../chunk'); + +module.exports = (systemName) => chunk(0x0001, Buffer.from(systemName), false); diff --git a/encoders/info/info-tracker-chunk.js b/encoders/info/info-tracker-chunk.js new file mode 100644 index 0000000..ef9ff8f --- /dev/null +++ b/encoders/info/info-tracker-chunk.js @@ -0,0 +1,3 @@ +const chunk = require('../chunk'); + +module.exports = (trackerId, trackerNameChunk) => chunk(trackerId, trackerNameChunk, true); diff --git a/encoders/info/info-tracker-list-chunk.js b/encoders/info/info-tracker-list-chunk.js new file mode 100644 index 0000000..a13b1e5 --- /dev/null +++ b/encoders/info/info-tracker-list-chunk.js @@ -0,0 +1,3 @@ +const chunk = require('../chunk'); + +module.exports = (trackerChunks) => chunk(0x0002, Buffer.concat(trackerChunks), true); diff --git a/encoders/info/info-tracker-name-chunk.js b/encoders/info/info-tracker-name-chunk.js new file mode 100644 index 0000000..78a1c32 --- /dev/null +++ b/encoders/info/info-tracker-name-chunk.js @@ -0,0 +1,3 @@ +const chunk = require('../chunk'); + +module.exports = (trackerName) => chunk(0x0000, Buffer.from(trackerName), false); diff --git a/encoders/packet-header-chunk.js b/encoders/packet-header-chunk.js new file mode 100644 index 0000000..16429ab --- /dev/null +++ b/encoders/packet-header-chunk.js @@ -0,0 +1,15 @@ +const chunk = require('./chunk'); + +module.exports = (timestamp, versionHigh, versionLow, frameId, framePacketCount) => { + const packetHeader = Buffer.alloc(12); + const timestampHigh = timestamp.toString(2).padStart(64, '0').substring(0, 32); + const timestampLow = timestamp.toString(2).padStart(64, '0').substring(32); + packetHeader.writeUInt32LE(parseInt(timestampLow, 2)); + packetHeader.writeUInt32LE(parseInt(timestampHigh, 2), 4); + packetHeader.writeUint8(versionHigh, 8); + packetHeader.writeUint8(versionLow, 9); + packetHeader.writeUint8(frameId, 10); + packetHeader.writeUint8(framePacketCount, 11); + + return chunk(0x0000, packetHeader, false); +}; diff --git a/example.js b/example.js deleted file mode 100644 index 91cd4ad..0000000 --- a/example.js +++ /dev/null @@ -1,16 +0,0 @@ -const dgram = require('dgram'); -const { Decoder } = require('.'); - -const client = dgram.createSocket('udp4'); - -const psnDecoder = new Decoder(); - -client.on('listening', () => { - client.addMembership('236.10.10.10'); -}); - -client.on('message', (message) => { - psnDecoder.decode(message); -}); - -client.bind(56565, '0.0.0.0'); diff --git a/examples/psn_client.js b/examples/psn_client.js new file mode 100644 index 0000000..e592be3 --- /dev/null +++ b/examples/psn_client.js @@ -0,0 +1,58 @@ +const dgram = require('dgram'); +const Decoder = require('../decoder'); + +const client = dgram.createSocket('udp4'); +const decoder = new Decoder(); + +client.on('listening', () => { + client.addMembership('236.10.10.10'); +}); + +client.on('message', (buffer) => { + decoder.decode(buffer); +}); +client.bind(56565, '0.0.0.0'); + +setInterval(() => { + if (decoder.info.system_name) { + console.log(`System Name: ${decoder.info.system_name}`); + } + if (Object.keys(decoder.info.trackers).length > 0) { + console.log(`Tracker Count: ${Object.keys(decoder.info.trackers).length}`); + } + + Object.entries(decoder.data.trackers).forEach(([trackerId, tracker]) => { + console.log( + `Tracker - id: ${trackerId} | name: ${ + decoder.info.trackers[trackerId]?.name ? decoder.info.trackers[trackerId]?.name : '' + }` + ); + if (tracker.pos_x !== undefined && tracker.pos_y !== undefined && tracker.pos_z !== undefined) { + console.log(`\tpos: ${tracker.pos_x}, ${tracker.pos_y}, ${tracker.pos_z}`); + } + + if (tracker.speed_x !== undefined && tracker.speed_y !== undefined && tracker.speed_z !== undefined) { + console.log(`\tspeed: ${tracker.speed_x}, ${tracker.speed_y}, ${tracker.speed_z}`); + } + + if (tracker.ori_x !== undefined && tracker.ori_y !== undefined && tracker.ori_z !== undefined) { + console.log(`\tori: ${tracker.ori_x}, ${tracker.ori_y}, ${tracker.ori_z}`); + } + + if (tracker.validity !== undefined) { + console.log(`\tstatus: ${tracker.validity}`); + } + + if (tracker.accel_x !== undefined && tracker.accel_y !== undefined && tracker.accel_z !== undefined) { + console.log(`\taccel: ${tracker.accel_x}, ${tracker.accel_y}, ${tracker.accel_z}`); + } + + if (tracker.trgtpos_x !== undefined && tracker.trgtpos_y !== undefined && tracker.trgtpos_z !== undefined) { + console.log(`\ttrgtpos: ${tracker.trgtpos_x}, ${tracker.trgtpos_y}, ${tracker.trgtpos_z}`); + } + + if (tracker.tracker_timestamp !== undefined) { + console.log(`\ttimestamp: ${tracker.tracker_timestamp}`); + } + }); +}, 1000); diff --git a/examples/psn_server.js b/examples/psn_server.js new file mode 100644 index 0000000..44c85b9 --- /dev/null +++ b/examples/psn_server.js @@ -0,0 +1,55 @@ +const dgram = require('dgram'); +const Encoder = require('../encoder'); +const Tracker = require('../models/Tracker'); + +const client = dgram.createSocket('udp4'); +const encoder = new Encoder('Test PSN Server', 2, 0); + +const trackers = []; + +trackers.push(new Tracker(0, 'Sun')); +trackers.push(new Tracker(1, 'Mercury')); +trackers.push(new Tracker(2, 'Venus')); +trackers.push(new Tracker(3, 'Earth')); +trackers.push(new Tracker(4, 'Mars')); +trackers.push(new Tracker(5, 'Jupiter')); +trackers.push(new Tracker(6, 'Saturn')); +trackers.push(new Tracker(7, 'Uranus')); +trackers.push(new Tracker(8, 'Neptune')); +trackers.push(new Tracker(9, 'Pluto')); + +const orbits = [1.0, 88.0, 224.7, 365.2, 687, 4332, 10760, 30700, 60200, 90600]; +const distFromSun = [0, 0.58, 1.08, 1.5, 2.28, 7.78, 14.29, 28.71, 45.04, 59.13]; + +let timestamp = 0; +setInterval(() => { + for (let index = 0; index < trackers.length; index += 1) { + const a = 1.0 / orbits[index]; + const b = distFromSun[index]; + const x = timestamp; + const cb = Math.cos(a * x) * b; + const sb = Math.sin(a * x) * b; + + trackers[index].setPos(sb, 0, cb); + trackers[index].setSpeed(a * cb, 0, -a * sb); + trackers[index].setOri(0, x / 1000.0, 0); + trackers[index].setAccel(-a * a * sb, 0, -a * a * cb); + trackers[index].setTrgtPos(3, 14, 16); + trackers[index].setStatus(index / 10.0); + trackers[index].setTimestamp(timestamp); + } + + const dataPackets = encoder.getDataPackets(timestamp, trackers); + + dataPackets.forEach((packet) => { + client.send(packet, 56565, '236.10.10.10'); + }); + timestamp += 1; +}, 20); + +setInterval(() => { + const infoPackets = encoder.getInfoPackets(timestamp, trackers); + infoPackets.forEach((packet) => { + client.send(packet, 56565, '236.10.10.10'); + }); +}, 1000); diff --git a/index.js b/index.js index 7f8ffe3..ca06aff 100644 --- a/index.js +++ b/index.js @@ -1,143 +1,7 @@ -const PacketParser = require('./parsers/index.js'); -const InfoPacket = require('./models/InfoPacket.js'); -const DataPacket = require('./models/DataPacket.js'); - -const DATA_PACKET = 0x6755; -const INFO_PACKET = 0x6756; - -class Decoder { - lastInfoPacketHeader; - - lastDataPacketHeader; - - infoPacketFrames = {}; - - dataPacketFrames = {}; - - constructor() { - this.info = { - trackers: {}, - }; - - this.data = { - trackers: {}, - }; - } - - updateInfo(framePackets) { - // console.log('info update'); - framePackets.forEach((packet) => { - packet.subChunks?.forEach((subChunk) => { - if (subChunk.id === 0x0001) { - // NOTE(jwetzell): system name subChunk - this.info.system_name = subChunk.system_name; - } else if (subChunk.id === 0x0002) { - subChunk.trackers?.forEach((tracker) => { - if (this.info.trackers[tracker.id] === undefined) { - this.info.trackers[tracker.id] = {}; - } - this.info.trackers[tracker.id].name = tracker.tracker_name.tracker_name; - }); - } - }); - }); - } - - updateData(framePackets) { - // console.log('data update'); - framePackets.forEach((packet) => { - packet.subChunks?.forEach((subChunk) => { - if (subChunk.id === 0x0001) { - subChunk.trackers?.forEach((tracker) => { - if (this.data.trackers[tracker.id] === undefined) { - this.data.trackers[tracker.id] = {}; - } - tracker.fields?.forEach((field) => { - Object.entries(field).forEach(([key, value]) => { - this.data.trackers[tracker.id][key] = value; - }); - }); - }); - } - }); - }); - } - - // TODO(jwetzell): support multiple frame packets this might require some rework of how chunks are assumed - decode(packetBuf) { - const packet = PacketParser(packetBuf); - - if (packet.id === INFO_PACKET) { - const infoPacket = new InfoPacket(packet); - if (infoPacket) { - if (infoPacket.subChunks?.length > 0) { - const currentInfoPacketHeader = infoPacket.getHeaderPacket(); - if (!currentInfoPacketHeader) { - // NOTE(jwetzell): not sure that info packets without a header subchunk are valid? - return; - } - - const systemSubChunk = infoPacket.getSystemPacket(); - if (!systemSubChunk) { - // NOTE(jwetzell): not sure that info packets without a system subchunk are valid? - return; - } - - if (this.infoPacketFrames[currentInfoPacketHeader.frame_id] === undefined) { - this.infoPacketFrames[currentInfoPacketHeader.frame_id] = []; - } - this.infoPacketFrames[currentInfoPacketHeader.frame_id].push(infoPacket); - - if ( - this.infoPacketFrames[currentInfoPacketHeader.frame_id].length === - currentInfoPacketHeader.frame_packet_count - ) { - // console.log('found complete info frame'); - this.updateInfo(this.infoPacketFrames[currentInfoPacketHeader.frame_id]); - delete this.infoPacketFrames[currentInfoPacketHeader.frame_id]; - } else { - // TODO(jwetzell): need to compute whether a frame is complete - // console.log('compute frame completion'); - } - - this.lastInfoPacketHeader = currentInfoPacketHeader; - } - } - } else if (packet.id === DATA_PACKET) { - const dataPacket = new DataPacket(packet); - if (dataPacket) { - if (dataPacket.subChunks?.length > 0) { - const currentDataPacketHeader = dataPacket.getHeaderPacket(); - if (!currentDataPacketHeader) { - // NOTE(jwetzell): not sure that info packets without a header subchunk are valid? - console.error('data packet with no header'); - return; - } - - if (this.dataPacketFrames[currentDataPacketHeader.frame_id] === undefined) { - this.dataPacketFrames[currentDataPacketHeader.frame_id] = []; - } - this.dataPacketFrames[currentDataPacketHeader.frame_id].push(dataPacket); - - if ( - this.dataPacketFrames[currentDataPacketHeader.frame_id].length === - currentDataPacketHeader.frame_packet_count - ) { - // console.log('found complete data frame'); - this.updateData(this.dataPacketFrames[currentDataPacketHeader.frame_id]); - delete this.dataPacketFrames[currentDataPacketHeader.frame_id]; - } else { - // TODO(jwetzell): need to compute whether a frame is complete - // console.log('compute data frame completion'); - } - - this.lastDataPacketHeader = currentDataPacketHeader; - } - } - } - } -} +const Decoder = require('./decoder.js'); +const Encoder = require('./encoder.js'); module.exports = { + Encoder, Decoder, }; diff --git a/models/Tracker.js b/models/Tracker.js new file mode 100644 index 0000000..3f4323d --- /dev/null +++ b/models/Tracker.js @@ -0,0 +1,92 @@ +const dataTrackerAccelChunk = require('../encoders/data/data-tracker-accel-chunk'); +const dataTrackerChunk = require('../encoders/data/data-tracker-chunk'); +const dataTrackerOriChunk = require('../encoders/data/data-tracker-ori-chunk'); +const dataTrackerPosChunk = require('../encoders/data/data-tracker-pos-chunk'); +const dataTrackerSpeedChunk = require('../encoders/data/data-tracker-speed-chunk'); +const dataTrackerStatusChunk = require('../encoders/data/data-tracker-status-chunk'); +const dataTrackerTimestampChunk = require('../encoders/data/data-tracker-timestamp-chunk'); +const dataTrackerTrgtposChunk = require('../encoders/data/data-tracker-trgtpos-chunk'); +const infoTrackerChunk = require('../encoders/info/info-tracker-chunk'); +const infoTrackerNameChunk = require('../encoders/info/info-tracker-name-chunk'); + +class Tracker { + constructor(id, name) { + this.id = id; + this.name = name; + this.pos = undefined; + this.speed = undefined; + this.ori = undefined; + this.validity = undefined; + this.accel = undefined; + this.trgtpos = undefined; + this.timestamp = undefined; + } + + setPos(x, y, z) { + this.pos = [x, y, z]; + } + + setSpeed(x, y, z) { + this.speed = [x, y, z]; + } + + setOri(x, y, z) { + this.ori = [x, y, z]; + } + + setStatus(validity) { + this.validity = validity; + } + + setAccel(x, y, z) { + this.accel = [x, y, z]; + } + + setTrgtPos(x, y, z) { + this.trgtpos = [x, y, z]; + } + + setTimestamp(timestamp) { + this.timestamp = timestamp; + } + + getDataChunk() { + const fieldChunks = []; + + if (this.pos) { + fieldChunks.push(dataTrackerPosChunk(...this.pos)); + } + + if (this.speed) { + fieldChunks.push(dataTrackerSpeedChunk(...this.speed)); + } + + if (this.ori) { + fieldChunks.push(dataTrackerOriChunk(...this.ori)); + } + + if (this.validity) { + fieldChunks.push(dataTrackerStatusChunk(this.validity)); + } + + if (this.accel) { + fieldChunks.push(dataTrackerAccelChunk(...this.accel)); + } + + if (this.trgtpos) { + fieldChunks.push(dataTrackerTrgtposChunk(...this.trgtpos)); + } + + if (this.timestamp) { + fieldChunks.push(dataTrackerTimestampChunk(this.timestamp)); + } + + return dataTrackerChunk(this.id, fieldChunks); + } + + getInfoChunk() { + return infoTrackerChunk(this.id, infoTrackerNameChunk(this.name)); + } +} + +module.exports = Tracker; diff --git a/models/DataPacket.js b/models/packets/data-packet.js similarity index 83% rename from models/DataPacket.js rename to models/packets/data-packet.js index 32398b0..24f6fe7 100644 --- a/models/DataPacket.js +++ b/models/packets/data-packet.js @@ -1,16 +1,16 @@ /* eslint-disable no-case-declarations */ /* eslint-disable no-param-reassign */ const { Parser } = require('binary-parser'); -const { ChunkParser, HeaderParser } = require('../parsers/common'); -const { DataTrackerListParser, DataTrackerFieldParser } = require('../parsers/data'); +const Decoders = require('../../decoders'); class DataPacket { constructor(packet) { this.packet = packet; this.subChunks = []; + // console.log(this.packet); if (this.packet.has_subchunks) { this.subChunkParser = new Parser().array('chunks', { - type: ChunkParser, + type: Decoders.Chunk, lengthInBytes: this.packet.data_len, }); this.subChunks = this.subChunkParser.parse(this.packet.chunk_data)?.chunks; @@ -21,16 +21,16 @@ class DataPacket { case 0: populatedSubChunk = { ...subChunk, - ...HeaderParser.parse(subChunk.chunk_data), + ...Decoders.PacketHeaderChunk.parse(subChunk.chunk_data), }; break; case 1: - const dataTrackerList = DataTrackerListParser.parse(subChunk.chunk_data); + const dataTrackerList = Decoders.Data.TrackerListChunk.parse(subChunk.chunk_data); dataTrackerList.trackers?.forEach((tracker) => { if (tracker.data && tracker.data_len > 0) { const fields = new Parser() .array('fields', { - type: DataTrackerFieldParser, + type: Decoders.Data.TrackerFieldChunk, lengthInBytes: tracker.data_len, }) .parse(tracker.data); diff --git a/models/packets/index.js b/models/packets/index.js new file mode 100644 index 0000000..3786239 --- /dev/null +++ b/models/packets/index.js @@ -0,0 +1,7 @@ +const DataPacket = require('./data-packet'); +const InfoPacket = require('./info-packet'); + +module.exports = { + DataPacket, + InfoPacket, +}; diff --git a/models/InfoPacket.js b/models/packets/info-packet.js similarity index 78% rename from models/InfoPacket.js rename to models/packets/info-packet.js index 5595d6d..b9ff214 100644 --- a/models/InfoPacket.js +++ b/models/packets/info-packet.js @@ -1,6 +1,5 @@ const { Parser } = require('binary-parser'); -const { ChunkParser, HeaderParser } = require('../parsers/common'); -const { InfoSystemNameParser, InfoTrackerListParser } = require('../parsers/info'); +const Decoders = require('../../decoders'); class InfoPacket { constructor(packet) { @@ -8,7 +7,7 @@ class InfoPacket { this.subChunks = []; if (this.packet.has_subchunks) { this.subChunkParser = new Parser().array('chunks', { - type: ChunkParser, + type: Decoders.Chunk, lengthInBytes: this.packet.data_len, }); this.subChunks = this.subChunkParser.parse(this.packet.chunk_data)?.chunks; @@ -19,19 +18,19 @@ class InfoPacket { case 0: populatedSubChunk = { ...subChunk, - ...HeaderParser.parse(subChunk.chunk_data), + ...Decoders.PacketHeaderChunk.parse(subChunk.chunk_data), }; break; case 1: populatedSubChunk = { ...subChunk, - ...InfoSystemNameParser.parse(subChunk.chunk_data), + ...Decoders.Info.SystemNameChunk.parse(subChunk.chunk_data), }; break; case 2: populatedSubChunk = { ...subChunk, - ...InfoTrackerListParser.parse(subChunk.chunk_data), + ...Decoders.Info.TrackerListChunk.parse(subChunk.chunk_data), }; break; default: