| /lib/decoder.js |
|---|
| @@ -0,0 +1,286 @@ |
|---|
| #!/usr/bin/env nodejs |
| /////////////////////////////////////////////////////////////////////////// |
| // Copyright (C) Wizardry and Steamworks 2018 - License: GNU GPLv3 // |
| // Please see: http://www.gnu.org/licenses/gpl.html for legal details, // |
| // rights of fair usage, the disclaimer and warranty conditions. // |
| /////////////////////////////////////////////////////////////////////////// |
| |
| const SENSOR_PACKET_SIZE = 80; |
| |
| const PACKET_DECODE_MASKS = { |
| BUMP_WHEELDROP: { |
| BUMP_RIGHT: 0x01, |
| BUMP_LEFT: 0x02, |
| WHEELDROP_RIGHT: 0x04, |
| WHEELDROP_LEFT: 0x08 |
| }, |
| WHEEL_OVERCURRENT: { |
| SIDEBRUSH: 0x01, |
| RESERVED: 0x02, |
| MAINBRUSH: 0x04, |
| RIGHT_WHEEL: 0x08, |
| LEFT_WHEEL: 0x10 |
| }, |
| BUTTONS: { |
| CLEAN: 0x01, |
| SPOT: 0x02, |
| DOCK: 0x03, |
| MINUTE: 0x04, |
| HOUR: 0x05, |
| DAY: 0x06, |
| SCHEDULE: 0x07, |
| CLOCK: 0x08 |
| }, |
| LIGHT_BUMPER: { |
| LIGHT_BUMPER_LEFT: 0x01, |
| LIGHT_BUMPER_FRONT_LEFT: 0x02, |
| LIGHT_BUMPER_CENTER_LEFT: 0x03, |
| LIGHT_BUMPER_CENTER_RIGHT: 0x04, |
| LIGHT_BUMPER_FRONT_RIGHT: 0x05, |
| LIGHT_BUMPER_RIGHT: 0x06 |
| }, |
| CHARGING_SOURCES: { |
| INTERNAL_CHARGER: 0x01, |
| HOME_BASE: 0x02 |
| } |
| } |
| |
| /* |
| * Decode sensor data for packet group 100 (all sensors). |
| */ |
| const decode_all_sensors = function(packetBuffer, callback) { |
| if (packetBuffer.length != SENSOR_PACKET_SIZE) { |
| callback(null, 0, 'Invalid packet size') |
| return |
| } |
| |
| var packet = {} |
| var pos = 0 |
| var byte = 0x00 |
| var mask = {} |
| |
| // Bumps and Wheel Drops (Packet ID: 7, Data Bytes: 1, unsigned) |
| byte = packetBuffer.readUInt8(pos++) |
| mask = PACKET_DECODE_MASKS.BUMP_WHEELDROP |
| |
| packet.bump = {} |
| packet.bump.right = byte & mask.BUMP_RIGHT |
| packet.bump.left = byte & mask.BUMP_LEFT |
| packet.wheeldrop = {} |
| packet.wheeldrop.right = byte & mask.WHEELDROP_RIGHT |
| packet.wheeldrop.left = byte & mask.WHEELDROP_LEFT |
| |
| // Wall (Packet ID: 8, Data Bytes: 1, unsigned) |
| packet.wall = packetBuffer.readUInt8(pos++) == 1 |
| |
| // Cliff Sensors (Packet ID: 9 - 12) |
| packet.cliff = {} |
| packet.cliff.left = packetBuffer.readUInt8(pos++) == 1 |
| packet.cliff.front_left = packetBuffer.readUInt8(pos++) == 1 |
| packet.cliff.front_right = packetBuffer.readUInt8(pos++) == 1 |
| packet.cliff.right = packetBuffer.readUInt8(pos++) == 1 |
| |
| // Virtual Wall (Packet ID: 13, Data Bytes: 1, unsigned) |
| packet.virtual_wall = packetBuffer.readUInt8(pos++) == 1 |
| |
| // Wheel Overcurrents (Packet ID: 14, Data Bytes: 1, unsigned) |
| byte = packetBuffer.readUInt8(pos++) |
| mask = PACKET_DECODE_MASKS.WHEEL_OVERCURRENT |
| packet.wheel_overcurrent = {} |
| packet.wheel_overcurrent.side_brush = byte & mask.SIDEBRUSH |
| packet.wheel_overcurrent.main_brush = byte & mask.MAINBRUSH |
| packet.wheel_overcurrent.right_wheel = byte & mask.RIGHT_WHEEL |
| packet.wheel_overcurrent.left_wheel = byte & mask.LEFT_WHEEL |
| |
| // Dirt Detect (Packet ID: 15, Data Bytes: 1) |
| packet.dirt_level = packetBuffer.readInt8(pos++) |
| |
| // For Packet IDs: 0 1 or 6, 1 unused byte (Packet ID: 16, Data Bytes: 1) |
| // Apparently also for 100 |
| ++pos |
| |
| // Infrared Character Omni (Packet ID: 17, Data Bytes: 1, unsigned) |
| packet.remote = packetBuffer.readUInt8(pos++) |
| |
| // Buttons (Packet ID: 18, Data Bytes: 1, unsigned) |
| // These toggle only when the button is held down! |
| byte = packetBuffer.readUInt8(pos++) |
| mask = PACKET_DECODE_MASKS.BUTTONS |
| packet.buttons = {} |
| packet.buttons.clean = byte & mask.CLEAN |
| packet.buttons.spot = byte & mask.SPOT |
| packet.buttons.dock = byte & mask.DOCK |
| packet.buttons.minute = byte & mask.MINUTE |
| packet.buttons.hour = byte & mask.HOUR |
| packet.buttons.day = byte & mask.DAY |
| packet.buttons.schedule = byte & mask.SCHEDULE |
| packet.buttons.clock = byte & mask.CLOCK |
| |
| // Distance (Packet ID: 19, Data Bytes: 2, signed) |
| packet.distance = packetBuffer.readInt16BE(pos) |
| pos += 2 |
| |
| // Angle (Packet ID: 20, Data Bytes: 2, signed) |
| packet.angle = packetBuffer.readInt16BE(pos) |
| pos += 2 |
| |
| // Charging State (Packet ID: 21, Data Bytes: 1, unsigned) |
| packet.battery = {} |
| packet.battery.charging_state = packetBuffer.readUInt8(pos++) |
| |
| // Voltage (Packet ID: 22, Data Bytes: 2, unsigned) |
| packet.battery.voltage = packetBuffer.readUInt16BE(pos) |
| pos += 2 |
| |
| // Current (Packet ID: 23, Data Bytes: 2, signed) |
| packet.battery.current = packetBuffer.readInt16BE(pos) |
| pos += 2 |
| |
| // Temperature (Packet ID: 24, Data Bytes: 1, signed) |
| packet.battery.temp = packetBuffer.readInt8(pos++) |
| |
| // Battery Charge (Packet ID: 25, Data Bytes: 2, unsigned) |
| packet.battery.charge = packetBuffer.readUInt16BE(pos) |
| pos += 2 |
| |
| // Battery Capacity (Packet ID: 26, Data Bytes: 2, unsigned) |
| packet.battery.capacity = packetBuffer.readUInt16BE(pos) |
| pos += 2 |
| |
| // Wall Signal (Packet ID: 27, Data Bytes: 2, unsigned) |
| packet.wall_signal = packetBuffer.readUInt16BE(pos) |
| pos += 2 |
| |
| // cliff signals (Packet IDs: 28-31, Data Bytes: 2, unsigned) |
| packet.cliff_signal = {} |
| packet.cliff_signal.left = packetBuffer.readUInt16BE(pos) |
| pos += 2 |
| packet.cliff_signal.front_left = packetBuffer.readUInt16BE(pos) |
| pos += 2 |
| packet.cliff_signal.front_right = packetBuffer.readUInt16BE(pos) |
| pos += 2 |
| packet.cliff_signal.right = packetBuffer.readUInt16BE(pos) |
| pos += 2 |
| |
| // Unused (Packet IDs: 32 - 33, Data Bytes: 3) |
| pos += 3 |
| |
| // Charging Sources Available (Packet ID: 34, Data Bytes: 1, unsigned) |
| byte = packetBuffer.readUInt8(pos++) |
| mask = PACKET_DECODE_MASKS.CHARGING_SOURCES |
| packet.charging_source = {} |
| packet.charging_source.internal_charger = byte & mask.INTERNAL_CHARGER |
| packet.charging_source.home_base = byte & mask.HOME_BASE |
| |
| // OI Mode (Packet ID: 35, Data Bytes 1, unsigned) |
| packet.oi_mode = packetBuffer.readUInt8(pos++) |
| |
| // Song Number (Packet ID: 36, Data Bytes: 1, unsigned) |
| packet.song = {} |
| packet.song.number = packetBuffer.readUInt8(pos++) |
| |
| // Song Playing (Packet ID: 37, Data Bytes: 1, unsigned) |
| packet.song.playing = packetBuffer.readUInt8(pos++) |
| |
| // Number of Stream Packets (Packet ID: 38, Data Bytes: 1, unsigned) |
| packet.stream_packets = packetBuffer.readUInt8(pos++) |
| |
| // Requested Velocity (Packet ID: 39, Data Bytes: 2, signed) |
| packet.last_requested_velocity = {} |
| packet.last_requested_velocity.general = packetBuffer.readInt16BE(pos) |
| pos += 2 |
| |
| // Requested Radius (Packet ID: 40, Data Bytes: 2, signed) |
| packet.requested_radius = packetBuffer.readInt16BE(pos) |
| pos += 2 |
| |
| // Requested Right Velocity (Packet ID: 41, Data Bytes: 2, signed) |
| packet.last_requested_velocity.right = packetBuffer.readInt16BE(pos) |
| pos += 2 |
| |
| // Requested Left Velocity (Packet ID: 42, Data Bytes: 2, signed) |
| packet.last_requested_velocity.left = packetBuffer.readInt16BE(pos) |
| pos += 2 |
| |
| // Left Encoder Counts (Packet ID: 43, Data Bytes: 2, signed) |
| packet.encoder = {} |
| packet.encoder.left = packetBuffer.readInt16BE(pos) |
| pos += 2 |
| |
| // Right Encoder Counts (Packet ID: 44, Data Bytes: 2, signed) |
| packet.encoder.right = packetBuffer.readInt16BE(pos) |
| pos += 2 |
| |
| // Light Bumper (Packet ID: 45, Data Bytes: 1, unsigned) |
| byte = packetBuffer.readUInt8(pos++) |
| mask = PACKET_DECODE_MASKS.LIGHT_BUMPER |
| packet.light_bumper = {} |
| packet.light_bumper.left = byte & mask.LIGHT_BUMPER_LEFT |
| packet.light_bumper.front_left = byte & mask.LIGHT_BUMPER_FRONT_LEFT |
| packet.light_bumper.center_left = byte & mask.LIGHT_BUMPER_CENTER_LEFT |
| packet.light_bumper.center_right = byte & mask.LIGHT_BUMPER_CENTER_RIGHT |
| packet.light_bumper.front_right = byte & mask.LIGHT_BUMPER_FRONT_RIGHT |
| packet.light_bumper.right = byte & mask.LIGHT_BUMPER_RIGHT |
| |
| // Light Bump Left Signal (Packet ID: 46, Data Bytes: 2, unsigned) |
| packet.light_bump_signal = {} |
| packet.light_bump_signal.left = packetBuffer.readUInt16BE(pos) |
| pos += 2 |
| |
| // Light Bump Front Left Signal (Packet ID: 47, Data Bytes: 2, unsigned) |
| packet.light_bump_signal.front_left = packetBuffer.readUInt16BE(pos) |
| pos += 2 |
| |
| // Light Bump Center Left Signal (Packet ID: 48, Data Bytes: 2, unsigned) |
| packet.light_bump_signal.center_left = packetBuffer.readUInt16BE(pos) |
| pos += 2 |
| |
| // Light Bump Center Right Signal (Packet ID: 49, Data Bytes: 2, unsigned) |
| packet.light_bump_signal.center_right = packetBuffer.readUInt16BE(pos) |
| pos += 2 |
| |
| // Light Bump Front Right Signal (Packet ID: 50, Data Bytes: 2, unsigned) |
| packet.light_bump_signal.front_right = packetBuffer.readUInt16BE(pos) |
| pos += 2 |
| |
| // Light Bump Right Signal (Packet ID: 51, Data Bytes: 2, unsigned) |
| packet.light_bump_signal.right = packetBuffer.readUInt16BE(pos) |
| pos += 2 |
| |
| // Infrared Character Left (Packet ID: 52, Data Bytes: 1, unsigned) |
| packet.infrared = {} |
| packet.infrared.character = {} |
| packet.infrared.character.left = packetBuffer.readUInt8(pos++) |
| |
| // Infrared Character Left (Packet ID: 53, Data Bytes: 1, unsigned) |
| packet.infrared.character.right = packetBuffer.readUInt8(pos++) |
| |
| // Left Motor Current (Packet IDs: 54 - 57, Data Bytes: 2, signed) |
| packet.motor_current = {} |
| packet.motor_current.left = packetBuffer.readInt16BE(pos) |
| pos += 2 |
| packet.motor_current.right = packetBuffer.readInt16BE(pos) |
| pos += 2 |
| packet.motor_current.main_brush = packetBuffer.readInt16BE(pos) |
| pos += 2 |
| packet.motor_current.side_brush = packetBuffer.readInt16BE(pos) |
| pos += 2 |
| |
| // Stasis (Packet ID: 58, Data Bytes: 1) |
| packet.stasis = packetBuffer.readUInt8(pos++) |
| |
| // Check whether the complete packet has been consumed. |
| if (pos != SENSOR_PACKET_SIZE) { |
| callback(null, pos, 'Insuficient data to assemble a complete packet') |
| return |
| } |
| |
| // Return the data |
| callback(packet, pos, null) |
| } |
| |
| module.exports = { |
| PACKET_DECODE_MASKS: PACKET_DECODE_MASKS, |
| SENSOR_PACKET_SIZE: SENSOR_PACKET_SIZE, |
| decode_all_sensors: decode_all_sensors |
| } |
