From 7564bcf6a9896656e023d6cf6081396a0889d5f9 Mon Sep 17 00:00:00 2001 From: Joel Wetzell Date: Sat, 15 Mar 2025 08:41:19 -0500 Subject: [PATCH] add decoder for accel velocity submodules --- src/decoders/index.ts | 4 ++ .../motion/centroid-accel-velocity.ts | 44 +++++++++++++++++ .../motion/tracked-point-accel-velocity.ts | 47 +++++++++++++++++++ src/models.ts | 28 +++++++++++ tests/decode.test.js | 45 ++++++++++++++++++ 5 files changed, 168 insertions(+) create mode 100644 src/decoders/motion/centroid-accel-velocity.ts create mode 100644 src/decoders/motion/tracked-point-accel-velocity.ts diff --git a/src/decoders/index.ts b/src/decoders/index.ts index fba0161..90416bf 100644 --- a/src/decoders/index.ts +++ b/src/decoders/index.ts @@ -1,13 +1,17 @@ +import CentroidAccelVelocity from './motion/centroid-accel-velocity'; import CentroidPosition from './motion/centroid-position'; import OrientationEuler from './motion/orientation-euler'; import OrientationQuaternion from './motion/orientation-quaternion'; +import TrackedPointAccelVelocity from './motion/tracked-point-accel-velocity'; import TrackedPointPosition from './motion/tracked-point-position'; import RTTrPHeader from './rttrp'; export const Decoders = { RTTrPHeader, CentroidPosition, + CentroidAccelVelocity, TrackedPointPosition, + TrackedPointAccelVelocity, OrientationQuaternion, OrientationEuler, }; diff --git a/src/decoders/motion/centroid-accel-velocity.ts b/src/decoders/motion/centroid-accel-velocity.ts new file mode 100644 index 0000000..654a252 --- /dev/null +++ b/src/decoders/motion/centroid-accel-velocity.ts @@ -0,0 +1,44 @@ +import { CentroidAccelVelocity } from '../../models'; + +export default (bytes: Uint8Array, isLittleEndian: boolean = false): CentroidAccelVelocity => { + if (bytes.length !== 51) { + throw new Error('Centroid Acceleration and Velocity module must be 51 bytes'); + } + + const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength); + + const type = view.getUint8(0); + if (type !== 0x20) { + throw new Error('Centroid Acceleration and Velocity module must have a type of 0x20'); + } + + const size = view.getUint16(1, isLittleEndian); + if (size !== 51) { + throw new Error('Centroid Acceleration and Velocity module must be 51 bytes'); + } + const x = view.getFloat64(3, isLittleEndian); + const y = view.getFloat64(11, isLittleEndian); + const z = view.getFloat64(19, isLittleEndian); + + const accelX = view.getFloat32(27, isLittleEndian); + const accelY = view.getFloat32(31, isLittleEndian); + const accelZ = view.getFloat32(35, isLittleEndian); + + const velocityX = view.getFloat32(39, isLittleEndian); + const velocityY = view.getFloat32(43, isLittleEndian); + const velocityZ = view.getFloat32(47, isLittleEndian); + + return { + type, + size, + x, + y, + z, + accelX, + accelY, + accelZ, + velocityX, + velocityY, + velocityZ, + }; +}; diff --git a/src/decoders/motion/tracked-point-accel-velocity.ts b/src/decoders/motion/tracked-point-accel-velocity.ts new file mode 100644 index 0000000..a64a857 --- /dev/null +++ b/src/decoders/motion/tracked-point-accel-velocity.ts @@ -0,0 +1,47 @@ +import { TrackedPointAccelVelocity } from '../../models'; + +export default (bytes: Uint8Array, isLittleEndian: boolean = false): TrackedPointAccelVelocity => { + if (bytes.length !== 52) { + throw new Error('Tracked Point Acceleration and Velocity module must be 52 bytes'); + } + + const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength); + + const type = view.getUint8(0); + if (type !== 0x21) { + throw new Error('Tracked Point Acceleration and Velocity module must have a type of 0x21'); + } + + const size = view.getUint16(1, isLittleEndian); + if (size !== 52) { + throw new Error('Tracked Point Acceleration and Velocity module must be 52 bytes'); + } + const x = view.getFloat64(3, isLittleEndian); + const y = view.getFloat64(11, isLittleEndian); + const z = view.getFloat64(19, isLittleEndian); + + const accelX = view.getFloat32(27, isLittleEndian); + const accelY = view.getFloat32(31, isLittleEndian); + const accelZ = view.getFloat32(35, isLittleEndian); + + const velocityX = view.getFloat32(39, isLittleEndian); + const velocityY = view.getFloat32(43, isLittleEndian); + const velocityZ = view.getFloat32(47, isLittleEndian); + + const index = view.getUint8(51); + + return { + type, + size, + x, + y, + z, + accelX, + accelY, + accelZ, + velocityX, + velocityY, + velocityZ, + index, + }; +}; diff --git a/src/models.ts b/src/models.ts index e19abdb..7cccc4e 100644 --- a/src/models.ts +++ b/src/models.ts @@ -18,6 +18,19 @@ export type CentroidPosition = { z: number; }; +export type CentroidAccelVelocity = { + type: number; + size: number; + x: number; + y: number; + z: number; + accelX: number; + accelY: number; + accelZ: number; + velocityX: number; + velocityY: number; + velocityZ: number; +}; export type TrackedPointPosition = { type: number; @@ -29,6 +42,21 @@ export type TrackedPointPosition = { index: number; }; +export type TrackedPointAccelVelocity = { + type: number; + size: number; + x: number; + y: number; + z: number; + accelX: number; + accelY: number; + accelZ: number; + velocityX: number; + velocityY: number; + velocityZ: number; + index: number; +}; + export type OrientationQuaternion = { type: number; size: number; diff --git a/tests/decode.test.js b/tests/decode.test.js index 4fc98a3..ebb4f95 100644 --- a/tests/decode.test.js +++ b/tests/decode.test.js @@ -86,6 +86,51 @@ const goodTests = [ }, decoder: Decoders.OrientationEuler, }, + { + description: 'Centroid Acceleration and Velocity', + bytes: new Uint8Array([ + 0x20, 0x00, 51, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x80, 0x00, 0x00, 0x40, 0xa0, 0x00, 0x00, 0x40, 0xc0, 0x00, + 0x00, 0x40, 0xe0, 0x00, 0x00, 0x41, 0x00, 0x00, 0x00, 0x41, 0x10, 0x00, 0x00, + ]), + expected: { + type: 0x20, + size: 51, + x: 1.0, + y: 2.0, + z: 3.0, + accelX: 4.0, + accelY: 5.0, + accelZ: 6.0, + velocityX: 7.0, + velocityY: 8.0, + velocityZ: 9.0, + }, + decoder: Decoders.CentroidAccelVelocity, + }, + { + description: 'Tracked Point Acceleration and Velocity', + bytes: new Uint8Array([ + 0x21, 0x00, 52, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x80, 0x00, 0x00, 0x40, 0xa0, 0x00, 0x00, 0x40, 0xc0, 0x00, + 0x00, 0x40, 0xe0, 0x00, 0x00, 0x41, 0x00, 0x00, 0x00, 0x41, 0x10, 0x00, 0x00, 0x01, + ]), + expected: { + type: 0x21, + size: 52, + x: 1.0, + y: 2.0, + z: 3.0, + accelX: 4.0, + accelY: 5.0, + accelZ: 6.0, + velocityX: 7.0, + velocityY: 8.0, + velocityZ: 9.0, + index: 1, + }, + decoder: Decoders.TrackedPointAccelVelocity, + }, ]; describe('RTTrP Bytes Decoding', () => {