From b5ce6706d65a48213620b14f432f0e3a8cd762d4 Mon Sep 17 00:00:00 2001 From: legop3 Date: Tue, 11 Nov 2025 22:40:56 -0500 Subject: [PATCH] decode sensors! --- server/public/app.js | 23 +++- server/src/index.js | 9 +- server/src/sensorDecoder.js | 223 ++++++++++++++++++++++++++++++++++++ server_client_structure.md | 10 +- 4 files changed, 259 insertions(+), 6 deletions(-) create mode 100644 server/src/sensorDecoder.js diff --git a/server/public/app.js b/server/public/app.js index df9f5271..e86b8a90 100644 --- a/server/public/app.js +++ b/server/public/app.js @@ -40,9 +40,9 @@ socket.on('rovers', (list) => { } }); -socket.on('sensorFrame', ({ roverId, frame }) => { +socket.on('sensorFrame', ({ roverId, frame, sensors }) => { if (roverId !== selectedRover) return; - sensorOutput.textContent = formatSensorFrame(frame.data); + sensorOutput.textContent = renderSensors(sensors, frame?.data); }); socket.on('commandAck', ({ roverId, status, error }) => { @@ -172,3 +172,22 @@ function formatSensorFrame(base64) { function clamp(value, min, max) { return Math.max(min, Math.min(max, value)); } + +function renderSensors(sensors = {}, rawBase64) { + const lines = []; + if (sensors && Object.keys(sensors).length) { + for (const [key, value] of Object.entries(sensors)) { + if (value && typeof value === 'object' && !Array.isArray(value)) { + lines.push(`${key}: ${JSON.stringify(value)}`); + } else { + lines.push(`${key}: ${value}`); + } + } + } else { + lines.push('No decoded sensor data yet.'); + } + if (rawBase64) { + lines.push('', 'raw:', formatSensorFrame(rawBase64)); + } + return lines.join('\n'); +} diff --git a/server/src/index.js b/server/src/index.js index b285a2bb..ac2ae89e 100644 --- a/server/src/index.js +++ b/server/src/index.js @@ -5,6 +5,7 @@ const morgan = require('morgan'); const { Server: SocketIOServer } = require('socket.io'); const { WebSocketServer } = require('ws'); const { v4: uuidv4 } = require('uuid'); +const { parseSensorFrame } = require('./sensorDecoder'); const PORT = process.env.PORT || 8080; @@ -62,9 +63,11 @@ function handleRoverConnection(ws) { if (!roverId || !rovers.has(roverId)) { return; } - rovers.get(roverId).lastSensor = msg; - rovers.get(roverId).lastSeen = Date.now(); - io.emit('sensorFrame', { roverId, frame: msg }); + const rover = rovers.get(roverId); + const decoded = parseSensorFrame(msg.data); + rover.lastSensor = { raw: msg, decoded }; + rover.lastSeen = Date.now(); + io.emit('sensorFrame', { roverId, frame: msg, sensors: decoded }); break; case 'ack': if (msg.id && pendingCommands.has(msg.id)) { diff --git a/server/src/sensorDecoder.js b/server/src/sensorDecoder.js new file mode 100644 index 00000000..260a503e --- /dev/null +++ b/server/src/sensorDecoder.js @@ -0,0 +1,223 @@ +const HEADER = 0x13; +const CHARGING_STATE = { + 0: 'not charging', + 1: 'reconditioning charging', + 2: 'full charging', + 3: 'trickle charging', + 4: 'waiting', + 5: 'charging fault', +}; + +const OI_MODES = { + 0: 'off', + 1: 'passive', + 2: 'safe', + 3: 'full', +}; + +const GROUP100_LAYOUT = [ + { id: 7, key: 'bumpsAndWheelDrops', bytes: 1, parser: parseBumps }, + { id: 8, key: 'wall', bytes: 1, parser: parseBool }, + { id: 9, key: 'cliffLeft', bytes: 1, parser: parseBool }, + { id: 10, key: 'cliffFrontLeft', bytes: 1, parser: parseBool }, + { id: 11, key: 'cliffFrontRight', bytes: 1, parser: parseBool }, + { id: 12, key: 'cliffRight', bytes: 1, parser: parseBool }, + { id: 13, key: 'virtualWall', bytes: 1, parser: parseBool }, + { id: 14, key: 'wheelOvercurrents', bytes: 1, parser: parseWheelCurrents }, + { id: 15, key: 'dirtDetect', bytes: 1, parser: parseUInt }, + { id: 16, key: 'dirtDetectLeft', bytes: 1, parser: parseUInt }, + { id: 17, key: 'infraredCharacterOmni', bytes: 1, parser: parseUInt }, + { id: 18, key: 'buttons', bytes: 1, parser: parseButtons }, + { id: 19, key: 'distanceMm', bytes: 2, parser: parseInt }, + { id: 20, key: 'angleDeg', bytes: 2, parser: parseInt }, + { id: 21, key: 'chargingState', bytes: 1, parser: parseChargingState }, + { id: 22, key: 'voltageMv', bytes: 2, parser: parseUInt }, + { id: 23, key: 'currentMa', bytes: 2, parser: parseInt }, + { id: 24, key: 'batteryTemperatureC', bytes: 1, parser: parseInt }, + { id: 25, key: 'batteryChargeMah', bytes: 2, parser: parseUInt }, + { id: 26, key: 'batteryCapacityMah', bytes: 2, parser: parseUInt }, + { id: 27, key: 'wallSignal', bytes: 2, parser: parseUInt }, + { id: 28, key: 'cliffLeftSignal', bytes: 2, parser: parseUInt }, + { id: 29, key: 'cliffFrontLeftSignal', bytes: 2, parser: parseUInt }, + { id: 30, key: 'cliffFrontRightSignal', bytes: 2, parser: parseUInt }, + { id: 31, key: 'cliffRightSignal', bytes: 2, parser: parseUInt }, + { id: 32, key: 'chargingSourcesAvailable', bytes: 1, parser: parseChargeSources }, + { id: 33, key: 'chargingSourcesReserved', bytes: 2, parser: parseUInt }, + { id: 34, key: 'chargingSources', bytes: 1, parser: parseChargeSources }, + { id: 35, key: 'oiMode', bytes: 1, parser: parseOiMode }, + { id: 36, key: 'songNumber', bytes: 1, parser: parseUInt }, + { id: 37, key: 'songPlaying', bytes: 1, parser: parseBool }, + { id: 38, key: 'streamPacketCount', bytes: 1, parser: parseUInt }, + { id: 39, key: 'requestedVelocity', bytes: 2, parser: parseInt }, + { id: 40, key: 'requestedRadius', bytes: 2, parser: parseInt }, + { id: 41, key: 'requestedRightVelocity', bytes: 2, parser: parseInt }, + { id: 42, key: 'requestedLeftVelocity', bytes: 2, parser: parseInt }, + { id: 43, key: 'encoderCountsLeft', bytes: 2, parser: parseUInt }, + { id: 44, key: 'encoderCountsRight', bytes: 2, parser: parseUInt }, + { id: 45, key: 'lightBumper', bytes: 1, parser: parseLightBumper }, + { id: 46, key: 'lightBumpLeftSignal', bytes: 2, parser: parseUInt }, + { id: 47, key: 'lightBumpFrontLeftSignal', bytes: 2, parser: parseUInt }, + { id: 48, key: 'lightBumpCenterLeftSignal', bytes: 2, parser: parseUInt }, + { id: 49, key: 'lightBumpCenterRightSignal', bytes: 2, parser: parseUInt }, + { id: 50, key: 'lightBumpFrontRightSignal', bytes: 2, parser: parseUInt }, + { id: 51, key: 'lightBumpRightSignal', bytes: 2, parser: parseUInt }, + { id: 52, key: 'infraredCharacterLeft', bytes: 1, parser: parseUInt }, + { id: 53, key: 'infraredCharacterRight', bytes: 1, parser: parseUInt }, + { id: 54, key: 'wheelLeftCurrentMa', bytes: 2, parser: parseInt }, + { id: 55, key: 'wheelRightCurrentMa', bytes: 2, parser: parseInt }, + { id: 56, key: 'mainBrushCurrentMa', bytes: 2, parser: parseInt }, + { id: 57, key: 'sideBrushCurrentMa', bytes: 2, parser: parseInt }, + { id: 58, key: 'stasis', bytes: 1, parser: parseBool }, +]; + +const GROUP100_TOTAL = GROUP100_LAYOUT.reduce((sum, spec) => sum + spec.bytes, 0); + +const TOP_LEVEL_PACKETS = { + 100: GROUP100_TOTAL, + 21: 1, + 34: 1, +}; + +function parseSensorFrame(base64Data) { + if (!base64Data) return null; + const buf = Buffer.from(base64Data, 'base64'); + if (buf.length < 4 || buf[0] !== HEADER) { + return null; + } + const nBytes = buf[1]; + if (buf.length < nBytes + 3) { + return null; + } + const payload = buf.slice(2, 2 + nBytes); + const checksum = buf[2 + nBytes]; + if (!validateChecksum(buf.slice(0, 2 + nBytes + 1), checksum)) { + return null; + } + const decoded = {}; + let offset = 0; + while (offset < payload.length) { + const packetId = payload[offset++]; + const size = TOP_LEVEL_PACKETS[packetId]; + if (!size || offset + size > payload.length) { + return null; + } + const segment = payload.slice(offset, offset + size); + offset += size; + if (packetId === 100) { + Object.assign(decoded, decodeGroup100(segment)); + } else if (packetId === 21 && decoded.chargingState == null) { + decoded.chargingState = parseChargingState(segment); + } else if (packetId === 34 && decoded.chargingSources == null) { + decoded.chargingSources = parseChargeSources(segment); + } + } + return decoded; +} + +function decodeGroup100(buf) { + if (buf.length !== GROUP100_TOTAL) { + return {}; + } + const values = {}; + let offset = 0; + for (const spec of GROUP100_LAYOUT) { + const slice = buf.slice(offset, offset + spec.bytes); + offset += spec.bytes; + try { + values[spec.key] = spec.parser ? spec.parser(slice) : parseUInt(slice); + } catch (err) { + values[spec.key] = null; + } + } + return values; +} + +function parseBool(buf) { + return Boolean(buf[0]); +} + +function parseUInt(buf) { + return buf.readUIntBE(0, buf.length); +} + +function parseInt(buf) { + return buf.readIntBE(0, buf.length); +} + +function parseBumps(buf) { + const value = buf[0]; + return { + bumpRight: Boolean(value & 0x01), + bumpLeft: Boolean(value & 0x02), + wheelDropRight: Boolean(value & 0x04), + wheelDropLeft: Boolean(value & 0x08), + }; +} + +function parseWheelCurrents(buf) { + const value = buf[0]; + return { + sideBrush: Boolean(value & 0x01), + mainBrush: Boolean(value & 0x04), + rightWheel: Boolean(value & 0x08), + leftWheel: Boolean(value & 0x10), + }; +} + +const BUTTON_LABELS = ['clean', 'spot', 'dock', 'minute', 'hour', 'day', 'schedule', 'clock']; +function parseButtons(buf) { + const v = buf[0]; + const result = {}; + BUTTON_LABELS.forEach((label, idx) => { + result[label] = Boolean(v & (1 << idx)); + }); + return result; +} + +function parseChargingState(buf) { + const code = buf[0]; + return { + code, + label: CHARGING_STATE[code] || 'unknown', + }; +} + +function parseChargeSources(buf) { + const value = buf[0]; + return { + internalCharger: Boolean(value & 0x01), + homeBase: Boolean(value & 0x02), + raw: value, + }; +} + +function parseOiMode(buf) { + const code = buf[0]; + return { + code, + label: OI_MODES[code] || 'unknown', + }; +} + +const LIGHT_BUMPER_LABELS = ['left', 'frontLeft', 'centerLeft', 'centerRight', 'frontRight', 'right']; +function parseLightBumper(buf) { + const value = buf[0]; + const obj = {}; + LIGHT_BUMPER_LABELS.forEach((label, idx) => { + obj[label] = Boolean(value & (1 << idx)); + }); + return obj; +} + +function validateChecksum(frame, checksum) { + let sum = 0; + for (const byte of frame) { + sum = (sum + byte) & 0xff; + } + return (sum & 0xff) === 0; +} + +module.exports = { + parseSensorFrame, + CHARGING_STATE, +}; diff --git a/server_client_structure.md b/server_client_structure.md index 0c626fd8..2cd1de2f 100644 --- a/server_client_structure.md +++ b/server_client_structure.md @@ -9,13 +9,17 @@ this service would import the roomba list from whatever other service contains i - modular code structure - one folder for each of these categories - globals + - GLOBALS ARE: "static" parts of the server that don't contain any interactive logic. - where the express, websocket, and socket.io instances will be - other global things - services + - SERVICES ARE: parts of the program that are part of the interaction pipeline. - contains things like the roomba manager - will also in the future contain other things like a discord bot, home assistant integration, etc - anything with a large amount of controlling logic should be in here - helpers + - HELPERS ARE: parts of the program that other modules only pull helper funcions or classes from. + - if a function or class is dedicated to a service, it should NOT be in a helper. - contains passive helpers - things like the logger system - no "service" logic in here, only things that are passively pulled out and used inside other modules @@ -40,4 +44,8 @@ I want there to be a ground up system where I can set the entire service to four - lockdown (only "lockdown" admins can view or drive. no one else can view, not even spectators). only admins can change modes. -do NOT implement any authentication stuff yet, just add a stub service with places set up to put the auth. logic \ No newline at end of file +do NOT implement any authentication stuff yet, just add a stub service with places set up to put the auth. logic + +# web client code structure: +- the same as the server's code structure + - all ES6 \ No newline at end of file