initial commit

packages/free-d/src/decode.ts

@@ -0,0 +1,55 @@
+import { FreeDPosition, FreeDMessageType } from './types';
+import { checksum, freeDUnitsToPosition, freeDUnitsToRotation } from './utils';
+
+function bytesToPositionMessage(bytes: Uint8Array): FreeDPosition {
+  const id = bytes[1];
+  const pan = freeDUnitsToRotation(bytes[2], bytes[3], bytes[4]);
+  const tilt = freeDUnitsToRotation(bytes[5], bytes[6], bytes[7]);
+  const roll = freeDUnitsToRotation(bytes[8], bytes[9], bytes[10]);
+
+  const posX = freeDUnitsToPosition(bytes[11], bytes[12], bytes[13]);
+  const posY = freeDUnitsToPosition(bytes[14], bytes[15], bytes[16]);
+  const posZ = freeDUnitsToPosition(bytes[17], bytes[18], bytes[19]);
+
+  const zoom = bytes[20] * 65536 + bytes[21] * 256 + bytes[22];
+  const focus = bytes[23] * 65536 + bytes[24] * 256 + bytes[25];
+
+  const spare = bytes[26] * 256 + bytes[27];
+
+  return {
+    type: FreeDMessageType.Position,
+    id,
+    pan,
+    tilt,
+    roll,
+    posX,
+    posY,
+    posZ,
+    zoom,
+    focus,
+    spare,
+  };
+}
+
+export function decode(bytes: Uint8Array): FreeDPosition | undefined {
+  if (bytes.length === 0) {
+    throw new Error('cannot decode 0 bytes');
+  }
+
+  const messageType = bytes[0];
+
+  switch (messageType) {
+    case FreeDMessageType.Position:
+      if (bytes.length !== 29) {
+        throw new Error('freeD position message should be 29 bytes long');
+      }
+
+      if (checksum(bytes.subarray(0, bytes.length - 1)) !== bytes[bytes.length - 1]) {
+        throw new Error('freeD message failed checksum');
+      }
+
+      return bytesToPositionMessage(bytes);
+    default:
+      throw new Error('unsupported freeD message type');
+  }
+}

packages/free-d/src/encode.ts

@@ -0,0 +1,41 @@
+import { FreeDPosition, FreeDMessageType } from './types';
+import { checksum, positionToFreeDUnits, rotationToFreeDUnits } from './utils';
+
+function positionMessageToBytes(message: FreeDPosition): Uint8Array {
+  const bytes = new Uint8Array(29);
+
+  bytes[0] = FreeDMessageType.Position;
+  bytes[1] = message.id;
+
+  bytes.set(rotationToFreeDUnits(message.pan), 2);
+  bytes.set(rotationToFreeDUnits(message.tilt), 5);
+  bytes.set(rotationToFreeDUnits(message.roll), 8);
+
+  bytes.set(positionToFreeDUnits(message.posX), 11);
+  bytes.set(positionToFreeDUnits(message.posY), 14);
+  bytes.set(positionToFreeDUnits(message.posZ), 17);
+
+  bytes[20] = Math.trunc(message.zoom / 65536);
+  bytes[21] = Math.trunc((message.zoom / 256) % 256);
+  bytes[22] = message.zoom % 256;
+
+  bytes[23] = Math.trunc(message.focus / 65536);
+  bytes[24] = Math.trunc((message.focus / 256) % 256);
+  bytes[25] = message.focus % 256;
+
+  if (message.spare !== undefined) {
+    bytes[26] = message.spare >> 8;
+    bytes[27] = message.spare % 256;
+  }
+
+  bytes[28] = checksum(bytes.subarray(0, 28));
+  return bytes;
+}
+export function encode(message: FreeDPosition): Uint8Array {
+  switch (message.type) {
+    case FreeDMessageType.Position:
+      return positionMessageToBytes(message);
+    default:
+      throw new Error('unsupported freeD message type');
+  }
+}

packages/free-d/src/index.ts

@@ -0,0 +1,3 @@
+export * from './decode';
+export * from './encode';
+export * from './types';

packages/free-d/src/types.ts

@@ -0,0 +1,17 @@
+export enum FreeDMessageType {
+  Position = 0xd1
+}
+
+export type FreeDPosition = {
+  type: FreeDMessageType.Position;
+  id: number;
+  pan: number;
+  tilt: number;
+  roll: number;
+  posZ: number;
+  posX: number;
+  posY: number;
+  zoom: number;
+  focus: number;
+  spare?: number;
+};

packages/free-d/src/utils.ts

@@ -0,0 +1,37 @@
+export function checksum(bytes: Uint8Array): number {
+  let checksum = 0x40;
+
+  bytes.forEach((byte) => {
+    checksum -= byte;
+    if (checksum < 0) {
+      checksum += 256;
+    }
+  });
+  return checksum;
+}
+
+export function rotationToFreeDUnits(rotation: number): Uint8Array {
+  const view = new DataView(new ArrayBuffer(4));
+  const units = rotation * 32768 * 256;
+  view.setInt32(0, units);
+  return new Uint8Array(view.buffer, 0, 3);
+}
+
+export function positionToFreeDUnits(position: number): Uint8Array {
+  const view = new DataView(new ArrayBuffer(4));
+  const units = position * 64 * 256;
+  view.setInt32(0, units);
+  return new Uint8Array(view.buffer, 0, 3);
+}
+
+export function freeDUnitsToRotation(upper: number, middle: number, lower: number): number {
+  const padded = new Uint8Array([upper, middle, lower, 0]);
+  const view = new DataView(padded.buffer, padded.byteOffset, padded.byteLength);
+  return view.getInt32(0) / 256 / 32768;
+}
+
+export function freeDUnitsToPosition(upper: number, middle: number, lower: number): number {
+  const padded = new Uint8Array([upper, middle, lower, 0]);
+  const view = new DataView(padded.buffer, padded.byteOffset, padded.byteLength);
+  return view.getInt32(0) / 256 / 64;
+}