135 lines
7.1 KiB
TypeScript
135 lines
7.1 KiB
TypeScript
/** 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));
|
|
}
|
|
}
|