mirror of
https://github.com/legop3/MultiRoombaRover.git
synced 2026-09-16 09:31:20 -04:00
slop
This commit is contained in:
@@ -0,0 +1,68 @@
|
||||
# Wii Balance Board weigh station
|
||||
|
||||
This service supports an original Nintendo `RVL-WBC-01` as an automatic rover
|
||||
weigh station. The server installer builds the native bridge, grants only that
|
||||
bridge `CAP_NET_ADMIN`, and installs a udev rule limited to the calibrated
|
||||
Balance Board input device.
|
||||
|
||||
## Commissioning on the real server
|
||||
|
||||
1. Run `sudo ./install_server.sh` from the `server` directory.
|
||||
2. Set `balanceBoard.enabled: true` in `server/config.yaml` and restart
|
||||
`multirover.service`.
|
||||
3. Open the Activities tab. While it says **Pairing setup**, press the red Sync
|
||||
button under the board's battery cover.
|
||||
4. Wait for the card to change to **Sleeping** or **Ready**. BlueZ retains the
|
||||
bond and the service also stores the selected board address in
|
||||
`server/data/balance-board.json`.
|
||||
5. For ordinary use, press only the front power button. The board should connect
|
||||
to the server without another Sync operation.
|
||||
|
||||
The bridge intentionally uses short Bluetooth Classic discovery windows only
|
||||
while no board is commissioned. It stops scanning after a successful bond.
|
||||
|
||||
## Physical station
|
||||
|
||||
Recess the board into a platform or place independent approach and departure
|
||||
ramps beside it. A ramp that rests partly on the board and partly on the floor
|
||||
will transfer some rover load to the floor and make the reading incorrect. At
|
||||
the stable capture position, every wheel must be supported by the board itself.
|
||||
|
||||
## Verification
|
||||
|
||||
On the server, useful checks are:
|
||||
|
||||
```sh
|
||||
getcap src/services/balanceBoardService/native/balance_board_worker
|
||||
modinfo hid-wiimote
|
||||
journalctl -u multirover.service -f
|
||||
```
|
||||
|
||||
The capability check should report `cap_net_admin=ep`. When the board connects,
|
||||
an input device named `Nintendo Wii Remote Balance Board` should appear under
|
||||
`/dev/input` and the Activities card should begin receiving corner loads.
|
||||
|
||||
Test at least the following before treating the station as unattended:
|
||||
|
||||
- Ten front-button wake, measurement, drive-off, and idle-disconnect cycles.
|
||||
- A server restart while the board is asleep.
|
||||
- A server restart while the board is connected.
|
||||
- A `bluetooth.service` restart followed by another front-button wake.
|
||||
- Battery removal and replacement without removing the stored BlueZ bond.
|
||||
|
||||
Replacing the server Bluetooth adapter, erasing `/var/lib/bluetooth`, or using
|
||||
**Forget board** invalidates the board's remembered host and requires the red
|
||||
Sync commissioning step again.
|
||||
|
||||
## Development simulation
|
||||
|
||||
The native worker has a cycle simulator that never opens Bluetooth or input
|
||||
devices:
|
||||
|
||||
```sh
|
||||
BALANCE_BOARD_SIMULATE=cycle ./native/balance_board_worker
|
||||
```
|
||||
|
||||
For a full local server/UI exercise, use a development config containing both
|
||||
`balanceBoard.enabled: true` and `balanceBoard.simulate: true`. The public
|
||||
example omits `simulate` because it is not a production hardware setting.
|
||||
@@ -0,0 +1,175 @@
|
||||
// Balance Board Hardware Bridge
|
||||
// Purpose: Supervises the capability-limited native worker and converts its JSON-line protocol into service events.
|
||||
// Scope: Owns process lifecycle, restart recovery, worker commands, and protocol validation; measurement policy remains in index.js.
|
||||
const { spawn } = require('child_process');
|
||||
const EventEmitter = require('events');
|
||||
const path = require('path');
|
||||
|
||||
const WORKER_PATH =
|
||||
process.env.BALANCE_BOARD_WORKER ||
|
||||
path.join(__dirname, 'native', 'balance_board_worker');
|
||||
const RESTART_DELAY_MS = 2000;
|
||||
const STDERR_LOG_INTERVAL_MS = 5000;
|
||||
|
||||
function createBalanceBoardHardware({ logger, address = '', simulate = false } = {}) {
|
||||
const events = new EventEmitter();
|
||||
let worker = null;
|
||||
let stdoutBuffer = '';
|
||||
let stopped = false;
|
||||
let restartTimer = null;
|
||||
let lastStderrLogAt = 0;
|
||||
let suppressedStderrLines = 0;
|
||||
let currentAddress = address;
|
||||
|
||||
function emitProtocolError(message) {
|
||||
events.emit('message', {
|
||||
type: 'status',
|
||||
state: 'error',
|
||||
error: message,
|
||||
});
|
||||
}
|
||||
|
||||
function processStdout(chunk) {
|
||||
stdoutBuffer += chunk.toString('utf8');
|
||||
let newline = stdoutBuffer.indexOf('\n');
|
||||
while (newline !== -1) {
|
||||
const line = stdoutBuffer.slice(0, newline).trim();
|
||||
stdoutBuffer = stdoutBuffer.slice(newline + 1);
|
||||
if (line) {
|
||||
try {
|
||||
const message = JSON.parse(line);
|
||||
if (!message || typeof message !== 'object' || typeof message.type !== 'string') {
|
||||
throw new Error('message needs a type');
|
||||
}
|
||||
events.emit('message', message);
|
||||
} catch (err) {
|
||||
// A corrupted stdout line means measurement framing can no longer be
|
||||
// trusted. Surface the exact line rather than silently discarding a
|
||||
// potential hardware failure that would otherwise look like zero kg.
|
||||
emitProtocolError(`balance board worker returned invalid JSON: ${err.message}`);
|
||||
logger?.warn?.('Balance Board worker protocol error', { line, error: err.message });
|
||||
}
|
||||
}
|
||||
newline = stdoutBuffer.indexOf('\n');
|
||||
}
|
||||
}
|
||||
|
||||
function scheduleRestart() {
|
||||
if (stopped || restartTimer) return;
|
||||
restartTimer = setTimeout(() => {
|
||||
restartTimer = null;
|
||||
start();
|
||||
}, RESTART_DELAY_MS);
|
||||
}
|
||||
|
||||
function start() {
|
||||
if (stopped || (worker && !worker.killed)) return;
|
||||
stdoutBuffer = '';
|
||||
|
||||
const child = spawn(WORKER_PATH, [], {
|
||||
env: {
|
||||
...process.env,
|
||||
BALANCE_BOARD_ADDRESS: currentAddress || '',
|
||||
BALANCE_BOARD_SIMULATE: simulate ? 'cycle' : '',
|
||||
},
|
||||
stdio: ['pipe', 'pipe', 'pipe'],
|
||||
});
|
||||
worker = child;
|
||||
|
||||
child.stdout.on('data', processStdout);
|
||||
child.stderr.on('data', (chunk) => {
|
||||
const text = chunk.toString('utf8').trim();
|
||||
if (!text) return;
|
||||
const now = Date.now();
|
||||
if (now - lastStderrLogAt >= STDERR_LOG_INTERVAL_MS) {
|
||||
const suffix = suppressedStderrLines
|
||||
? ` (${suppressedStderrLines} worker stderr lines suppressed)`
|
||||
: '';
|
||||
logger?.warn?.(`Balance Board worker: ${text}${suffix}`);
|
||||
lastStderrLogAt = now;
|
||||
suppressedStderrLines = 0;
|
||||
} else {
|
||||
suppressedStderrLines += 1;
|
||||
}
|
||||
});
|
||||
child.on('error', (err) => {
|
||||
if (worker === child) worker = null;
|
||||
emitProtocolError(`balance board worker failed to start: ${err.message}`);
|
||||
scheduleRestart();
|
||||
});
|
||||
child.on('close', (code, signal) => {
|
||||
if (worker === child) worker = null;
|
||||
if (!stopped) {
|
||||
emitProtocolError(`balance board worker exited (${signal || code})`);
|
||||
scheduleRestart();
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
function send(command, payload = {}) {
|
||||
if (!worker || worker.killed || !worker.stdin?.writable) {
|
||||
throw new Error('balance board worker is not running');
|
||||
}
|
||||
// Commands are intentionally a tiny fixed vocabulary. The native worker
|
||||
// never accepts shell fragments or arbitrary Bluetooth addresses from the
|
||||
// browser, so an admin maintenance action cannot become command execution.
|
||||
worker.stdin.write(`${JSON.stringify({ command, ...payload })}\n`);
|
||||
}
|
||||
|
||||
function stop() {
|
||||
stopped = true;
|
||||
if (restartTimer) {
|
||||
clearTimeout(restartTimer);
|
||||
restartTimer = null;
|
||||
}
|
||||
if (!worker) return;
|
||||
const child = worker;
|
||||
worker = null;
|
||||
try {
|
||||
child.stdin.write(`${JSON.stringify({ command: 'stop' })}\n`);
|
||||
} catch (_err) {
|
||||
// The worker may have already closed stdin while its exit event is still
|
||||
// queued. SIGTERM below remains the reliable cleanup path.
|
||||
}
|
||||
child.kill('SIGTERM');
|
||||
setTimeout(() => {
|
||||
// bluetoothctl may still be finishing a bounded pairing command inside a
|
||||
// worker thread. Do not let that delay server shutdown indefinitely.
|
||||
if (child.exitCode == null && child.signalCode == null) child.kill('SIGKILL');
|
||||
}, 1500).unref();
|
||||
}
|
||||
|
||||
function restart() {
|
||||
if (worker) {
|
||||
const child = worker;
|
||||
worker = null;
|
||||
child.kill('SIGTERM');
|
||||
setTimeout(() => {
|
||||
if (child.exitCode == null && child.signalCode == null) child.kill('SIGKILL');
|
||||
}, 1500).unref();
|
||||
}
|
||||
if (restartTimer) clearTimeout(restartTimer);
|
||||
restartTimer = setTimeout(() => {
|
||||
restartTimer = null;
|
||||
start();
|
||||
}, 250);
|
||||
}
|
||||
|
||||
return {
|
||||
events,
|
||||
start,
|
||||
stop,
|
||||
restart,
|
||||
send,
|
||||
setAddress(nextAddress) {
|
||||
// The factory can be created before first commissioning. Preserve the
|
||||
// newly paired address for later bridge restarts in the same Node process
|
||||
// instead of reverting the replacement worker to discovery mode.
|
||||
currentAddress = typeof nextAddress === 'string' ? nextAddress.trim().toUpperCase() : '';
|
||||
},
|
||||
};
|
||||
}
|
||||
|
||||
module.exports = {
|
||||
createBalanceBoardHardware,
|
||||
};
|
||||
@@ -0,0 +1,448 @@
|
||||
// Balance Board Service
|
||||
// Purpose: Turns calibrated corner loads into an automatic rover weigh-station lifecycle.
|
||||
// Scope: Owns configuration, tare/stability policy, session state, Socket.IO delivery, persistence, and admin maintenance actions.
|
||||
const fs = require('fs');
|
||||
const EventEmitter = require('events');
|
||||
const io = require('../../globals/io');
|
||||
const logger = require('../../globals/logger').child('balanceBoardService');
|
||||
const { loadConfig } = require('../../helpers/configLoader');
|
||||
const { resolveDataDir, resolveDataPath } = require('../../helpers/dataPaths');
|
||||
const { isFeatureEnabled } = require('../../helpers/features');
|
||||
const { isAdmin } = require('../roleService');
|
||||
const { publishEvent } = require('../eventBus');
|
||||
const { createBalanceBoardHardware } = require('./hardware');
|
||||
|
||||
const events = new EventEmitter();
|
||||
const config = loadConfig();
|
||||
const rawConfig = config.balanceBoard || {};
|
||||
const enabled = isFeatureEnabled('balanceBoard');
|
||||
const DATA_DIR = resolveDataDir();
|
||||
const STORE_PATH = resolveDataPath('balance-board.json');
|
||||
const FRAME_ROOM = 'balance-board-viewers';
|
||||
const TARE_SAMPLE_COUNT = 20;
|
||||
const MAX_TARE_WEIGHT_KG = 2;
|
||||
const DEFAULTS = {
|
||||
minimumWeightKg: 1,
|
||||
stableDurationMs: 1500,
|
||||
stableToleranceKg: 0.15,
|
||||
exitWeightKg: 0.35,
|
||||
disconnectWhenEmptyMs: 30000,
|
||||
};
|
||||
|
||||
function finiteNumber(value, fallback, minimum = -Infinity, maximum = Infinity) {
|
||||
const number = Number(value);
|
||||
if (!Number.isFinite(number)) return fallback;
|
||||
return Math.max(minimum, Math.min(maximum, number));
|
||||
}
|
||||
|
||||
const settings = {
|
||||
minimumWeightKg: finiteNumber(rawConfig.minimumWeightKg, DEFAULTS.minimumWeightKg, 0.1, 100),
|
||||
stableDurationMs: finiteNumber(rawConfig.stableDurationMs, DEFAULTS.stableDurationMs, 250, 10000),
|
||||
stableToleranceKg: finiteNumber(rawConfig.stableToleranceKg, DEFAULTS.stableToleranceKg, 0.01, 5),
|
||||
exitWeightKg: finiteNumber(rawConfig.exitWeightKg, DEFAULTS.exitWeightKg, 0, 20),
|
||||
disconnectWhenEmptyMs: finiteNumber(
|
||||
rawConfig.disconnectWhenEmptyMs,
|
||||
DEFAULTS.disconnectWhenEmptyMs,
|
||||
0,
|
||||
30 * 60 * 1000,
|
||||
),
|
||||
// Simulation is intentionally undocumented in the public example config.
|
||||
// It exists for development and CI where no Bluetooth board is attached.
|
||||
simulate: Boolean(rawConfig.simulate || process.env.BALANCE_BOARD_SIMULATE),
|
||||
};
|
||||
|
||||
function loadStore() {
|
||||
try {
|
||||
const parsed = JSON.parse(fs.readFileSync(STORE_PATH, 'utf8'));
|
||||
const address = typeof parsed?.address === 'string' ? parsed.address.trim().toUpperCase() : '';
|
||||
return { address };
|
||||
} catch (err) {
|
||||
if (err.code !== 'ENOENT') logger.warn('Failed to load Balance Board store', err.message);
|
||||
return { address: '' };
|
||||
}
|
||||
}
|
||||
|
||||
let store = enabled ? loadStore() : { address: '' };
|
||||
let hardware = null;
|
||||
let phase = enabled ? (store.address ? 'waiting' : 'commissioning') : 'disabled';
|
||||
let hardwareState = enabled ? 'starting' : 'disabled';
|
||||
let connected = false;
|
||||
let lastError = null;
|
||||
let batteryPercent = null;
|
||||
let latestFrame = null;
|
||||
let lastMeasurement = null;
|
||||
let tareSamples = [];
|
||||
let tare = { topRight: 0, bottomRight: 0, topLeft: 0, bottomLeft: 0 };
|
||||
let stableSamples = [];
|
||||
let emptySince = null;
|
||||
let disconnectRequested = false;
|
||||
|
||||
function persistStore() {
|
||||
fs.mkdirSync(DATA_DIR, { recursive: true });
|
||||
const next = {
|
||||
address: store.address || '',
|
||||
updatedAt: Date.now(),
|
||||
};
|
||||
const temporary = `${STORE_PATH}.${process.pid}.${Date.now()}.tmp`;
|
||||
fs.writeFileSync(temporary, `${JSON.stringify(next, null, 2)}\n`, 'utf8');
|
||||
fs.renameSync(temporary, STORE_PATH);
|
||||
}
|
||||
|
||||
function round(value, digits = 2) {
|
||||
const factor = 10 ** digits;
|
||||
return Math.round((Number(value) || 0) * factor) / factor;
|
||||
}
|
||||
|
||||
function normalizeCorners(raw = {}) {
|
||||
// The native bridge reports centi-kilograms because that is the calibrated
|
||||
// unit produced by hid-wiimote. Convert once at the service boundary so every
|
||||
// browser and future event consumer receives ordinary kilograms.
|
||||
return {
|
||||
topRight: Math.max(0, Number(raw.topRight) || 0) / 100,
|
||||
bottomRight: Math.max(0, Number(raw.bottomRight) || 0) / 100,
|
||||
topLeft: Math.max(0, Number(raw.topLeft) || 0) / 100,
|
||||
bottomLeft: Math.max(0, Number(raw.bottomLeft) || 0) / 100,
|
||||
};
|
||||
}
|
||||
|
||||
function applyTare(corners) {
|
||||
return {
|
||||
topRight: Math.max(0, corners.topRight - tare.topRight),
|
||||
bottomRight: Math.max(0, corners.bottomRight - tare.bottomRight),
|
||||
topLeft: Math.max(0, corners.topLeft - tare.topLeft),
|
||||
bottomLeft: Math.max(0, corners.bottomLeft - tare.bottomLeft),
|
||||
};
|
||||
}
|
||||
|
||||
function describeLoad(corners) {
|
||||
const totalKg = corners.topRight + corners.bottomRight + corners.topLeft + corners.bottomLeft;
|
||||
if (totalKg <= 0.001) return { totalKg: 0, center: { x: 0, y: 0 } };
|
||||
const right = corners.topRight + corners.bottomRight;
|
||||
const left = corners.topLeft + corners.bottomLeft;
|
||||
const top = corners.topRight + corners.topLeft;
|
||||
const bottom = corners.bottomRight + corners.bottomLeft;
|
||||
return {
|
||||
totalKg: round(totalKg, 2),
|
||||
// Normalized -1..1 coordinates make the browser independent of the board's
|
||||
// physical dimensions while retaining the full centering information.
|
||||
center: {
|
||||
x: round((right - left) / totalKg, 3),
|
||||
y: round((bottom - top) / totalKg, 3),
|
||||
},
|
||||
};
|
||||
}
|
||||
|
||||
function getState() {
|
||||
return {
|
||||
enabled,
|
||||
paired: Boolean(store.address) || settings.simulate,
|
||||
address: store.address || (settings.simulate ? 'SIMULATED' : null),
|
||||
connected,
|
||||
hardwareState,
|
||||
phase,
|
||||
batteryPercent,
|
||||
lastError,
|
||||
lastMeasurement,
|
||||
settings: {
|
||||
minimumWeightKg: settings.minimumWeightKg,
|
||||
stableDurationMs: settings.stableDurationMs,
|
||||
},
|
||||
};
|
||||
}
|
||||
|
||||
function emitStateChange(reason) {
|
||||
events.emit('change', { reason, state: getState() });
|
||||
}
|
||||
|
||||
function setPhase(next, reason = next) {
|
||||
if (phase === next) return;
|
||||
phase = next;
|
||||
emitStateChange(reason);
|
||||
}
|
||||
|
||||
function resetMeasurementCycle() {
|
||||
stableSamples = [];
|
||||
emptySince = Date.now();
|
||||
disconnectRequested = false;
|
||||
setPhase('waiting', 'station-empty');
|
||||
}
|
||||
|
||||
function beginTare() {
|
||||
tareSamples = [];
|
||||
stableSamples = [];
|
||||
tare = { topRight: 0, bottomRight: 0, topLeft: 0, bottomLeft: 0 };
|
||||
setPhase('zeroing', 'tare-started');
|
||||
}
|
||||
|
||||
function finishTare() {
|
||||
if (!tareSamples.length) return;
|
||||
const totals = tareSamples.reduce(
|
||||
(sum, sample) => ({
|
||||
topRight: sum.topRight + sample.topRight,
|
||||
bottomRight: sum.bottomRight + sample.bottomRight,
|
||||
topLeft: sum.topLeft + sample.topLeft,
|
||||
bottomLeft: sum.bottomLeft + sample.bottomLeft,
|
||||
}),
|
||||
{ topRight: 0, bottomRight: 0, topLeft: 0, bottomLeft: 0 },
|
||||
);
|
||||
tare = Object.fromEntries(
|
||||
Object.entries(totals).map(([key, value]) => [key, value / tareSamples.length]),
|
||||
);
|
||||
tareSamples = [];
|
||||
resetMeasurementCycle();
|
||||
}
|
||||
|
||||
function isStable(samples) {
|
||||
if (samples.length < 2) return false;
|
||||
const duration = samples[samples.length - 1].ts - samples[0].ts;
|
||||
if (duration < settings.stableDurationMs) return false;
|
||||
const weights = samples.map((sample) => sample.totalKg);
|
||||
if (Math.max(...weights) - Math.min(...weights) > settings.stableToleranceKg) return false;
|
||||
|
||||
const centerXs = samples.map((sample) => sample.center.x);
|
||||
const centerYs = samples.map((sample) => sample.center.y);
|
||||
if (Math.max(...centerXs) - Math.min(...centerXs) > 0.06) return false;
|
||||
if (Math.max(...centerYs) - Math.min(...centerYs) > 0.06) return false;
|
||||
|
||||
// Total weight can remain constant while a rover is still rolling from one
|
||||
// side to the other. Requiring every load cell to settle prevents that motion
|
||||
// from being mistaken for a stable measurement.
|
||||
return ['topRight', 'bottomRight', 'topLeft', 'bottomLeft'].every((corner) => {
|
||||
const values = samples.map((sample) => sample.corners[corner]);
|
||||
return Math.max(...values) - Math.min(...values) <= settings.stableToleranceKg;
|
||||
});
|
||||
}
|
||||
|
||||
function captureMeasurement(frame) {
|
||||
lastMeasurement = {
|
||||
totalKg: frame.totalKg,
|
||||
corners: frame.corners,
|
||||
center: frame.center,
|
||||
capturedAt: frame.ts,
|
||||
};
|
||||
setPhase('captured', 'measurement-captured');
|
||||
publishEvent({
|
||||
source: 'balanceBoard',
|
||||
type: 'balanceBoard.measurement',
|
||||
payload: lastMeasurement,
|
||||
});
|
||||
logger.info('Balance Board measurement captured', {
|
||||
totalKg: lastMeasurement.totalKg,
|
||||
center: lastMeasurement.center,
|
||||
});
|
||||
}
|
||||
|
||||
function processFrame(message = {}) {
|
||||
if (!connected) {
|
||||
connected = true;
|
||||
hardwareState = 'connected';
|
||||
lastError = null;
|
||||
beginTare();
|
||||
emitStateChange('hardware-connected');
|
||||
}
|
||||
|
||||
const rawCorners = normalizeCorners(message.corners);
|
||||
const rawLoad = describeLoad(rawCorners);
|
||||
if (Number.isFinite(Number(message.batteryPercent))) {
|
||||
batteryPercent = Math.max(0, Math.min(100, Number(message.batteryPercent)));
|
||||
}
|
||||
|
||||
if (phase === 'zeroing') {
|
||||
// Power is normally pressed before a rover approaches, making connection
|
||||
// time the safest automatic zero point. Refuse an obviously loaded board so
|
||||
// a rover already parked on it cannot be silently subtracted as the tare.
|
||||
if (rawLoad.totalKg <= MAX_TARE_WEIGHT_KG) tareSamples.push(rawCorners);
|
||||
if (tareSamples.length >= TARE_SAMPLE_COUNT) finishTare();
|
||||
}
|
||||
|
||||
const corners = applyTare(rawCorners);
|
||||
const load = describeLoad(corners);
|
||||
const frame = {
|
||||
ts: Date.now(),
|
||||
corners: Object.fromEntries(Object.entries(corners).map(([key, value]) => [key, round(value, 2)])),
|
||||
totalKg: load.totalKg,
|
||||
center: load.center,
|
||||
batteryPercent,
|
||||
phase,
|
||||
};
|
||||
latestFrame = frame;
|
||||
io.to(FRAME_ROOM).emit('balanceBoard:frame', frame);
|
||||
|
||||
if (phase === 'zeroing') return;
|
||||
if (frame.totalKg <= settings.exitWeightKg) {
|
||||
if (phase !== 'waiting') resetMeasurementCycle();
|
||||
if (!emptySince) emptySince = frame.ts;
|
||||
if (
|
||||
settings.disconnectWhenEmptyMs > 0 &&
|
||||
!disconnectRequested &&
|
||||
frame.ts - emptySince >= settings.disconnectWhenEmptyMs
|
||||
) {
|
||||
// `disconnect` is advisory: genuine boards normally power down after the
|
||||
// HID connection closes. If a firmware clone ignores it, the station
|
||||
// remains safe and simply continues reporting an empty connected board.
|
||||
disconnectRequested = true;
|
||||
try {
|
||||
hardware?.send('disconnect');
|
||||
} catch (err) {
|
||||
logger.warn('Failed to request Balance Board idle disconnect', err.message);
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
emptySince = null;
|
||||
if (phase === 'captured') return;
|
||||
if (frame.totalKg < settings.minimumWeightKg) {
|
||||
stableSamples = [];
|
||||
setPhase('entering', 'load-entering');
|
||||
return;
|
||||
}
|
||||
|
||||
if (phase !== 'stabilizing') setPhase('stabilizing', 'load-detected');
|
||||
stableSamples.push({
|
||||
ts: frame.ts,
|
||||
totalKg: frame.totalKg,
|
||||
center: frame.center,
|
||||
corners: frame.corners,
|
||||
});
|
||||
// Retain one frame of scheduling slack. If we removed everything older than
|
||||
// the exact window first, a 20 Hz stream would usually keep only 1450 ms of
|
||||
// history and could therefore approach but never satisfy a 1500 ms window.
|
||||
const cutoff = frame.ts - settings.stableDurationMs - 100;
|
||||
stableSamples = stableSamples.filter((sample) => sample.ts >= cutoff);
|
||||
if (isStable(stableSamples)) captureMeasurement(frame);
|
||||
}
|
||||
|
||||
function handleWorkerMessage(message = {}) {
|
||||
if (message.type === 'frame') {
|
||||
processFrame(message);
|
||||
return;
|
||||
}
|
||||
if (message.type === 'paired') {
|
||||
const address = typeof message.address === 'string' ? message.address.trim().toUpperCase() : '';
|
||||
if (address && address !== store.address) {
|
||||
store.address = address;
|
||||
persistStore();
|
||||
}
|
||||
hardware?.setAddress(address);
|
||||
hardwareState = 'waiting';
|
||||
lastError = null;
|
||||
setPhase('waiting', 'board-paired');
|
||||
emitStateChange('board-paired');
|
||||
return;
|
||||
}
|
||||
if (message.type !== 'status') return;
|
||||
|
||||
hardwareState = String(message.state || 'unknown');
|
||||
lastError = message.error ? String(message.error) : null;
|
||||
if (hardwareState === 'connected') {
|
||||
if (!connected) {
|
||||
connected = true;
|
||||
beginTare();
|
||||
}
|
||||
} else {
|
||||
connected = false;
|
||||
latestFrame = null;
|
||||
tareSamples = [];
|
||||
stableSamples = [];
|
||||
if (hardwareState === 'commissioning' || hardwareState === 'pairing') {
|
||||
setPhase(hardwareState, hardwareState);
|
||||
} else if (hardwareState === 'error') {
|
||||
setPhase('error', 'hardware-error');
|
||||
} else {
|
||||
setPhase(store.address || settings.simulate ? 'waiting' : 'commissioning', 'hardware-waiting');
|
||||
}
|
||||
}
|
||||
emitStateChange('hardware-status');
|
||||
}
|
||||
|
||||
function requireAdmin(socket) {
|
||||
if (!isAdmin(socket)) throw new Error('Not authorized for Balance Board maintenance');
|
||||
if (!enabled) throw new Error('Balance Board support is disabled');
|
||||
}
|
||||
|
||||
io.on('connection', (socket) => {
|
||||
socket.on('balanceBoard:subscribe', (_payload = {}, cb = () => {}) => {
|
||||
socket.join(FRAME_ROOM);
|
||||
if (latestFrame) socket.emit('balanceBoard:frame', latestFrame);
|
||||
cb({ success: true });
|
||||
});
|
||||
socket.on('balanceBoard:unsubscribe', () => socket.leave(FRAME_ROOM));
|
||||
|
||||
socket.on('balanceBoard:tare', (_payload = {}, cb = () => {}) => {
|
||||
try {
|
||||
requireAdmin(socket);
|
||||
if (!connected) throw new Error('Balance Board is not connected');
|
||||
beginTare();
|
||||
cb({ success: true });
|
||||
} catch (err) {
|
||||
cb({ error: err.message });
|
||||
}
|
||||
});
|
||||
|
||||
socket.on('balanceBoard:pair', (_payload = {}, cb = () => {}) => {
|
||||
try {
|
||||
requireAdmin(socket);
|
||||
hardware?.send('pair');
|
||||
store.address = '';
|
||||
hardware?.setAddress('');
|
||||
persistStore();
|
||||
setPhase('commissioning', 'pairing-requested');
|
||||
cb({ success: true });
|
||||
} catch (err) {
|
||||
cb({ error: err.message });
|
||||
}
|
||||
});
|
||||
|
||||
socket.on('balanceBoard:forget', (_payload = {}, cb = () => {}) => {
|
||||
try {
|
||||
requireAdmin(socket);
|
||||
hardware?.send('forget');
|
||||
store.address = '';
|
||||
hardware?.setAddress('');
|
||||
persistStore();
|
||||
setPhase('commissioning', 'board-forgotten');
|
||||
cb({ success: true });
|
||||
} catch (err) {
|
||||
cb({ error: err.message });
|
||||
}
|
||||
});
|
||||
|
||||
socket.on('balanceBoard:restart', (_payload = {}, cb = () => {}) => {
|
||||
try {
|
||||
requireAdmin(socket);
|
||||
hardware?.restart();
|
||||
hardwareState = 'starting';
|
||||
emitStateChange('worker-restarted');
|
||||
cb({ success: true });
|
||||
} catch (err) {
|
||||
cb({ error: err.message });
|
||||
}
|
||||
});
|
||||
});
|
||||
|
||||
if (enabled) {
|
||||
hardware = createBalanceBoardHardware({
|
||||
logger,
|
||||
address: store.address,
|
||||
simulate: settings.simulate,
|
||||
});
|
||||
hardware.events.on('message', handleWorkerMessage);
|
||||
hardware.start();
|
||||
} else {
|
||||
logger.info('Balance Board disabled by config');
|
||||
}
|
||||
|
||||
function installShutdownHooks() {
|
||||
const shutdown = () => hardware?.stop();
|
||||
process.once('exit', shutdown);
|
||||
process.once('SIGINT', shutdown);
|
||||
process.once('SIGTERM', shutdown);
|
||||
}
|
||||
|
||||
installShutdownHooks();
|
||||
|
||||
module.exports = {
|
||||
getState,
|
||||
balanceBoardEvents: events,
|
||||
};
|
||||
@@ -0,0 +1,21 @@
|
||||
CXX ?= g++
|
||||
|
||||
# The bridge deliberately uses the stable Linux input and Bluetooth management
|
||||
# ABIs directly. Keeping it free of third-party libraries makes installation on
|
||||
# the Fedora server predictable and avoids binding the rover service to an old
|
||||
# Wii-specific userspace package.
|
||||
CXXFLAGS ?= -O2 -std=c++17 -Wall -Wextra -pedantic
|
||||
LDLIBS += -pthread
|
||||
|
||||
TARGET := balance_board_worker
|
||||
SRC := balance_board_worker.cpp
|
||||
|
||||
.PHONY: all clean
|
||||
|
||||
all: $(TARGET)
|
||||
|
||||
$(TARGET): $(SRC)
|
||||
$(CXX) $(CXXFLAGS) -o $@ $< $(LDLIBS)
|
||||
|
||||
clean:
|
||||
rm -f $(TARGET)
|
||||
@@ -0,0 +1,683 @@
|
||||
// Wii Balance Board native bridge.
|
||||
//
|
||||
// Purpose:
|
||||
// Pair one original Nintendo RVL-WBC-01 through modern BlueZ, then expose the
|
||||
// calibrated Linux input readings as newline-delimited JSON for the Node server.
|
||||
//
|
||||
// Why this process exists:
|
||||
// The Linux hid-wiimote driver already performs the board-specific calibration,
|
||||
// but BlueZ removed its Wii PIN helper in 2025. A Wii device expects six raw PIN
|
||||
// bytes equal to the host Bluetooth adapter address in wire order. D-Bus represents PINs
|
||||
// as UTF-8 strings and cannot safely carry arbitrary bytes, so this bridge races
|
||||
// BlueZ's agent response with the correct raw MGMT_OP_PIN_CODE_REPLY. Only the
|
||||
// board currently being commissioned is eligible for that reply.
|
||||
//
|
||||
// Security boundary:
|
||||
// The installed binary receives CAP_NET_ADMIN solely to open the Bluetooth
|
||||
// management socket. The much larger Node server remains unprivileged. Normal
|
||||
// sensor access happens through a narrowly scoped udev rule.
|
||||
|
||||
#include <linux/input.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <atomic>
|
||||
#include <cerrno>
|
||||
#include <chrono>
|
||||
#include <csignal>
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <dirent.h>
|
||||
#include <fcntl.h>
|
||||
#include <fstream>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <poll.h>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/wait.h>
|
||||
#include <thread>
|
||||
#include <unistd.h>
|
||||
#include <vector>
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr const char* kBoardBluetoothName = "Nintendo RVL-WBC-01";
|
||||
constexpr const char* kBoardInputName = "Nintendo Wii Remote Balance Board";
|
||||
constexpr int kBluetoothProtocolHci = 1;
|
||||
constexpr uint16_t kHciChannelControl = 3;
|
||||
constexpr uint16_t kHciDeviceNone = 0xffff;
|
||||
constexpr uint16_t kMgmtPinCodeRequestEvent = 0x000e;
|
||||
constexpr uint16_t kMgmtPinCodeReplyCommand = 0x0016;
|
||||
constexpr uint8_t kBluetoothClassicAddressType = 0;
|
||||
constexpr int kFrameIntervalMs = 50;
|
||||
constexpr int kDeviceScanIntervalMs = 500;
|
||||
constexpr int kCommissionRetryMs = 12000;
|
||||
constexpr int kBatteryRefreshMs = 5000;
|
||||
|
||||
std::atomic<bool> running{true};
|
||||
std::mutex output_mutex;
|
||||
|
||||
struct BluetoothAddress {
|
||||
std::string display;
|
||||
// The kernel Bluetooth management API carries addresses least-significant
|
||||
// byte first. These exact six bytes are also the Wii pairing PIN.
|
||||
std::array<uint8_t, 6> wire{};
|
||||
};
|
||||
|
||||
struct PairingSharedState {
|
||||
std::mutex mutex;
|
||||
std::optional<BluetoothAddress> active_target;
|
||||
std::optional<BluetoothAddress> active_pin;
|
||||
std::optional<std::string> commissioned_address;
|
||||
bool commissioning = false;
|
||||
bool pairing_available = true;
|
||||
};
|
||||
|
||||
struct BoardReadings {
|
||||
int top_right = 0;
|
||||
int bottom_right = 0;
|
||||
int top_left = 0;
|
||||
int bottom_left = 0;
|
||||
};
|
||||
|
||||
struct CommandResult {
|
||||
int exit_code = -1;
|
||||
std::string output;
|
||||
};
|
||||
|
||||
uint64_t monotonic_ms() {
|
||||
using namespace std::chrono;
|
||||
return duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();
|
||||
}
|
||||
|
||||
std::string json_escape(const std::string& value) {
|
||||
std::ostringstream out;
|
||||
for (unsigned char ch : value) {
|
||||
switch (ch) {
|
||||
case '\\': out << "\\\\"; break;
|
||||
case '"': out << "\\\""; break;
|
||||
case '\n': out << "\\n"; break;
|
||||
case '\r': out << "\\r"; break;
|
||||
case '\t': out << "\\t"; break;
|
||||
default:
|
||||
if (ch < 0x20) {
|
||||
out << "\\u" << std::hex << std::setw(4) << std::setfill('0')
|
||||
<< static_cast<int>(ch) << std::dec;
|
||||
} else {
|
||||
out << static_cast<char>(ch);
|
||||
}
|
||||
}
|
||||
}
|
||||
return out.str();
|
||||
}
|
||||
|
||||
void emit_json(const std::string& fields) {
|
||||
// Pairing and input monitoring run on separate threads. Serialize complete
|
||||
// lines so two status changes can never interleave and corrupt Node's parser.
|
||||
std::lock_guard<std::mutex> lock(output_mutex);
|
||||
std::cout << "{" << fields << "}\n";
|
||||
std::cout.flush();
|
||||
}
|
||||
|
||||
void emit_status(const std::string& state, const std::string& address = "",
|
||||
const std::string& error = "") {
|
||||
std::ostringstream fields;
|
||||
fields << "\"type\":\"status\",\"state\":\"" << json_escape(state) << "\"";
|
||||
if (!address.empty()) fields << ",\"address\":\"" << json_escape(address) << "\"";
|
||||
if (!error.empty()) fields << ",\"error\":\"" << json_escape(error) << "\"";
|
||||
emit_json(fields.str());
|
||||
}
|
||||
|
||||
void emit_frame(const BoardReadings& readings, std::optional<int> battery_percent = std::nullopt) {
|
||||
std::ostringstream fields;
|
||||
fields << "\"type\":\"frame\",\"corners\":{"
|
||||
<< "\"topRight\":" << readings.top_right << ","
|
||||
<< "\"bottomRight\":" << readings.bottom_right << ","
|
||||
<< "\"topLeft\":" << readings.top_left << ","
|
||||
<< "\"bottomLeft\":" << readings.bottom_left << "}";
|
||||
if (battery_percent.has_value()) fields << ",\"batteryPercent\":" << *battery_percent;
|
||||
emit_json(fields.str());
|
||||
}
|
||||
|
||||
std::optional<int> read_board_battery() {
|
||||
static uint64_t last_read_at = 0;
|
||||
static std::optional<int> cached;
|
||||
const uint64_t now = monotonic_ms();
|
||||
if (now - last_read_at < kBatteryRefreshMs) return cached;
|
||||
last_read_at = now;
|
||||
|
||||
DIR* directory = opendir("/sys/class/power_supply");
|
||||
if (!directory) return cached;
|
||||
while (dirent* entry = readdir(directory)) {
|
||||
if (std::strncmp(entry->d_name, "wiimote_battery_", 16) != 0) continue;
|
||||
std::ifstream capacity(std::string("/sys/class/power_supply/") + entry->d_name + "/capacity");
|
||||
int value = -1;
|
||||
if (capacity >> value) cached = std::max(0, std::min(100, value));
|
||||
break;
|
||||
}
|
||||
closedir(directory);
|
||||
return cached;
|
||||
}
|
||||
|
||||
void signal_handler(int) {
|
||||
running.store(false);
|
||||
}
|
||||
|
||||
std::optional<BluetoothAddress> parse_address(const std::string& raw) {
|
||||
std::array<unsigned int, 6> bytes{};
|
||||
if (std::sscanf(raw.c_str(), "%2x:%2x:%2x:%2x:%2x:%2x",
|
||||
&bytes[0], &bytes[1], &bytes[2], &bytes[3], &bytes[4], &bytes[5]) != 6) {
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
BluetoothAddress address;
|
||||
char normalized[18]{};
|
||||
std::snprintf(normalized, sizeof(normalized), "%02X:%02X:%02X:%02X:%02X:%02X",
|
||||
bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5]);
|
||||
address.display = normalized;
|
||||
for (std::size_t i = 0; i < address.wire.size(); ++i) {
|
||||
address.wire[i] = static_cast<uint8_t>(bytes[address.wire.size() - 1 - i]);
|
||||
}
|
||||
return address;
|
||||
}
|
||||
|
||||
CommandResult run_command(const std::vector<std::string>& args) {
|
||||
CommandResult result;
|
||||
if (args.empty()) return result;
|
||||
|
||||
int pipe_fds[2]{};
|
||||
if (pipe(pipe_fds) != 0) {
|
||||
result.output = std::strerror(errno);
|
||||
return result;
|
||||
}
|
||||
|
||||
const pid_t pid = fork();
|
||||
if (pid == 0) {
|
||||
dup2(pipe_fds[1], STDOUT_FILENO);
|
||||
dup2(pipe_fds[1], STDERR_FILENO);
|
||||
close(pipe_fds[0]);
|
||||
close(pipe_fds[1]);
|
||||
|
||||
std::vector<char*> argv;
|
||||
argv.reserve(args.size() + 1);
|
||||
for (const auto& arg : args) argv.push_back(const_cast<char*>(arg.c_str()));
|
||||
argv.push_back(nullptr);
|
||||
execvp(argv[0], argv.data());
|
||||
_exit(127);
|
||||
}
|
||||
|
||||
close(pipe_fds[1]);
|
||||
if (pid < 0) {
|
||||
close(pipe_fds[0]);
|
||||
result.output = std::strerror(errno);
|
||||
return result;
|
||||
}
|
||||
|
||||
std::array<char, 1024> buffer{};
|
||||
ssize_t count = 0;
|
||||
while ((count = read(pipe_fds[0], buffer.data(), buffer.size())) > 0) {
|
||||
result.output.append(buffer.data(), static_cast<std::size_t>(count));
|
||||
}
|
||||
close(pipe_fds[0]);
|
||||
|
||||
int status = 0;
|
||||
while (waitpid(pid, &status, 0) < 0 && errno == EINTR) {}
|
||||
if (WIFEXITED(status)) result.exit_code = WEXITSTATUS(status);
|
||||
return result;
|
||||
}
|
||||
|
||||
std::optional<BluetoothAddress> find_cached_board() {
|
||||
const CommandResult devices = run_command({"bluetoothctl", "devices"});
|
||||
std::istringstream lines(devices.output);
|
||||
std::string line;
|
||||
while (std::getline(lines, line)) {
|
||||
// BlueZ prints `Device AA:BB:CC:DD:EE:FF Nintendo RVL-WBC-01`.
|
||||
// Match the complete board name so a nearby Wiimote is never eligible for
|
||||
// the raw PIN response or accidentally stored as the weigh station.
|
||||
if (line.find(kBoardBluetoothName) == std::string::npos) continue;
|
||||
const std::size_t device_prefix = line.find("Device ");
|
||||
if (device_prefix == std::string::npos || line.size() < device_prefix + 24) continue;
|
||||
const std::string raw_address = line.substr(device_prefix + 7, 17);
|
||||
if (auto address = parse_address(raw_address)) return address;
|
||||
}
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
std::optional<BluetoothAddress> find_default_controller() {
|
||||
const CommandResult controller = run_command({"bluetoothctl", "show"});
|
||||
std::istringstream lines(controller.output);
|
||||
std::string line;
|
||||
while (std::getline(lines, line)) {
|
||||
const std::size_t controller_prefix = line.find("Controller ");
|
||||
if (controller_prefix == std::string::npos || line.size() < controller_prefix + 28) continue;
|
||||
if (auto address = parse_address(line.substr(controller_prefix + 11, 17))) return address;
|
||||
}
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
bool command_succeeded(const CommandResult& result) {
|
||||
if (result.exit_code != 0) return false;
|
||||
return result.output.find("Failed") == std::string::npos &&
|
||||
result.output.find("not available") == std::string::npos;
|
||||
}
|
||||
|
||||
void prepare_known_device(const BluetoothAddress& address) {
|
||||
run_command({"bluetoothctl", "--timeout", "8", "trust", address.display});
|
||||
// WakeAllowed tells current BlueZ releases to accept the board's incoming HID
|
||||
// connection after its front power button is pressed. Older releases may not
|
||||
// implement the command; trust + the stored link key still remain effective.
|
||||
run_command({"bluetoothctl", "--timeout", "8", "wake", address.display, "on"});
|
||||
}
|
||||
|
||||
void trust_and_connect(const BluetoothAddress& address) {
|
||||
prepare_known_device(address);
|
||||
// A board remains awake for only a short window after Sync. Connecting the
|
||||
// HID profile immediately is what teaches it to initiate future connections
|
||||
// when its front power button is pressed.
|
||||
run_command({"bluetoothctl", "--timeout", "8", "connect", address.display});
|
||||
}
|
||||
|
||||
void commissioning_loop(PairingSharedState* shared) {
|
||||
while (running.load()) {
|
||||
bool should_commission = false;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(shared->mutex);
|
||||
should_commission = shared->commissioning && !shared->commissioned_address.has_value();
|
||||
}
|
||||
|
||||
if (!should_commission) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(250));
|
||||
continue;
|
||||
}
|
||||
|
||||
emit_status("commissioning");
|
||||
// Discovery is deliberately bounded rather than permanently enabled. Short
|
||||
// BR/EDR-only windows are enough for the red Sync button while minimizing
|
||||
// interference with other Bluetooth equipment on the server.
|
||||
run_command({"bluetoothctl", "--timeout", "8", "scan", "bredr"});
|
||||
auto address = find_cached_board();
|
||||
if (!address.has_value()) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(kCommissionRetryMs));
|
||||
continue;
|
||||
}
|
||||
|
||||
const auto controller = find_default_controller();
|
||||
if (!controller.has_value()) {
|
||||
emit_status("commissioning", address->display,
|
||||
"no powered Bluetooth controller is available for pairing");
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(kCommissionRetryMs));
|
||||
continue;
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(shared->mutex);
|
||||
shared->active_target = address;
|
||||
// Red-Sync commissioning stores the host as the board's future reconnect
|
||||
// target. BlueZ's retired wiimote plugin therefore used the local adapter
|
||||
// address—not the board address—as the six raw PIN bytes.
|
||||
shared->active_pin = controller;
|
||||
}
|
||||
emit_status("pairing", address->display);
|
||||
|
||||
// The management-socket listener answers the PIN request while this command
|
||||
// keeps BlueZ's normal device, SDP, bonding, and input-profile machinery in
|
||||
// charge of everything else.
|
||||
const CommandResult pair_result = run_command({
|
||||
"bluetoothctl", "--timeout", "12", "--agent", "NoInputNoOutput", "pair", address->display});
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(shared->mutex);
|
||||
shared->active_target.reset();
|
||||
shared->active_pin.reset();
|
||||
}
|
||||
|
||||
if (!command_succeeded(pair_result)) {
|
||||
emit_status("commissioning", address->display,
|
||||
"pairing failed; press the red Sync button and try again");
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(kCommissionRetryMs));
|
||||
continue;
|
||||
}
|
||||
|
||||
trust_and_connect(*address);
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(shared->mutex);
|
||||
shared->commissioned_address = address->display;
|
||||
shared->commissioning = false;
|
||||
}
|
||||
emit_json("\"type\":\"paired\",\"address\":\"" + json_escape(address->display) + "\"");
|
||||
emit_status("waiting", address->display);
|
||||
}
|
||||
}
|
||||
|
||||
#pragma pack(push, 1)
|
||||
struct SockaddrHci {
|
||||
uint16_t family;
|
||||
uint16_t device;
|
||||
uint16_t channel;
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
int open_management_socket() {
|
||||
const int fd = socket(AF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC | SOCK_NONBLOCK,
|
||||
kBluetoothProtocolHci);
|
||||
if (fd < 0) return -1;
|
||||
|
||||
const SockaddrHci address{
|
||||
static_cast<uint16_t>(AF_BLUETOOTH), kHciDeviceNone, kHciChannelControl};
|
||||
if (bind(fd, reinterpret_cast<const sockaddr*>(&address), sizeof(address)) != 0) {
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
return fd;
|
||||
}
|
||||
|
||||
void write_u16_le(uint8_t* output, uint16_t value) {
|
||||
output[0] = static_cast<uint8_t>(value & 0xff);
|
||||
output[1] = static_cast<uint8_t>((value >> 8) & 0xff);
|
||||
}
|
||||
|
||||
void answer_pin_request(int fd, uint16_t adapter_index,
|
||||
const BluetoothAddress& target,
|
||||
const BluetoothAddress& pin) {
|
||||
// Packet layout is a six-byte mgmt header followed by mgmt_addr_info,
|
||||
// pin_len, and the fixed sixteen-byte PIN buffer. Serializing by hand avoids
|
||||
// compiler padding and documents every privileged byte sent to the kernel.
|
||||
constexpr std::size_t header_size = 6;
|
||||
constexpr std::size_t payload_size = 7 + 1 + 16;
|
||||
std::array<uint8_t, header_size + payload_size> packet{};
|
||||
write_u16_le(packet.data(), kMgmtPinCodeReplyCommand);
|
||||
write_u16_le(packet.data() + 2, adapter_index);
|
||||
write_u16_le(packet.data() + 4, payload_size);
|
||||
std::copy(target.wire.begin(), target.wire.end(), packet.begin() + header_size);
|
||||
packet[header_size + 6] = kBluetoothClassicAddressType;
|
||||
packet[header_size + 7] = 6;
|
||||
std::copy(pin.wire.begin(), pin.wire.end(), packet.begin() + header_size + 8);
|
||||
if (write(fd, packet.data(), packet.size()) != static_cast<ssize_t>(packet.size())) {
|
||||
emit_status("error", target.display, "failed to answer the Wii pairing PIN request");
|
||||
}
|
||||
}
|
||||
|
||||
void process_management_events(int fd, PairingSharedState* shared) {
|
||||
if (fd < 0) return;
|
||||
std::array<uint8_t, 1024> buffer{};
|
||||
ssize_t count = 0;
|
||||
while ((count = read(fd, buffer.data(), buffer.size())) > 0) {
|
||||
if (count < 14) continue;
|
||||
const uint16_t event = static_cast<uint16_t>(buffer[0] | (buffer[1] << 8));
|
||||
const uint16_t adapter_index = static_cast<uint16_t>(buffer[2] | (buffer[3] << 8));
|
||||
const uint16_t payload_size = static_cast<uint16_t>(buffer[4] | (buffer[5] << 8));
|
||||
if (event != kMgmtPinCodeRequestEvent || payload_size < 8 || count < 6 + payload_size) continue;
|
||||
|
||||
std::optional<BluetoothAddress> target;
|
||||
std::optional<BluetoothAddress> pin;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(shared->mutex);
|
||||
target = shared->active_target;
|
||||
pin = shared->active_pin;
|
||||
}
|
||||
if (!target.has_value() || !pin.has_value() ||
|
||||
!std::equal(target->wire.begin(), target->wire.end(), buffer.begin() + 6)) {
|
||||
continue;
|
||||
}
|
||||
answer_pin_request(fd, adapter_index, *target, *pin);
|
||||
}
|
||||
}
|
||||
|
||||
std::optional<std::string> find_board_input_path() {
|
||||
DIR* directory = opendir("/dev/input");
|
||||
if (!directory) return std::nullopt;
|
||||
|
||||
std::optional<std::string> found;
|
||||
while (dirent* entry = readdir(directory)) {
|
||||
if (std::strncmp(entry->d_name, "event", 5) != 0) continue;
|
||||
const std::string path = std::string("/dev/input/") + entry->d_name;
|
||||
const int fd = open(path.c_str(), O_RDONLY | O_NONBLOCK | O_CLOEXEC);
|
||||
if (fd < 0) continue;
|
||||
std::array<char, 256> name{};
|
||||
if (ioctl(fd, EVIOCGNAME(name.size()), name.data()) >= 0 &&
|
||||
std::string(name.data()) == kBoardInputName) {
|
||||
found = path;
|
||||
close(fd);
|
||||
break;
|
||||
}
|
||||
close(fd);
|
||||
}
|
||||
closedir(directory);
|
||||
return found;
|
||||
}
|
||||
|
||||
void read_initial_axis(int fd, unsigned int axis, int* destination) {
|
||||
input_absinfo info{};
|
||||
if (ioctl(fd, EVIOCGABS(axis), &info) == 0) *destination = std::max(0, info.value);
|
||||
}
|
||||
|
||||
int open_board_input(const std::string& path, BoardReadings* readings) {
|
||||
const int fd = open(path.c_str(), O_RDONLY | O_NONBLOCK | O_CLOEXEC);
|
||||
if (fd < 0) return -1;
|
||||
// hid-wiimote applies factory calibration before these values reach evdev.
|
||||
// Reading the current axes prevents the first JSON frame from showing three
|
||||
// zero corners merely because only one axis changed after the file was opened.
|
||||
read_initial_axis(fd, ABS_HAT0X, &readings->top_right);
|
||||
read_initial_axis(fd, ABS_HAT0Y, &readings->bottom_right);
|
||||
read_initial_axis(fd, ABS_HAT1X, &readings->top_left);
|
||||
read_initial_axis(fd, ABS_HAT1Y, &readings->bottom_left);
|
||||
return fd;
|
||||
}
|
||||
|
||||
bool process_input_events(int fd, BoardReadings* readings, uint64_t* last_frame_at) {
|
||||
std::array<input_event, 64> events{};
|
||||
const ssize_t bytes = read(fd, events.data(), sizeof(events));
|
||||
if (bytes == 0) return false;
|
||||
if (bytes < 0) return errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR;
|
||||
|
||||
const std::size_t count = static_cast<std::size_t>(bytes) / sizeof(input_event);
|
||||
bool synchronized = false;
|
||||
for (std::size_t i = 0; i < count; ++i) {
|
||||
const input_event& event = events[i];
|
||||
if (event.type == EV_ABS) {
|
||||
const int value = std::max(0, event.value);
|
||||
if (event.code == ABS_HAT0X) readings->top_right = value;
|
||||
if (event.code == ABS_HAT0Y) readings->bottom_right = value;
|
||||
if (event.code == ABS_HAT1X) readings->top_left = value;
|
||||
if (event.code == ABS_HAT1Y) readings->bottom_left = value;
|
||||
} else if (event.type == EV_SYN && event.code == SYN_REPORT) {
|
||||
synchronized = true;
|
||||
}
|
||||
}
|
||||
|
||||
const uint64_t now = monotonic_ms();
|
||||
if (synchronized && now - *last_frame_at >= kFrameIntervalMs) {
|
||||
// Reading capacity asks hid-wiimote for a fresh status report, so cache it
|
||||
// for several seconds instead of injecting a Bluetooth command per frame.
|
||||
emit_frame(*readings, read_board_battery());
|
||||
*last_frame_at = now;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
std::optional<std::string> extract_command_value(const std::string& line, const std::string& key) {
|
||||
const std::string token = "\"" + key + "\"";
|
||||
const std::size_t key_at = line.find(token);
|
||||
if (key_at == std::string::npos) return std::nullopt;
|
||||
const std::size_t colon = line.find(':', key_at + token.size());
|
||||
const std::size_t first_quote = line.find('"', colon + 1);
|
||||
const std::size_t second_quote = line.find('"', first_quote + 1);
|
||||
if (colon == std::string::npos || first_quote == std::string::npos || second_quote == std::string::npos) {
|
||||
return std::nullopt;
|
||||
}
|
||||
return line.substr(first_quote + 1, second_quote - first_quote - 1);
|
||||
}
|
||||
|
||||
void handle_command(const std::string& line, PairingSharedState* shared) {
|
||||
const std::string command = extract_command_value(line, "command").value_or("");
|
||||
if (command == "pair") {
|
||||
std::lock_guard<std::mutex> lock(shared->mutex);
|
||||
if (!shared->pairing_available) {
|
||||
emit_status("error", "", "Bluetooth pairing capability is unavailable; reinstall the bridge capability");
|
||||
return;
|
||||
}
|
||||
shared->commissioned_address.reset();
|
||||
shared->commissioning = true;
|
||||
} else if (command == "forget") {
|
||||
std::optional<std::string> address;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(shared->mutex);
|
||||
if (!shared->pairing_available) {
|
||||
emit_status("error", address.value_or(""),
|
||||
"cannot forget the board while Bluetooth pairing capability is unavailable");
|
||||
return;
|
||||
}
|
||||
address = shared->commissioned_address;
|
||||
shared->commissioned_address.reset();
|
||||
// Do not let the discovery loop race the BlueZ removal. It may otherwise
|
||||
// rediscover and attempt to pair the still-bonded object before `remove`
|
||||
// has finished deleting its keys and cached SDP record.
|
||||
shared->commissioning = false;
|
||||
}
|
||||
if (address.has_value()) run_command({"bluetoothctl", "--timeout", "8", "remove", *address});
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(shared->mutex);
|
||||
shared->commissioning = true;
|
||||
}
|
||||
emit_status("commissioning");
|
||||
} else if (command == "disconnect") {
|
||||
std::optional<std::string> address;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(shared->mutex);
|
||||
address = shared->commissioned_address;
|
||||
}
|
||||
// Idle disconnect is intentionally host-initiated only after the server has
|
||||
// observed an empty station for its configured delay. The bond remains, so
|
||||
// the next front power-button press still reconnects without commissioning.
|
||||
if (address.has_value()) {
|
||||
run_command({"bluetoothctl", "--timeout", "8", "disconnect", *address});
|
||||
}
|
||||
} else if (command == "stop") {
|
||||
running.store(false);
|
||||
}
|
||||
}
|
||||
|
||||
void stdin_loop(PairingSharedState* shared) {
|
||||
std::string line;
|
||||
while (running.load() && std::getline(std::cin, line)) handle_command(line, shared);
|
||||
}
|
||||
|
||||
void simulated_loop() {
|
||||
emit_status("waiting", "SIMULATED");
|
||||
const std::array<BoardReadings, 12> sequence{{
|
||||
{0, 0, 0, 0}, {0, 0, 0, 0}, {40, 30, 35, 25}, {95, 82, 90, 76},
|
||||
{103, 97, 101, 99}, {104, 98, 101, 99}, {103, 98, 102, 99},
|
||||
{103, 98, 101, 100}, {104, 98, 101, 99}, {75, 65, 70, 60},
|
||||
{20, 12, 15, 10}, {0, 0, 0, 0},
|
||||
}};
|
||||
while (running.load()) {
|
||||
emit_status("connected", "SIMULATED");
|
||||
for (const auto& readings : sequence) {
|
||||
for (int frame = 0; frame < 12 && running.load(); ++frame) {
|
||||
emit_frame(readings, 82);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(kFrameIntervalMs));
|
||||
}
|
||||
}
|
||||
emit_status("waiting", "SIMULATED");
|
||||
for (int pause = 0; pause < 30 && running.load(); ++pause) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main() {
|
||||
std::signal(SIGINT, signal_handler);
|
||||
std::signal(SIGTERM, signal_handler);
|
||||
|
||||
const std::string simulation = std::getenv("BALANCE_BOARD_SIMULATE")
|
||||
? std::getenv("BALANCE_BOARD_SIMULATE") : "";
|
||||
if (simulation == "1" || simulation == "true" || simulation == "cycle") {
|
||||
simulated_loop();
|
||||
return 0;
|
||||
}
|
||||
|
||||
PairingSharedState pairing;
|
||||
const std::string configured_address = std::getenv("BALANCE_BOARD_ADDRESS")
|
||||
? std::getenv("BALANCE_BOARD_ADDRESS") : "";
|
||||
if (auto parsed = parse_address(configured_address)) {
|
||||
pairing.commissioned_address = parsed->display;
|
||||
// A sleeping commissioned board is expected at server startup. Trust and
|
||||
// wake policy are idempotent, but do not page the sleeping device or delay
|
||||
// startup; its front power button will initiate the actual HID connection.
|
||||
prepare_known_device(*parsed);
|
||||
} else {
|
||||
pairing.commissioning = true;
|
||||
}
|
||||
|
||||
const int management_fd = open_management_socket();
|
||||
if (management_fd < 0) {
|
||||
pairing.pairing_available = false;
|
||||
if (!pairing.commissioned_address.has_value()) {
|
||||
// An already bonded board can reconnect and stream through evdev without
|
||||
// the management socket. Missing capability is fatal only when the bridge
|
||||
// actually needs to create a new bond.
|
||||
pairing.commissioning = false;
|
||||
emit_status("error", configured_address,
|
||||
"Bluetooth management socket unavailable; install the worker capability");
|
||||
}
|
||||
}
|
||||
|
||||
std::thread commission_thread(commissioning_loop, &pairing);
|
||||
std::thread input_thread(stdin_loop, &pairing);
|
||||
if (management_fd >= 0 || pairing.commissioned_address.has_value()) {
|
||||
emit_status(pairing.commissioning ? "commissioning" : "waiting", configured_address);
|
||||
}
|
||||
|
||||
int input_fd = -1;
|
||||
BoardReadings readings;
|
||||
uint64_t last_device_scan_at = 0;
|
||||
uint64_t last_frame_at = 0;
|
||||
|
||||
while (running.load()) {
|
||||
process_management_events(management_fd, &pairing);
|
||||
|
||||
if (input_fd < 0 && monotonic_ms() - last_device_scan_at >= kDeviceScanIntervalMs) {
|
||||
last_device_scan_at = monotonic_ms();
|
||||
if (auto path = find_board_input_path()) {
|
||||
input_fd = open_board_input(*path, &readings);
|
||||
if (input_fd >= 0) {
|
||||
std::string address;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(pairing.mutex);
|
||||
address = pairing.commissioned_address.value_or("");
|
||||
}
|
||||
emit_status("connected", address);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (input_fd >= 0 && !process_input_events(input_fd, &readings, &last_frame_at)) {
|
||||
close(input_fd);
|
||||
input_fd = -1;
|
||||
std::string address;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(pairing.mutex);
|
||||
address = pairing.commissioned_address.value_or("");
|
||||
}
|
||||
emit_status("waiting", address);
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
|
||||
if (input_fd >= 0) close(input_fd);
|
||||
if (management_fd >= 0) close(management_fd);
|
||||
if (input_thread.joinable()) input_thread.detach();
|
||||
if (commission_thread.joinable()) commission_thread.join();
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user