feat(rover): add drive profiles and leased remote command channel

2026-09-25 16:37:56 +03:00
parent 53818230f9
commit 63cbb08ea0
18 changed files with 1845 additions and 0 deletions
@@ -0,0 +1,134 @@
+/** Behavioural reference ONLY. Not connected to the current VESC PPM path. */
+import {axisKeys, bounded, mix, parseProfile, type Axes, type ControlProfile, type Sides} from './profile';
+
+export interface Sample { value: number; at: number; sequence: number }
+export interface DriveEvidence { at: number; healthy: boolean; stopped: boolean }
+export interface CoreCommand {
+  token: string; sequence: number; at: number; expires: number; demand: Sides;
+}
+export interface Observation {
+  now: number;
+  // These are trusted receiver timestamps and link status, NOT USB read times.
+  link: { state: 'live' | 'lost' | 'unknown'; at: number };
+  axes: Partial<Record<keyof Axes, Sample>>;
+  drives: Record<string, DriveEvidence>;
+  command?: CoreCommand;
+  requestCore?: {owner: 'remote' | 'autonomy'; sequence: number; at: number};
+}
+export interface Policy {
+  maxAgeMs: number; neutralMs: number; maxCommandMs: number;
+  /** Verified RC controls that can take over, including non-driving stick axes. */
+  monitoredAxes: readonly (keyof Axes)[];
+}
+export type State = 'hold' | 'rc-ready' | 'rc-manual' | 'core';
+export interface Decision {
+  state: State; reason: string; demand: Sides; token: string | null;
+  owner: 'remote' | 'autonomy' | 'rc' | null;
+  stopAll: boolean; flushMotionQueue: boolean; cancelMotionTasks: boolean;
+}
+const zero = (): Sides => ({left: 0, right: 0});
+
+export class AuthorityModel {
+  private state: State = 'hold';
+  private epoch = 0;
+  private token: string | null = null;
+  private owner: Decision['owner'] = null;
+  private neutralSince: number | null = null;
+  private lastNow = -Infinity;
+  private lastCommand = -1;
+  private lastRequest = -1;
+  private samples: Partial<Record<keyof Axes, Sample>> = {};
+  private readonly profile: ControlProfile;
+  private readonly motors: string[];
+  private readonly policy: Policy;
+
+  constructor(profile: ControlProfile, motors: readonly string[], policy: Policy, private readonly bootId: string) {
+    this.profile = parseProfile(profile);
+    this.motors = [...motors]; this.policy = {...policy, monitoredAxes:[...policy.monitoredAxes]};
+    if (!bootId || !motors.length || new Set(motors).size !== motors.length || motors.some(id => !id)
+        || !bounded(policy.maxAgeMs, 1, 10000) || !bounded(policy.neutralMs, 1, 10000)
+        || !bounded(policy.maxCommandMs, 1, 10000)
+        || new Set(policy.monitoredAxes).size !== policy.monitoredAxes.length
+        || policy.monitoredAxes.some(key => !['leftY','rightY','leftX','rightX'].includes(key))
+        || axisKeys(this.profile).some(key => !policy.monitoredAxes.includes(key))) throw Error('Invalid authority policy');
+  }
+  private result(reason: string, demand = zero(), revoke = false): Decision {
+    return {state: this.state, reason, demand, token: this.token, owner: this.owner,
+      stopAll: this.state === 'hold', flushMotionQueue: revoke, cancelMotionTasks: revoke};
+  }
+  private hold(reason: string): Decision {
+    const revoke = this.state === 'core';
+    if (this.state !== 'hold') this.epoch++;
+    this.state = 'hold'; this.token = null; this.owner = null;
+    this.neutralSince = null; this.lastCommand = -1;
+    return this.result(reason, zero(), revoke);
+  }
+  step(input: Observation): Decision {
+    const now = input.now;
+    if (!Number.isFinite(now) || now < 0 || now < this.lastNow) return this.hold('clock-invalid');
+    const gap = now - this.lastNow; this.lastNow = now;
+    const fresh = (at: number) => Number.isFinite(at) && at <= now && now - at <= this.policy.maxAgeMs;
+    const request = input.requestCore;
+    const newRequest = !!request && Number.isSafeInteger(request.sequence) && request.sequence > this.lastRequest;
+    // Consume even a premature request: it must never become effective later.
+    if (newRequest) this.lastRequest = request.sequence;
+    // A gap cannot count towards continuous observed neutral.
+    if (gap > this.policy.maxAgeMs) {
+      const first = gap === Infinity;
+      if (!first) return this.hold('observation-gap');
+      this.neutralSince = null;
+    }
+    if (input.link?.state !== 'live' || !fresh(input.link.at)) return this.hold('receiver-unverified');
+    const axes = {leftY: 0, rightY: 0, leftX: 0, rightX: 0};
+    let neutral = true;
+    let observedThrough = input.link.at;
+    const candidates: Partial<Record<keyof Axes, Sample>> = {};
+    for (const key of this.policy.monitoredAxes) {
+      const sample = input.axes[key], prev = this.samples[key];
+      if (!sample || !bounded(sample.value, -1, 1) || !fresh(sample.at)
+          || !Number.isSafeInteger(sample.sequence) || sample.sequence < 0
+          || (prev && (sample.sequence < prev.sequence || sample.at < prev.at
+            || (sample.sequence === prev.sequence && (sample.value !== prev.value || sample.at !== prev.at)))))
+        return this.hold('axis-invalid');
+      candidates[key] = {...sample}; axes[key] = sample.value;
+      observedThrough = Math.min(observedThrough, sample.at);
+      neutral &&= Math.abs(sample.value) <= this.profile.deadband;
+    }
+    this.samples = candidates;
+    let stopped = true;
+    for (const id of this.motors) {
+      const drive = input.drives[id];
+      if (!drive || drive.healthy !== true || !fresh(drive.at)) return this.hold('drive-unverified');
+      observedThrough = Math.min(observedThrough, drive.at);
+      stopped &&= drive.stopped === true;
+    }
+    // RC intent is evaluated before any Core command or reacquisition request.
+    if (this.state === 'core' && !neutral) return this.hold('rc-takeover');
+    if (neutral && stopped) this.neutralSince ??= now;
+    else this.neutralSince = null;
+    const stableNeutral = this.neutralSince !== null && observedThrough - this.neutralSince >= this.policy.neutralMs;
+    if (this.state === 'hold') {
+      if (!stableNeutral) return this.result(stopped ? 'await-neutral' : 'await-stop');
+      this.state = 'rc-ready'; this.owner = 'rc';
+      // A request queued before reaching neutral cannot acquire Core authority.
+      return this.result('rc-ready');
+    }
+    if (newRequest && request && fresh(request.at) && this.state !== 'core' && stableNeutral) {
+      if (!['remote', 'autonomy'].includes(request.owner)) return this.hold('source-invalid');
+      this.epoch++; this.token = `${this.bootId}:${this.epoch}:${this.profile.revision}`;
+      this.owner = request.owner; this.state = 'core'; this.lastCommand = -1;
+      return this.result('core-granted');
+    }
+    if (this.state === 'core') {
+      const c = input.command;
+      if (!c || c.token !== this.token || !Number.isSafeInteger(c.sequence) || c.sequence <= this.lastCommand
+          || !fresh(c.at) || !Number.isFinite(c.expires) || c.expires <= now
+          || c.expires - c.at > this.policy.maxCommandMs || c.expires < c.at
+          || !bounded(c.demand.left, -1, 1) || !bounded(c.demand.right, -1, 1)) return this.hold('core-command-invalid');
+      this.lastCommand = c.sequence;
+      return this.result('core-command', {left: c.demand.left * this.profile.outputScale, right: c.demand.right * this.profile.outputScale});
+    }
+    if (this.state === 'rc-ready' && !neutral) this.state = 'rc-manual';
+    return this.result(neutral ? 'rc-neutral' : 'rc-command', mix(this.profile, axes));
+  }
+}