init

2026-10-21 19:32:02 -04:00
commit b74d278c8e
96 changed files with 54476 additions and 0 deletions
@@ -0,0 +1,516 @@
+
+// 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"
+  }
+}
+