@robotical/roboticaljs
Version:
Javascript/TS library for Robotical products
350 lines (282 loc) • 14.4 kB
text/typescript
// import { RaftLog } from "@robotical/raftjs";
import { RaftDeviceManager } from '@robotical/raftjs';
import { TextDecoder } from 'text-encoding';
// export interface IMUStateInfo {
// gx: number;
// gy: number;
// gz: number;
// ax: number;
// ay: number;
// az: number;
// tsMs: number;
// }
// export interface LightStateInfo {
// irVals: Array<number>;
// ambientVals: Array<number>;
// tsMs: number;
// }
// export interface PowerStateInfo {
// battV: number;
// tsMs: number;
// }
// export interface DeviceLastTsState {
// lastMs: number;
// offsetMs: number;
// }
// export interface DeviceMsgJsonElem {
// _t: string; // Device type
// _o?: string; // Device online (1 = online, 0 = offline)
// x: string; // Hex encoded message values
// }
// export interface DeviceMsgJsonBus {
// [devAddr: string]: DeviceMsgJsonElem;
// }
// export interface DeviceMsgJson {
// [busName: string]: DeviceMsgJsonBus;
// }
// export class CogStateInfo {
// LSM6DS: IMUStateInfo = { gx: 0, gy: 0, gz: 0, ax: 0, ay: 0, az: 0, tsMs: 0 };
// Light: LightStateInfo = { irVals: [], ambientVals: [], tsMs: 0 };
// Power: PowerStateInfo = { battV: 0, tsMs: 0 };
// // Last timestamp for each device
// private _deviceLastTs: { [devName: string]: DeviceLastTsState } = {};
// updateFromMsg(rxMsg: Uint8Array, _frameTimeMs: number): Array<string> {
// // console.log("_frameTimeMs: ", _frameTimeMs);
// // RaftLog.info(`CogStateInfo: updateFromMsg: rxMsg: ${rxMsg} frameTimeMs: ${frameTimeMs}`);
// // Convert Uint8Array to string
// const decoder = new TextDecoder('utf-8');
// const jsonString = decoder.decode(rxMsg.slice(2));
// // Debug
// // RaftLog.info(`CogStateInfo: updateFromMsg: jsonString: ${jsonString}`);
// // Parse JSON string to TypeScript object
// let deviceMsg: DeviceMsgJson;
// try {
// deviceMsg = JSON.parse(jsonString);
// } catch (error) {
// RaftLog.warn(`CogStateInfo: updateFromMsg: JSON parse error: ${error}`);
// return [];
// }
// // Debug
// // for (const busName in deviceMsg) {
// // for (const devAddr in deviceMsg[busName]) {
// // for (const attrGroupName in deviceMsg[busName][devAddr]) {
// // RaftLog.info(`CogStateInfo: updateFromMsg: busName ${busName} devAddr ${devAddr} attrGroupName: ${attrGroupName}`);
// // RaftLog.info(`CogStateInfo: updateFromMsg: attrValue: ${deviceMsg[busName][devAddr][attrGroupName]}`);
// // }
// // }
// // }
// // Iterate over values and extract
// for (const busName in deviceMsg) {
// for (const devAddr in deviceMsg[busName]) {
// // Validate
// if ((deviceMsg[busName][devAddr].x === undefined) || (deviceMsg[busName][devAddr]._t === undefined)) {
// RaftLog.info(`CogStateInfo: updateFromMsg: Invalid message`);
// continue;
// }
// // Extract message and device type
// const msgHex = deviceMsg[busName][devAddr].x;
// const devType = deviceMsg[busName][devAddr]._t;
// // Check if message is empty
// if (msgHex === '') {
// RaftLog.info(`CogStateInfo: updateFromMsg: Empty message`);
// continue;
// }
// // Convert the hex string to an arraybuffer by converting each pair of hex chars to a byte
// const msgBytes = this.hexToBytes(msgHex);
// // Create an ArrayBuffer from the Uint8Array
// const arrayBuffer = new Uint8Array(msgBytes).buffer;
// const msgBuffer = new DataView(arrayBuffer);
// // Extract timestamp
// let timestamp = msgBuffer.getUint16(0);
// if (this._deviceLastTs[devType] === undefined) {
// this._deviceLastTs[devType] = { lastMs: 0, offsetMs: 0 };
// }
// const devTsState = this._deviceLastTs[devType];
// if (timestamp < devTsState.lastMs) {
// devTsState.offsetMs += 0x10000;
// }
// devTsState.lastMs = timestamp;
// timestamp += devTsState.offsetMs;
// const msgBufferOffset = 2;
// // RaftLog.info(`CogStateInfo: updateFromMsg: timestamp: ${timestamp}`);
// // Handle device types
// switch (devType) {
// case 'LSM6DS':
// // RaftLog.info(`CogStateInfo: updateFromMsg: IMU`);
// // LSD6DS IMU data is coded as 16-bit signed integers little-endian
// // gx, gy, gz, ax, ay, az
// this.LSM6DS = {
// gx: msgBuffer.getInt16(msgBufferOffset, true) / 16.384,
// gy: msgBuffer.getInt16(msgBufferOffset + 2, true) / 16.384,
// gz: msgBuffer.getInt16(msgBufferOffset + 4, true) / 16.384,
// ax: msgBuffer.getInt16(msgBufferOffset + 6, true) / 8192,
// ay: msgBuffer.getInt16(msgBufferOffset + 8, true) / 8192,
// az: msgBuffer.getInt16(msgBufferOffset + 10, true) / 8192,
// tsMs: timestamp
// };
// // Debug
// // RaftLog.info(`CogStateInfo: updateFromMsg: LSM6DS: gx: ${this.LSM6DS.gx.toFixed(2)} gy: ${this.LSM6DS.gy.toFixed(2)} gz: ${this.LSM6DS.gz.toFixed(2)} ax: ${this.LSM6DS.ax.toFixed(2)} ay: ${this.LSM6DS.ay.toFixed(2)} az: ${this.LSM6DS.az.toFixed(2)} tsMs: ${this.LSM6DS.tsMs}`);
// // const imu = deviceMsg[busName][devAddr] as DeviceMsgJsonElem;
// // this.imu = {x: imu.x as number, y: imu.y as number, z: imu.z as number, lastMs: frameTimeMs};
// break;
// case "RoboCogLightV1":
// case "Light": // for older Cog pcbs with fw version 1.1.1
// // RaftLog.info(`CogStateInfo: updateFromMsg: Light`);
// // Light data is coded as 16-bit unsigned integers big-endian
// // irVals * 3, ambientVals * 1
// this.Light = {
// irVals: [msgBuffer.getUint16(msgBufferOffset, false), msgBuffer.getUint16(msgBufferOffset + 2, false), msgBuffer.getUint16(msgBufferOffset + 4, false)],
// ambientVals: [msgBuffer.getUint16(msgBufferOffset + 6, false)],
// tsMs: timestamp
// };
// // Debug
// // RaftLog.info(`CogStateInfo: updateFromMsg: Light: irVals: ${this.Light.irVals} ambientVals: ${this.Light.ambientVals}`);
// break;
// case "RoboCogPowerV1":
// case "Power": // for older Cog pcbs with fw version 1.1.1
// this.Power = {
// battV: msgBuffer.getUint16(msgBufferOffset, false) / 1000,
// tsMs: timestamp
// };
// break;
// default:
// RaftLog.info(`CogStateInfo: updateFromMsg: Unknown device type ${devType}`);
// break;
// }
// }
// }
// return [];
// }
// private hexToBytes(hex: string): Uint8Array {
// const bytes = new Uint8Array(hex.length / 2);
// for (let i = 0; i < bytes.length; i++) {
// bytes[i] = parseInt(hex.substr(i * 2, 2), 16);
// }
// return bytes;
// }
// }
// export interface IMUStateInfo {
// gx: number;
// gy: number;
// gz: number;
// ax: number;
// ay: number;
// az: number;
// tsMs: number;
// }
// export interface DeviceLastTsState {
// lastMs: number;
// offsetMs: number;
// }
// export interface DeviceMsgJsonElem {
// _t: string; // Device type
// _o: string; // Device name
// x: string; // Hex encoded message values
// }
// export interface DeviceMsgJsonBus {
// [devAddr: string]: DeviceMsgJsonElem;
// }
// export interface DeviceMsgJson {
// [busName: string]: DeviceMsgJsonBus;
// }
export class CogStateInfo {
// LSM6DS: IMUStateInfo = {gx: 0, gy: 0, gz: 0, ax: 0, ay: 0, az: 0, tsMs: 0};
// // Last timestamp for each device
// private _deviceLastTs: {[devName: string]: DeviceLastTsState} = {};
public constructor(private _deviceManager: RaftDeviceManager) {
}
get deviceManager(): RaftDeviceManager {
return this._deviceManager;
}
updateFromMsg(rxMsg: Uint8Array, frameTimeMs: number, isBinary: boolean): Array<string> {
// Debug
// RaftLog.info(`CogStateInfo: updateFromMsg: rxMsg: ${rxMsg} frameTimeMs: ${frameTimeMs}`);
if (isBinary) {
// console.log(`CogStateInfo: updateFromMsg: ${RaftUtils.bufferToHex(rxMsg)}`);
this._deviceManager.handleClientMsgBinary(rxMsg);
} else {
// Convert Uint8Array to string
const decoder = new TextDecoder('utf-8');
const jsonString = decoder.decode(rxMsg.slice(2));
// Handle using device manager
this._deviceManager.handleClientMsgJson(jsonString);
}
// // Debug
// // RaftLog.info(`CogStateInfo: updateFromMsg: jsonString: ${jsonString}`);
// // Parse JSON string to TypeScript object
// const deviceMsg: DeviceMsgJson = JSON.parse(jsonString);
// // Debug
// // for (const busName in deviceMsg) {
// // for (const devAddr in deviceMsg[busName]) {
// // for (const attrGroupName in deviceMsg[busName][devAddr]) {
// // RaftLog.info(`CogStateInfo: updateFromMsg: busName ${busName} devAddr ${devAddr} attrGroupName: ${attrGroupName}`);
// // RaftLog.info(`CogStateInfo: updateFromMsg: attrValue: ${deviceMsg[busName][devAddr][attrGroupName]}`);
// // }
// // }
// // }
// // Iterate over values and extract
// for (const busName in deviceMsg) {
// for (const devAddr in deviceMsg[busName]) {
// // Validate
// if ((deviceMsg[busName][devAddr].x === undefined) || (deviceMsg[busName][devAddr]._t === undefined)) {
// RaftLog.warn(`CogStateInfo: updateFromMsg: Invalid message`);
// continue;
// }
// // Extract message and device type
// const msgHex = deviceMsg[busName][devAddr].x;
// const devType = deviceMsg[busName][devAddr]._t;
// // Check if message is empty
// if (msgHex === '') {
// RaftLog.warn(`CogStateInfo: updateFromMsg: Empty message`);
// continue;
// }
// // Convert the hex string to an arraybuffer by converting each pair of hex chars to a byte
// const msgBytes = this.hexToBytes(msgHex);
// // Convert to a Buffer
// const msgBuffer = Buffer.from(msgBytes);
// // Extract timestamp
// let timestamp = msgBuffer.readUInt16BE(0);
// if (this._deviceLastTs[devType] === undefined) {
// this._deviceLastTs[devType] = {lastMs: 0, offsetMs: 0};
// }
// const devTsState = this._deviceLastTs[devType];
// if (timestamp < devTsState.lastMs) {
// devTsState.offsetMs += 0x10000;
// }
// devTsState.lastMs = timestamp;
// timestamp += devTsState.offsetMs;
// let msgBufferOffset = 2;
// // RaftLog.info(`CogStateInfo: updateFromMsg: timestamp: ${timestamp}`);
// // Handle device types
// switch (devType) {
// case 'LSM6DS':
// // RaftLog.info(`CogStateInfo: updateFromMsg: IMU`);
// // LSD6DS IMU data is coded as 16-bit signed integers little-endian
// // gx, gy, gz, ax, ay, az
// this.LSM6DS = {
// gx: msgBuffer.readInt16LE(msgBufferOffset) / 16.384,
// gy: msgBuffer.readInt16LE(msgBufferOffset + 2) / 16.384,
// gz: msgBuffer.readInt16LE(msgBufferOffset + 4) / 16.384,
// ax: msgBuffer.readInt16LE(msgBufferOffset + 6) / 8192,
// ay: msgBuffer.readInt16LE(msgBufferOffset + 8) / 8192,
// az: msgBuffer.readInt16LE(msgBufferOffset + 10) / 8192,
// tsMs: timestamp
// };
// // Debug
// // RaftLog.info(`CogStateInfo: updateFromMsg: LSM6DS: gx: ${this.LSM6DS.gx.toFixed(2)} gy: ${this.LSM6DS.gy.toFixed(2)} gz: ${this.LSM6DS.gz.toFixed(2)} ax: ${this.LSM6DS.ax.toFixed(2)} ay: ${this.LSM6DS.ay.toFixed(2)} az: ${this.LSM6DS.az.toFixed(2)} tsMs: ${this.LSM6DS.tsMs}`);
// // const imu = deviceMsg[busName][devAddr] as DeviceMsgJsonElem;
// // this.imu = {x: imu.x as number, y: imu.y as number, z: imu.z as number, lastMs: frameTimeMs};
// break;
// }
// }
// }
return [];
}
// private hexToBytes(hex: string): Uint8Array {
// const bytes = new Uint8Array(hex.length / 2);
// for (let i = 0; i < bytes.length; i++) {
// bytes[i] = parseInt(hex.substr(i * 2, 2), 16);
// }
// return bytes;
// }
}