generated from ztimson/template
517 lines
18 KiB
JavaScript
517 lines
18 KiB
JavaScript
|
|
// Telemetry service
|
|
// * Orientation: supervises the configured Python MPU script and tracks it with an explicit state machine.
|
|
// * Position: talks to gpsd over its JSON TCP protocol (no extra dependency).
|
|
// * Time: the OS clock (already disciplined by RTC/GPS), exposed via now().
|
|
//
|
|
// Sensor state machine
|
|
//
|
|
// stopped ──start()──▶ starting ──first sample──▶ running ◀──sample──┐
|
|
// │ │ │
|
|
// │ exit / no data │ no sample │
|
|
// ▼ ▼ for staleMs │
|
|
// backoff ◀──── exit / stale too long ──────── stale
|
|
// │
|
|
// └─ delay (1s,2s,4s … 30s) ─▶ starting
|
|
//
|
|
// Events: 'orientation', 'sensor-state', 'gps-state', 'gps'
|
|
import { EventEmitter } from 'node:events';
|
|
import { spawn } from 'node:child_process';
|
|
import readline from 'node:readline';
|
|
import net from 'node:net';
|
|
|
|
const SENSOR_TRANSITIONS = {
|
|
stopped: ['starting'],
|
|
starting: ['running', 'backoff', 'stopped'],
|
|
running: ['stale', 'backoff', 'stopped'],
|
|
stale: ['running', 'backoff', 'stopped'],
|
|
backoff: ['starting', 'stopped'],
|
|
};
|
|
|
|
const isFiniteNum = (v) => typeof v === 'number' && Number.isFinite(v);
|
|
|
|
function eulerToQuat(yaw, pitch, roll) {
|
|
const y = (yaw * Math.PI) / 360;
|
|
const p = (pitch * Math.PI) / 360;
|
|
const r = (roll * Math.PI) / 360;
|
|
const cy = Math.cos(y), sy = Math.sin(y);
|
|
const cp = Math.cos(p), sp = Math.sin(p);
|
|
const cr = Math.cos(r), sr = Math.sin(r);
|
|
return [
|
|
sr * cp * cy - cr * sp * sy,
|
|
cr * sp * cy + sr * cp * sy,
|
|
cr * cp * sy - sr * sp * cy,
|
|
cr * cp * cy + sr * sp * sy,
|
|
];
|
|
}
|
|
|
|
|
|
export class TelemetryService extends EventEmitter {
|
|
#cfg;
|
|
#log;
|
|
|
|
#sensor = {
|
|
state: 'stopped',
|
|
proc: null,
|
|
startedAt: 0,
|
|
lastSampleAt: 0,
|
|
runningSince: 0,
|
|
failures: 0,
|
|
restarts: 0,
|
|
lastError: null,
|
|
backoffTimer: null,
|
|
};
|
|
#orientation = null;
|
|
#batteryTimer = null;
|
|
#batteryPolling = false;
|
|
#battery = {
|
|
state: 'unavailable', available: false, source: 'pisugar-server', socketPath: '/tmp/pisugar-server.sock',
|
|
voltageV: null, percent: null, percentEstimate: null, percentIsEstimate: false,
|
|
charging: null, powerPlugged: null, chargingAllowed: null, updatedAt: null, error: 'not polled yet',
|
|
};
|
|
|
|
#gps = {
|
|
socket: null,
|
|
connected: false,
|
|
retryTimer: null,
|
|
connectFailures: 0,
|
|
lastTpvAt: 0,
|
|
tpv: null,
|
|
sky: null,
|
|
lastGoodFix: null,
|
|
emittedState: 'disconnected',
|
|
};
|
|
|
|
#watchdog = null;
|
|
#stopping = true;
|
|
|
|
constructor(cfg, { log }) {
|
|
super();
|
|
this.#cfg = cfg;
|
|
this.#log = log;
|
|
}
|
|
|
|
// ───────────────────────────── lifecycle ─────────────────────────────
|
|
|
|
start() {
|
|
if (!this.#stopping) return;
|
|
this.#stopping = false;
|
|
this.#startSensor();
|
|
this.#connectGps();
|
|
this.#pollBattery().catch(() => {});
|
|
if (this.#cfg.pisugar?.enabled !== false) {
|
|
this.#batteryTimer = setInterval(() => this.#pollBattery().catch(() => {}), this.#cfg.pisugar?.pollMs || 5000);
|
|
this.#batteryTimer.unref();
|
|
}
|
|
this.#watchdog = setInterval(() => this.#tick(), 1000);
|
|
this.#watchdog.unref();
|
|
}
|
|
|
|
async stop() {
|
|
this.#stopping = true;
|
|
clearInterval(this.#watchdog);
|
|
clearInterval(this.#batteryTimer);
|
|
clearTimeout(this.#sensor.backoffTimer);
|
|
clearTimeout(this.#gps.retryTimer);
|
|
this.#transitionSensor('stopped', 'service stopped');
|
|
|
|
const proc = this.#sensor.proc;
|
|
this.#sensor.proc = null;
|
|
if (proc) {
|
|
await new Promise((resolve) => {
|
|
const t = setTimeout(() => proc.kill('SIGKILL'), 2000);
|
|
proc.once('close', () => {
|
|
clearTimeout(t);
|
|
resolve();
|
|
});
|
|
proc.kill('SIGTERM');
|
|
});
|
|
}
|
|
this.#gps.socket?.destroy();
|
|
this.#gps.socket = null;
|
|
}
|
|
|
|
// ───────────────────────────── public API ─────────────────────────────
|
|
|
|
/** Current time. The OS clock is already synced to RTC/GPS. */
|
|
now() {
|
|
return new Date();
|
|
}
|
|
|
|
/**
|
|
* Latest orientation, or null if the sensor is not delivering fresh data.
|
|
* @param {{maxAgeMs?: number}} [opts]
|
|
*/
|
|
getOrientation({ maxAgeMs = this.#cfg.staleMs } = {}) {
|
|
const o = this.#orientation;
|
|
if (!o) return null;
|
|
const ageMs = Date.now() - o.ts;
|
|
if (ageMs > maxAgeMs) return null;
|
|
return { yaw: o.yaw, pitch: o.pitch, roll: o.roll, quat: o.quat, ageMs, ts: o.ts };
|
|
}
|
|
|
|
/** Last good GPS fix (kept even if the fix is currently lost) or null if there has never been one. */
|
|
getPosition() {
|
|
const f = this.#gps.lastGoodFix;
|
|
if (!f) return null;
|
|
const ageMs = Date.now() - f.at;
|
|
return {
|
|
lat: f.lat,
|
|
lon: f.lon,
|
|
altM: f.altM ?? 0,
|
|
mode: f.mode,
|
|
ageMs,
|
|
live: ageMs < this.#cfg.gpsStaleMs,
|
|
};
|
|
}
|
|
|
|
getSensorState() {
|
|
return this.#sensor.state;
|
|
}
|
|
|
|
getGpsState() {
|
|
return this.#gpsStateNow();
|
|
}
|
|
|
|
getSnapshot() {
|
|
const now = Date.now();
|
|
const s = this.#sensor;
|
|
const o = this.#orientation;
|
|
const g = this.#gps;
|
|
const pos = this.getPosition();
|
|
return {
|
|
time: { iso: new Date(now).toISOString(), epochMs: now },
|
|
sensor: {
|
|
state: s.state,
|
|
sampleAgeMs: s.lastSampleAt ? now - s.lastSampleAt : null,
|
|
restarts: s.restarts,
|
|
lastError: s.lastError,
|
|
},
|
|
orientation: o
|
|
? { yaw: o.yaw, pitch: o.pitch, roll: o.roll, quat: o.quat, ageMs: now - o.ts }
|
|
: null,
|
|
battery: { ...this.#battery },
|
|
gps: {
|
|
state: this.#gpsStateNow(),
|
|
position: pos,
|
|
satellites: g.sky ? { used: g.sky.used, seen: g.sky.seen } : null,
|
|
dop: g.sky ? { hdop: g.sky.hdop, vdop: g.sky.vdop, pdop: g.sky.pdop } : null,
|
|
speedMps: g.tpv?.speed ?? null,
|
|
// GPS time minus system time. Should be a few ms if the OS sync is healthy.
|
|
clockSkewMs: g.tpv?.skewMs ?? null,
|
|
errorM: g.tpv ? { x: g.tpv.epx ?? null, y: g.tpv.epy ?? null, v: g.tpv.epv ?? null } : null,
|
|
},
|
|
};
|
|
}
|
|
|
|
// ───────────────────────────── PiSugar battery ─────────────────────────────
|
|
|
|
getBattery() {
|
|
return { ...this.#battery };
|
|
}
|
|
|
|
async #pisugarCommand(command) {
|
|
const path = this.#cfg.pisugar?.socketPath || '/tmp/pisugar-server.sock';
|
|
return await new Promise((resolve, reject) => {
|
|
const socket = net.createConnection({ path });
|
|
let output = '';
|
|
let settled = false;
|
|
const finish = (err, value) => {
|
|
if (settled) return;
|
|
settled = true;
|
|
clearTimeout(timer);
|
|
socket.destroy();
|
|
if (err) reject(err);
|
|
else resolve(value.trim());
|
|
};
|
|
const timer = setTimeout(() => finish(new Error(`PiSugar socket timed out for: ${command}`)), 1500);
|
|
socket.setEncoding('utf8');
|
|
socket.on('connect', () => socket.write(`${command}\n`));
|
|
socket.on('data', (chunk) => {
|
|
output += chunk;
|
|
const newline = output.indexOf('\n');
|
|
if (newline >= 0) finish(null, output.slice(0, newline));
|
|
});
|
|
socket.on('error', (err) => finish(err));
|
|
socket.on('end', () => finish(null, output));
|
|
});
|
|
}
|
|
|
|
async #pollBattery() {
|
|
const cfg = this.#cfg.pisugar || {};
|
|
if (cfg.enabled === false || this.#batteryPolling) return;
|
|
this.#batteryPolling = true;
|
|
|
|
try {
|
|
const read = async (name) => {
|
|
const response = await this.#pisugarCommand(`get ${name}`);
|
|
const match = response.match(/:\s*(.+?)\s*$/);
|
|
if (!match) throw new Error(`Unexpected PiSugar response for get ${name}: ${response}`);
|
|
return match[1];
|
|
};
|
|
const [percentText, voltageText, pluggedText, allowChargingText] = await Promise.all([
|
|
read('battery'), read('battery_v'), read('battery_power_plugged'), read('battery_allow_charging'),
|
|
]);
|
|
const percent = Number(percentText);
|
|
const voltageV = Number(voltageText);
|
|
const powerPlugged = pluggedText.toLowerCase() === 'true';
|
|
const chargingAllowed = allowChargingText.toLowerCase() === 'true';
|
|
if (!Number.isFinite(percent) || !Number.isFinite(voltageV)) throw new Error('PiSugar returned invalid battery values');
|
|
|
|
this.#battery = {
|
|
state: 'ok', available: true, source: 'pisugar-server',
|
|
socketPath: cfg.socketPath || '/tmp/pisugar-server.sock',
|
|
voltageV: Math.round(voltageV * 1000) / 1000,
|
|
percent: Math.round(percent), percentEstimate: Math.round(percent), percentIsEstimate: false,
|
|
powerPlugged, chargingAllowed, charging: powerPlugged && chargingAllowed,
|
|
updatedAt: new Date().toISOString(), error: null,
|
|
};
|
|
this.emit('battery', this.getBattery());
|
|
} catch (err) {
|
|
const message = err.code === 'ENOENT'
|
|
? `PiSugar socket unavailable (${cfg.socketPath || '/tmp/pisugar-server.sock'}); is pisugar-server running?`
|
|
: err.message;
|
|
const shouldLog = this.#battery.error !== message;
|
|
this.#battery = {
|
|
...this.#battery,
|
|
state: this.#battery.updatedAt ? 'stale' : 'unavailable',
|
|
available: Boolean(this.#battery.updatedAt),
|
|
source: 'pisugar-server', socketPath: cfg.socketPath || '/tmp/pisugar-server.sock', error: message,
|
|
};
|
|
if (shouldLog) this.#log.warn(`PiSugar battery read failed: ${message}`);
|
|
} finally {
|
|
this.#batteryPolling = false;
|
|
}
|
|
}
|
|
|
|
// ───────────────────────────── sensor ─────────────────────────────
|
|
|
|
#transitionSensor(next, reason) {
|
|
const s = this.#sensor;
|
|
if (s.state === next) return;
|
|
if (!SENSOR_TRANSITIONS[s.state]?.includes(next)) {
|
|
this.#log.debug(`ignored illegal sensor transition ${s.state} -> ${next}`);
|
|
return;
|
|
}
|
|
const prev = s.state;
|
|
s.state = next;
|
|
if (next === 'running') s.runningSince = Date.now();
|
|
this.#log.info(`sensor ${prev} -> ${next}${reason ? ` (${reason})` : ''}`);
|
|
this.emit('sensor-state', { state: next, prev, reason });
|
|
}
|
|
|
|
#startSensor() {
|
|
if (this.#stopping) return;
|
|
const s = this.#sensor;
|
|
this.#transitionSensor('starting');
|
|
s.startedAt = Date.now();
|
|
s.lastSampleAt = 0;
|
|
s.runningSince = 0;
|
|
|
|
let proc;
|
|
try {
|
|
proc = spawn(this.#cfg.python, [this.#cfg.script], {
|
|
stdio: ['ignore', 'pipe', 'pipe'],
|
|
env: { ...process.env, BNO_ADDR: String(this.#cfg.bnoAddress), BNO_HZ: String(this.#cfg.bnoHz) },
|
|
});
|
|
} catch (err) {
|
|
this.#sensorDied(null, `spawn failed: ${err.message}`);
|
|
return;
|
|
}
|
|
s.proc = proc;
|
|
|
|
readline.createInterface({ input: proc.stdout }).on('line', (l) => this.#onSensorLine(l));
|
|
readline.createInterface({ input: proc.stderr }).on('line', (l) => this.#onSensorStderr(l));
|
|
proc.on('error', (err) => this.#sensorDied(proc, `process error: ${err.message}`));
|
|
proc.on('exit', (code, signal) => this.#sensorDied(proc, `exited (code=${code}, signal=${signal})`));
|
|
}
|
|
|
|
#onSensorLine(line) {
|
|
let d;
|
|
try {
|
|
d = JSON.parse(line);
|
|
} catch {
|
|
this.#log.debug(`non-JSON from sensor: ${line.slice(0, 120)}`);
|
|
return;
|
|
}
|
|
if (![d.yaw, d.pitch, d.roll].every(isFiniteNum)) return;
|
|
const q = Array.isArray(d.quat) && d.quat.length === 4 && d.quat.every(isFiniteNum)
|
|
? d.quat
|
|
: eulerToQuat(d.yaw, d.pitch, d.roll);
|
|
|
|
const now = Date.now();
|
|
this.#orientation = { yaw: d.yaw, pitch: d.pitch, roll: d.roll, quat: q, ts: now };
|
|
this.#sensor.lastSampleAt = now;
|
|
|
|
const st = this.#sensor.state;
|
|
if (st === 'starting' || st === 'stale') this.#transitionSensor('running', 'receiving samples');
|
|
this.emit('orientation', this.#orientation);
|
|
}
|
|
|
|
#onSensorStderr(line) {
|
|
let msg = line;
|
|
let level = 'warn';
|
|
try {
|
|
const d = JSON.parse(line);
|
|
if (d.error) {
|
|
level = 'error';
|
|
msg = d.error;
|
|
this.#sensor.lastError = String(d.error);
|
|
} else if (d.warning) {
|
|
msg = d.warning;
|
|
}
|
|
} catch {
|
|
/* plain traceback line */
|
|
}
|
|
this.#log[level](`bno: ${msg}`);
|
|
}
|
|
|
|
/** Called when the python process exits/errors. `proc` guards against double-handling. */
|
|
#sensorDied(proc, reason) {
|
|
const s = this.#sensor;
|
|
if (proc && s.proc !== proc) return; // already handled
|
|
s.proc = null;
|
|
if (this.#stopping) return;
|
|
|
|
s.lastError = reason;
|
|
// A process that ran healthily for a while resets the backoff.
|
|
if (s.runningSince && Date.now() - s.runningSince > 15000) s.failures = 0;
|
|
s.failures += 1;
|
|
s.restarts += 1;
|
|
|
|
const delay = Math.min(30000, 1000 * 2 ** Math.min(s.failures - 1, 5));
|
|
this.#transitionSensor('backoff', `${reason}; retry in ${delay / 1000}s`);
|
|
clearTimeout(s.backoffTimer);
|
|
s.backoffTimer = setTimeout(() => this.#startSensor(), delay);
|
|
}
|
|
|
|
// ───────────────────────────── gpsd ─────────────────────────────
|
|
|
|
#connectGps() {
|
|
if (this.#stopping) return;
|
|
const g = this.#gps;
|
|
const { gpsdHost: host, gpsdPort: port } = this.#cfg;
|
|
const sock = net.createConnection({ host, port });
|
|
sock.setEncoding('utf8');
|
|
sock.setKeepAlive(true, 10000);
|
|
g.socket = sock;
|
|
|
|
let buf = '';
|
|
sock.on('connect', () => {
|
|
g.connected = true;
|
|
g.connectFailures = 0;
|
|
this.#log.info(`gpsd connected (${host}:${port})`);
|
|
sock.write('?WATCH={"enable":true,"json":true}\n');
|
|
this.#emitGpsStateIfChanged();
|
|
});
|
|
sock.on('data', (chunk) => {
|
|
buf += chunk;
|
|
let i;
|
|
while ((i = buf.indexOf('\n')) >= 0) {
|
|
const line = buf.slice(0, i).trim();
|
|
buf = buf.slice(i + 1);
|
|
if (line) this.#onGpsLine(line);
|
|
}
|
|
if (buf.length > 1_000_000) buf = '';
|
|
});
|
|
sock.on('error', (err) => {
|
|
// Don't spam the dashboard if gpsd simply isn't running.
|
|
if (g.connectFailures === 0 || g.connectFailures % 20 === 0) {
|
|
this.#log.warn(`gpsd connection error: ${err.code || err.message}`);
|
|
}
|
|
});
|
|
sock.on('close', () => {
|
|
if (g.socket === sock) g.socket = null;
|
|
const wasConnected = g.connected;
|
|
g.connected = false;
|
|
g.connectFailures += 1;
|
|
if (wasConnected) this.#log.warn('gpsd disconnected');
|
|
this.#emitGpsStateIfChanged();
|
|
if (this.#stopping) return;
|
|
clearTimeout(g.retryTimer);
|
|
g.retryTimer = setTimeout(() => this.#connectGps(), Math.min(10000, 2000 + g.connectFailures * 1000));
|
|
});
|
|
}
|
|
|
|
#onGpsLine(line) {
|
|
let d;
|
|
try {
|
|
d = JSON.parse(line);
|
|
} catch {
|
|
return;
|
|
}
|
|
const g = this.#gps;
|
|
if (d.class === 'TPV') {
|
|
const now = Date.now();
|
|
const tpv = { ...d, receivedAt: now };
|
|
if (d.time) {
|
|
const t = Date.parse(d.time);
|
|
if (Number.isFinite(t)) tpv.skewMs = Math.round(t - now);
|
|
}
|
|
g.tpv = tpv;
|
|
g.lastTpvAt = now;
|
|
if (d.mode >= 2 && isFiniteNum(d.lat) && isFiniteNum(d.lon)) {
|
|
g.lastGoodFix = {
|
|
lat: d.lat,
|
|
lon: d.lon,
|
|
altM: [d.altMSL, d.altHAE, d.alt].find(isFiniteNum) ?? null,
|
|
mode: d.mode,
|
|
at: now,
|
|
};
|
|
this.emit('gps', this.getPosition());
|
|
}
|
|
this.#emitGpsStateIfChanged();
|
|
} else if (d.class === 'SKY') {
|
|
const sats = Array.isArray(d.satellites) ? d.satellites : null;
|
|
g.sky = {
|
|
used: d.uSat ?? sats?.filter((s) => s.used).length ?? null,
|
|
seen: d.nSat ?? sats?.length ?? null,
|
|
hdop: d.hdop ?? null,
|
|
vdop: d.vdop ?? null,
|
|
pdop: d.pdop ?? null,
|
|
};
|
|
}
|
|
}
|
|
|
|
/** disconnected | no_gpsd_data | no_fix | fix_2d | fix_3d */
|
|
#gpsStateNow() {
|
|
const g = this.#gps;
|
|
if (!g.connected) return 'disconnected';
|
|
if (!g.tpv) return 'no_gpsd_data';
|
|
if (Date.now() - g.lastTpvAt > this.#cfg.gpsStaleMs) return 'no_gpsd_data';
|
|
if (g.tpv.mode === 3) return 'fix_3d';
|
|
if (g.tpv.mode === 2) return 'fix_2d';
|
|
return 'no_fix';
|
|
}
|
|
|
|
#emitGpsStateIfChanged() {
|
|
const now = this.#gpsStateNow();
|
|
if (now === this.#gps.emittedState) return;
|
|
const prev = this.#gps.emittedState;
|
|
this.#gps.emittedState = now;
|
|
this.#log.info(`gps ${prev} -> ${now}`);
|
|
this.emit('gps-state', { state: now, prev });
|
|
}
|
|
|
|
// ───────────────────────────── watchdog ─────────────────────────────
|
|
|
|
#tick() {
|
|
const now = Date.now();
|
|
const s = this.#sensor;
|
|
const { staleMs, restartAfterMs, startTimeoutMs } = this.#cfg;
|
|
|
|
if (s.state === 'running' && now - s.lastSampleAt > staleMs) {
|
|
this.#transitionSensor('stale', `no sample for ${now - s.lastSampleAt} ms`);
|
|
} else if (s.state === 'stale' && now - s.lastSampleAt > restartAfterMs) {
|
|
this.#log.warn('sensor stale too long; restarting python');
|
|
s.proc?.kill('SIGKILL'); // 'exit' handler moves us to backoff
|
|
} else if (s.state === 'starting' && now - s.startedAt > startTimeoutMs) {
|
|
this.#log.warn('sensor produced no data after start; restarting python');
|
|
s.proc?.kill('SIGKILL');
|
|
}
|
|
|
|
this.#emitGpsStateIfChanged(); // catches "TPV stopped arriving"
|
|
}
|
|
}
|
|
|