From c96e731d4f9b4a56d38d1f50392d0066dfa205da Mon Sep 17 00:00:00 2001 From: Aaron Qian Date: Tue, 22 Sep 2026 17:16:34 -0700 Subject: [PATCH] app: mode-driven control cluster with the goal series following the mode --- src/components/ui/alert.tsx | 72 ++++ src/lib/control.test.ts | 267 ++++++++++++++ src/lib/control.ts | 283 +++++++++++++++ src/lib/in-flight.test.ts | 30 -- src/lib/in-flight.ts | 19 - src/lib/telemetry-poll.test.ts | 27 +- src/lib/telemetry-poll.ts | 24 +- src/routes/live.tsx | 618 +++++++++++++++++++++++++++++---- tests/e2e/control.spec.ts | 122 +++++++ 9 files changed, 1340 insertions(+), 122 deletions(-) create mode 100644 src/components/ui/alert.tsx create mode 100644 src/lib/control.test.ts create mode 100644 src/lib/control.ts delete mode 100644 src/lib/in-flight.test.ts delete mode 100644 src/lib/in-flight.ts create mode 100644 tests/e2e/control.spec.ts diff --git a/src/components/ui/alert.tsx b/src/components/ui/alert.tsx new file mode 100644 index 0000000..36bb972 --- /dev/null +++ b/src/components/ui/alert.tsx @@ -0,0 +1,72 @@ +import * as React from "react"; +import { cva, type VariantProps } from "class-variance-authority"; +import { cn } from "cn"; + +const alertVariants = cva( + "group/alert relative grid w-full gap-0.5 rounded-lg border px-4 py-3 text-left text-sm has-data-[slot=alert-action]:relative has-data-[slot=alert-action]:pr-18 has-[>svg]:grid-cols-[auto_1fr] has-[>svg]:gap-x-2.5 *:[svg]:row-span-2 *:[svg]:translate-y-0.5 *:[svg]:text-current *:[svg:not([class*='size-'])]:size-4", + { + variants: { + variant: { + default: "bg-card text-card-foreground", + destructive: + "bg-card text-destructive *:data-[slot=alert-description]:text-destructive/90 *:[svg]:text-current", + }, + }, + defaultVariants: { + variant: "default", + }, + }, +); + +function Alert({ + className, + variant, + ...props +}: React.ComponentProps<"div"> & VariantProps) { + return ( +
+ ); +} + +function AlertTitle({ className, ...props }: React.ComponentProps<"div">) { + return ( +
svg]/alert:col-start-2 [&_a]:underline [&_a]:underline-offset-3 [&_a]:hover:text-foreground", + className, + )} + {...props} + /> + ); +} + +function AlertDescription({ className, ...props }: React.ComponentProps<"div">) { + return ( +
+ ); +} + +function AlertAction({ className, ...props }: React.ComponentProps<"div">) { + return ( +
+ ); +} + +export { Alert, AlertTitle, AlertDescription, AlertAction }; diff --git a/src/lib/control.test.ts b/src/lib/control.test.ts new file mode 100644 index 0000000..4df18b4 --- /dev/null +++ b/src/lib/control.test.ts @@ -0,0 +1,267 @@ +import { readFileSync } from "node:fs"; +import { afterEach, beforeEach, expect, test, vi } from "vitest"; +import fixture from "../../tests/fixtures/stall-24mhz.json"; +import { + clampGoal, + CONTROL_REGISTERS, + decodeControl, + dutyPercent, + goalOf, + goalRange, + goalSpec, + isMode, + isWindow, + LatestWins, + LIMIT_REGISTERS, + limitsFromTable, + MODES, + modeLabel, + modeName, + positionRange, + type GoalContext, + type Limits, +} from "./control"; +import { spanOver, type FieldInfo, type Sample } from "./telemetry-poll"; +import { ADC_MAX_COUNT, ampsPerCount, senseFromTable, type Calibration } from "./units"; + +const { fields } = JSON.parse( + readFileSync( + new URL("../../../open-servo-core/descriptors/osc-servo/0.1.json", import.meta.url), + "utf8", + ), +) as { fields: (FieldInfo & { variants?: { name: string; value: number }[] })[] }; + +const sense = senseFromTable((name) => { + const v = (fixture.meta.sense as Record)[name]; + if (v === undefined) throw new Error(`fixture carries no ${name}`); + return v; +}); +const cal: Calibration = { + rawMin: 200, + rawMax: 3800, + angleMinCdeg: 0, + angleMaxCdeg: 18000, + gearRatioCenti: 100, +}; +const swapped: Calibration = { ...cal, rawMin: 3800, rawMax: 200 }; +const limits: Limits = { dutyMaxQ15: 30000, velocityLimitCps: 4000, currentLimitCounts: 280 }; +const ctx: GoalContext = { cal, sense, limits, raw: false }; + +test("the control and limit spans over the 0.1 descriptor fit one READ each", () => { + expect(spanOver(fields, CONTROL_REGISTERS)).toMatchObject({ addr: 384, count: 18 }); + expect(spanOver(fields, LIMIT_REGISTERS)).toMatchObject({ addr: 54, count: 20 }); +}); + +test("decodeControl reads the switch, the mode and every goal", () => { + const span = spanOver(fields, CONTROL_REGISTERS); + const bytes = new Uint8Array(span.count); + const view = new DataView(bytes.buffer); + const at = (name: string) => { + const f = fields.find((x) => x.name === name); + if (f === undefined) throw new Error(name); + return f.addr - span.addr; + }; + view.setUint8(at("torque_enable"), 1); + view.setUint8(at("mode"), 2); + view.setInt16(at("goal_duty"), -1234, true); + view.setInt32(at("goal_position"), 2048, true); + view.setInt32(at("goal_velocity"), -600, true); + view.setInt16(at("goal_current"), 150, true); + expect(decodeControl(span, bytes)).toEqual({ + torque: true, + mode: 2, + goals: { goal_duty: -1234, goal_position: 2048, goal_velocity: -600, goal_current: 150 }, + }); +}); + +test("limitsFromTable names the three ceilings", () => { + const regs: Record = { + duty_max_q15: 30000, + velocity_limit_cps: 4000, + current_limit_counts: 280, + }; + expect(limitsFromTable((name) => regs[name] ?? NaN)).toEqual(limits); +}); + +test("the modes carry the descriptor's variant names and plain labels", () => { + expect(MODES.map((m) => m.name)).toEqual(["OpenLoop", "Current", "Velocity", "Position"]); + expect(modeLabel("OpenLoop")).toBe("Open loop"); + expect(isMode("Velocity")).toBe(true); + expect(isMode("Torque")).toBe(false); + const variants = fields.find((f) => f.name === "mode")?.variants ?? []; + expect(modeName(variants, 3)).toBe("Position"); + expect(modeName(variants, 9)).toBeUndefined(); +}); + +test("goalOf picks the sample field behind each goal register", () => { + const sample = { goal: 1, goalVelocity: 2, goalCurrent: 3, goalDuty: 4 } as Sample; + expect(goalOf(sample, "goal_position")).toBe(1); + expect(goalOf(sample, "goal_velocity")).toBe(2); + expect(goalOf(sample, "goal_current")).toBe(3); + expect(goalOf(sample, "goal_duty")).toBe(4); +}); + +test("the position goal spans the calibrated sensor, or the ADC while the calibration is unusable", () => { + expect(positionRange(cal)).toEqual({ min: 200, max: 3800 }); + expect(positionRange(swapped)).toEqual({ min: 0, max: ADC_MAX_COUNT }); + expect(goalRange("Position", swapped, limits)).toEqual({ min: 0, max: ADC_MAX_COUNT }); +}); + +test("clampGoal rounds to whole counts and pins to the range", () => { + const range = positionRange(cal); + expect(clampGoal(1000.4, range)).toBe(1000); + expect(clampGoal(1000.6, range)).toBe(1001); + expect(clampGoal(-5, range)).toBe(200); + expect(clampGoal(5000, range)).toBe(3800); +}); + +test("the position goal is degrees through the calibration, or counts in raw mode", () => { + const deg = goalSpec("Position", ctx); + expect(deg).toMatchObject({ register: "goal_position", unit: "deg", digits: 1 }); + expect(deg.range).toEqual({ min: 200, max: 3800 }); + expect(deg.toDisplay(2000)).toBeCloseTo(90, 6); + expect(deg.fromDisplay(90)).toBe(2000); + expect(deg.fromDisplay(45)).toBe(1100); + const raw = goalSpec("Position", { ...ctx, raw: true }); + expect(raw).toMatchObject({ unit: "counts", digits: 0 }); + expect(raw.toDisplay(2000)).toBe(2000); + expect(raw.fromDisplay(1234.6)).toBe(1235); +}); + +test("the velocity goal is deg/s over the velocity limit, counts/s in raw mode", () => { + const spec = goalSpec("Velocity", ctx); + expect(spec).toMatchObject({ register: "goal_velocity", unit: "deg/s", digits: 0 }); + expect(spec.range).toEqual({ min: -4000, max: 4000 }); + // 180 deg over 3600 counts is 0.05 deg per count. + expect(spec.toDisplay(1000)).toBeCloseTo(50, 6); + expect(spec.fromDisplay(50)).toBe(1000); + const raw = goalSpec("Velocity", { ...ctx, raw: true }); + expect(raw.unit).toBe("counts/s"); + expect(raw.toDisplay(1000)).toBe(1000); + expect(raw.fromDisplay(999.6)).toBe(1000); +}); + +test("the current goal is milliamps over the current limit with no bias, raw or not", () => { + const spec = goalSpec("Current", ctx); + expect(spec).toMatchObject({ register: "goal_current", unit: "mA", digits: 0 }); + expect(spec.range).toEqual({ min: -280, max: 280 }); + const maPerCount = ampsPerCount(sense) * 1000; + expect(spec.toDisplay(100)).toBeCloseTo(100 * maPerCount, 9); + expect(spec.fromDisplay(100 * maPerCount)).toBe(100); + expect(goalSpec("Current", { ...ctx, raw: true }).unit).toBe("mA"); +}); + +test("the open-loop goal is a percent of full duty, capped by duty_max_q15 and the i16", () => { + const spec = goalSpec("OpenLoop", ctx); + expect(spec).toMatchObject({ register: "goal_duty", unit: "%", digits: 1 }); + expect(spec.range).toEqual({ min: -30000, max: 30000 }); + expect(spec.toDisplay(16384)).toBe(50); + expect(spec.fromDisplay(-50)).toBe(-16384); + expect(dutyPercent(-8192)).toBe(-25); + const wide = goalSpec("OpenLoop", { ...ctx, limits: { ...limits, dutyMaxQ15: 65535 } }); + expect(wide.range).toEqual({ min: -32767, max: 32767 }); +}); + +test("isWindow accepts the listed windows only", () => { + expect(isWindow(4)).toBe(true); + expect(isWindow(30)).toBe(true); + expect(isWindow(5)).toBe(false); +}); + +beforeEach(() => { + vi.useFakeTimers(); +}); + +afterEach(() => { + vi.useRealTimers(); +}); + +function deferredSender() { + const sent: number[] = []; + const resolvers: (() => void)[] = []; + const send = (v: number) => + new Promise((resolve) => { + sent.push(v); + resolvers.push(resolve); + }); + const settle = async () => { + resolvers.shift()?.(); + await vi.advanceTimersByTimeAsync(0); + }; + return { sent, send, settle }; +} + +test("the first value goes out at once and a burst collapses to its newest", async () => { + const { sent, send, settle } = deferredSender(); + const errors: unknown[] = []; + const w = new LatestWins(send, 200, (e) => errors.push(e)); + w.push(1); + expect(sent).toEqual([1]); + w.push(2); + w.push(3); + w.push(4); + expect(sent).toEqual([1]); + await settle(); + await vi.advanceTimersByTimeAsync(199); + expect(sent).toEqual([1]); + await vi.advanceTimersByTimeAsync(1); + expect(sent).toEqual([1, 4]); + await settle(); + await vi.advanceTimersByTimeAsync(200); + expect(sent).toEqual([1, 4]); + expect(errors).toEqual([]); +}); + +test("a value pushed within the gap after a settled send waits for the gap", async () => { + const { sent, send, settle } = deferredSender(); + const w = new LatestWins(send, 200, () => undefined); + w.push(1); + await settle(); + await vi.advanceTimersByTimeAsync(50); + w.push(2); + expect(sent).toEqual([1]); + await vi.advanceTimersByTimeAsync(150); + expect(sent).toEqual([1, 2]); +}); + +test("a value pushed after the gap has passed goes out at once", async () => { + const { sent, send, settle } = deferredSender(); + const w = new LatestWins(send, 200, () => undefined); + w.push(1); + await settle(); + await vi.advanceTimersByTimeAsync(200); + w.push(2); + expect(sent).toEqual([1, 2]); +}); + +test("a rejected send reports the error and the next value still goes out", async () => { + const errors: unknown[] = []; + const sent: number[] = []; + const w = new LatestWins( + (v) => { + sent.push(v); + return v === 1 ? Promise.reject(new Error("boom")) : Promise.resolve(); + }, + 200, + (e) => errors.push(e), + ); + w.push(1); + await vi.advanceTimersByTimeAsync(0); + expect(errors).toHaveLength(1); + w.push(2); + await vi.advanceTimersByTimeAsync(200); + expect(sent).toEqual([1, 2]); +}); + +test("stop drops the pending value and ignores later pushes", async () => { + const { sent, send, settle } = deferredSender(); + const w = new LatestWins(send, 200, () => undefined); + w.push(1); + w.push(2); + w.stop(); + await settle(); + await vi.advanceTimersByTimeAsync(500); + w.push(3); + await vi.advanceTimersByTimeAsync(500); + expect(sent).toEqual([1]); +}); diff --git a/src/lib/control.ts b/src/lib/control.ts new file mode 100644 index 0000000..a06f4e5 --- /dev/null +++ b/src/lib/control.ts @@ -0,0 +1,283 @@ +// The Live page's control cluster, minus React: the servo's mode and goal +// registers, each goal's range and unit conversion, and the write policy +// behind a slider drag. + +import { decodeSpan, type Sample, type Span } from "./telemetry-poll"; +import { + ADC_MAX_COUNT, + ampsPerCount, + calibrationStatus, + currentMa, + degPerCount, + degToCounts, + positionDeg, + velocityDegPerS, + type Calibration, + type ReadRegister, + type Sense, +} from "./units"; + +/** Seconds of samples the panels show. */ +export const WINDOWS_S = [4, 10, 30] as const; +export type WindowS = (typeof WINDOWS_S)[number]; + +export function isWindow(value: number): value is WindowS { + return WINDOWS_S.some((w) => w === value); +} + +/** Gap between goal writes while a slider drags: at most 5 per second. */ +export const GOAL_WRITE_GAP_MS = 200; + +/** Duties are q15 fractions of full drive (firmware regions/control.rs). */ +const Q15 = 2 ** 15; +/** goal_duty is an i16, so full drive itself is one count out of reach. */ +const DUTY_MAX_Q15 = Q15 - 1; + +/** The `mode` enum's variants by name, each with the goal register it runs on. */ +export const MODES = [ + { name: "OpenLoop", label: "Open loop", goal: "goal_duty" }, + { name: "Current", label: "Current", goal: "goal_current" }, + { name: "Velocity", label: "Velocity", goal: "goal_velocity" }, + { name: "Position", label: "Position", goal: "goal_position" }, +] as const; +export type ModeName = (typeof MODES)[number]["name"]; +export type GoalRegister = (typeof MODES)[number]["goal"]; + +export function isMode(name: string): name is ModeName { + return MODES.some((m) => m.name === name); +} + +export function modeLabel(name: ModeName): string { + return MODES.find((m) => m.name === name)?.label ?? name; +} + +/** One READ (protocol sec 3.1) covers the switch, the mode and every goal. */ +export const CONTROL_REGISTERS: readonly string[] = [ + "torque_enable", + "mode", + ...MODES.map((m) => m.goal), +]; + +export const LIMIT_REGISTERS: readonly string[] = [ + "duty_max_q15", + "velocity_limit_cps", + "current_limit_counts", +]; + +export interface ControlState { + torque: boolean; + /** The `mode` enum's discriminant as written. */ + mode: number; + goals: Record; +} + +/** The config ceilings the goal validators and the trajectory clamp to. */ +export interface Limits { + dutyMaxQ15: number; + velocityLimitCps: number; + currentLimitCounts: number; +} + +export function decodeControl(span: Span, bytes: Uint8Array): ControlState { + const read = decodeSpan(span, bytes); + return { + torque: read("torque_enable") !== 0, + mode: read("mode"), + goals: { + goal_duty: read("goal_duty"), + goal_position: read("goal_position"), + goal_velocity: read("goal_velocity"), + goal_current: read("goal_current"), + }, + }; +} + +export function limitsFromTable(read: ReadRegister): Limits { + return { + dutyMaxQ15: read("duty_max_q15"), + velocityLimitCps: read("velocity_limit_cps"), + currentLimitCounts: read("current_limit_counts"), + }; +} + +/** Inclusive, in counts. */ +export interface Range { + min: number; + max: number; +} + +/** The calibrated sensor span, or the whole ADC while the calibration is not usable. */ +export function positionRange(cal: Calibration): Range { + return calibrationStatus(cal).valid + ? { min: cal.rawMin, max: cal.rawMax } + : { min: 0, max: ADC_MAX_COUNT }; +} + +function symmetric(max: number): Range { + return { min: -max, max }; +} + +/** What the mode's goal validator or clamp lets through (firmware regions/control.rs). */ +export function goalRange(mode: ModeName, cal: Calibration, limits: Limits): Range { + switch (mode) { + case "OpenLoop": + return symmetric(Math.min(limits.dutyMaxQ15, DUTY_MAX_Q15)); + case "Current": + return symmetric(limits.currentLimitCounts); + case "Velocity": + return symmetric(limits.velocityLimitCps); + case "Position": + return positionRange(cal); + } +} + +export function clampGoal(counts: number, range: Range): number { + return Math.min(range.max, Math.max(range.min, Math.round(counts))); +} + +export function dutyPercent(q15: number): number { + return (q15 * 100) / Q15; +} + +export function modeName( + variants: readonly { name: string; value: number }[], + value: number, +): ModeName | undefined { + const name = variants.find((v) => v.value === value)?.name; + return name !== undefined && isMode(name) ? name : undefined; +} + +export function goalOf(sample: Sample, register: GoalRegister): number { + switch (register) { + case "goal_duty": + return sample.goalDuty; + case "goal_position": + return sample.goal; + case "goal_velocity": + return sample.goalVelocity; + case "goal_current": + return sample.goalCurrent; + } +} + +export interface UnitsContext { + cal: Calibration; + sense: Sense; + /** Counts for the position family; electrical goals stay in real units. */ + raw: boolean; +} + +/** How one mode's goal reads: `toDisplay` and `fromDisplay` map counts to the shown unit. */ +export interface GoalUnits { + register: GoalRegister; + unit: string; + digits: number; + toDisplay: (counts: number) => number; + /** Whole counts, not yet clamped. */ + fromDisplay: (value: number) => number; +} + +export function goalUnits(mode: ModeName, { cal, sense, raw }: UnitsContext): GoalUnits { + switch (mode) { + case "OpenLoop": + return { + register: "goal_duty", + unit: "%", + digits: 1, + toDisplay: dutyPercent, + fromDisplay: (pct) => Math.round((pct * Q15) / 100), + }; + case "Current": { + // A command, not a shunt reading: no bias to take out. + const perCount = ampsPerCount(sense) * 1000; + return { + register: "goal_current", + unit: "mA", + digits: 0, + toDisplay: (counts) => currentMa(counts, 0, sense), + fromDisplay: (ma) => (perCount > 0 ? Math.round(ma / perCount) : 0), + }; + } + case "Velocity": { + const perCount = degPerCount(cal); + return { + register: "goal_velocity", + unit: raw ? "counts/s" : "deg/s", + digits: 0, + toDisplay: (cps) => (raw ? cps : velocityDegPerS(cps, cal)), + fromDisplay: (v) => Math.round(raw || perCount === 0 ? v : v / perCount), + }; + } + case "Position": + return { + register: "goal_position", + unit: raw ? "counts" : "deg", + digits: raw ? 0 : 1, + toDisplay: (counts) => (raw ? counts : positionDeg(counts, cal)), + fromDisplay: (v) => (raw ? Math.round(v) : degToCounts(v, cal)), + }; + } +} + +export interface GoalContext extends UnitsContext { + limits: Limits; +} + +export interface GoalSpec extends GoalUnits { + range: Range; +} + +export function goalSpec(mode: ModeName, ctx: GoalContext): GoalSpec { + return { ...goalUnits(mode, ctx), range: goalRange(mode, ctx.cal, ctx.limits) }; +} + +/** + * Latest wins: a burst of values collapses to the newest, one send runs at a + * time, and the next starts no sooner than `gapMs` after the previous settled. + * The first value of a burst goes out at once. + */ +export class LatestWins { + private next: { value: T } | undefined; + private busy = false; + private timer: ReturnType | undefined; + private stopped = false; + + constructor( + private readonly send: (value: T) => Promise, + private readonly gapMs: number, + private readonly onError: (error: unknown) => void, + ) {} + + push(value: T): void { + if (this.stopped) return; + this.next = { value }; + if (!this.busy && this.timer === undefined) this.flush(); + } + + /** Drops what has not been sent; a send already running finishes. */ + stop(): void { + this.stopped = true; + this.next = undefined; + clearTimeout(this.timer); + this.timer = undefined; + } + + private flush(): void { + const next = this.next; + if (next === undefined) return; + this.next = undefined; + this.busy = true; + this.send(next.value) + .catch((e: unknown) => { + if (!this.stopped) this.onError(e); + }) + .finally(() => { + this.busy = false; + if (this.stopped) return; + this.timer = setTimeout(() => { + this.timer = undefined; + this.flush(); + }, this.gapMs); + }); + } +} diff --git a/src/lib/in-flight.test.ts b/src/lib/in-flight.test.ts deleted file mode 100644 index c50a23d..0000000 --- a/src/lib/in-flight.test.ts +++ /dev/null @@ -1,30 +0,0 @@ -import { expect, test } from "vitest"; -import { inFlight } from "./in-flight"; - -test("a call while one is pending is skipped, the next one after it runs", async () => { - let resolve: (v: number) => void = () => undefined; - let calls = 0; - const guarded = inFlight( - () => - new Promise((r) => { - calls++; - resolve = r; - }), - ); - const first = guarded(); - const skipped = guarded(); - expect(calls).toBe(1); - await expect(skipped).resolves.toBeUndefined(); - resolve(7); - await expect(first).resolves.toBe(7); - const third = guarded(); - expect(calls).toBe(2); - resolve(9); - await expect(third).resolves.toBe(9); -}); - -test("a rejection releases the flag", async () => { - const guarded = inFlight(() => Promise.reject(new Error("boom"))); - await expect(guarded()).rejects.toThrow("boom"); - await expect(guarded()).rejects.toThrow("boom"); -}); diff --git a/src/lib/in-flight.ts b/src/lib/in-flight.ts deleted file mode 100644 index 232c085..0000000 --- a/src/lib/in-flight.ts +++ /dev/null @@ -1,19 +0,0 @@ -// Until the session serializes commands itself: the adapter answers "busy" to -// a second in-flight command, so a call made while one is pending is skipped -// (resolves undefined) instead of reaching the adapter. One flag for the whole -// app, so a read the previous selection left pending still counts. -let pending = false; - -export function inFlight( - fn: (...args: A) => Promise, -): (...args: A) => Promise { - return async (...args) => { - if (pending) return undefined; - pending = true; - try { - return await fn(...args); - } finally { - pending = false; - } - }; -} diff --git a/src/lib/telemetry-poll.test.ts b/src/lib/telemetry-poll.test.ts index be3ef05..257f7ef 100644 --- a/src/lib/telemetry-poll.test.ts +++ b/src/lib/telemetry-poll.test.ts @@ -47,10 +47,10 @@ test("spanOver rejects a missing register and a span over one READ", () => { ).toThrow("do not fit one READ"); }); -test("the sample span over the 0.1 descriptor is goal_position through ntc_raw", () => { +test("the sample span over the 0.1 descriptor is goal_duty through ntc_raw", () => { const span = spanOver(fields, SAMPLE_REGISTERS); - expect(span.addr).toBe(392); - expect(span.count).toBe(208); + expect(span.addr).toBe(390); + expect(span.count).toBe(210); }); test("the config and bias spans over the 0.1 descriptor fit one READ each", () => { @@ -85,6 +85,10 @@ test("decodeSpan reads little-endian values by width and sign", () => { expect(read("u32")).toBe(0x12345678); expect(read("i32")).toBe(-3); expect(() => read("nope")).toThrow("span has no nope"); + const flags = spanOver([field("on", 0, 1, "bool"), field("mode", 1, 1, "enum")], ["on", "mode"]); + const readFlags = decodeSpan(flags, new Uint8Array([1, 3])); + expect(readFlags("on")).toBe(1); + expect(readFlags("mode")).toBe(3); expect(() => decodeSpan(span, bytes.subarray(1))).toThrow("expected 14"); }); @@ -97,8 +101,13 @@ test("decodeSample scales omega_hat_cps out of Q16 and keeps the rest in counts" if (f === undefined) throw new Error(name); return f.addr - span.addr; }; + view.setInt16(at("goal_duty"), -1000, true); view.setInt32(at("goal_position"), 2048, true); + view.setInt32(at("goal_velocity"), -500, true); + view.setInt16(at("goal_current"), 250, true); + view.setUint8(at("mode_active"), 3); view.setInt32(at("omega_hat_cps"), -3 * 65536, true); + view.setInt16(at("duty_applied_q15"), -900, true); view.setUint16(at("pos"), 1234, true); view.setUint16(at("current"), 300, true); view.setUint16(at("vmotor_a"), 800, true); @@ -109,6 +118,11 @@ test("decodeSample scales omega_hat_cps out of Q16 and keeps the rest in counts" t: 1.5, pos: 1234, goal: 2048, + goalVelocity: -500, + goalCurrent: 250, + goalDuty: -1000, + dutyApplied: -900, + modeActive: 3, velocity: -3, current: 300, vbus: 3600, @@ -122,6 +136,11 @@ const sampleAt = (t: number): Sample => ({ t, pos: 0, goal: 0, + goalVelocity: 0, + goalCurrent: 0, + goalDuty: 0, + dutyApplied: 0, + modeActive: 0, velocity: 0, current: 0, vbus: 0, @@ -201,7 +220,7 @@ test("startTelemetry reads config, then biases, then samples on every tick", asy expect(configs[0]?.biases.currentBiasCounts).toBe(0x5352); now = 7; await vi.advanceTimersByTimeAsync(100); - expect(fake.calls.at(-1)).toEqual([392, 208]); + expect(fake.calls.at(-1)).toEqual([390, 210]); settle(fake); await vi.advanceTimersByTimeAsync(0); expect(samples).toHaveLength(1); diff --git a/src/lib/telemetry-poll.ts b/src/lib/telemetry-poll.ts index 759468d..959c32e 100644 --- a/src/lib/telemetry-poll.ts +++ b/src/lib/telemetry-poll.ts @@ -13,7 +13,6 @@ import { } from "./units"; export const POLL_HZ = 10; -export const WINDOW_S = 30; /** Largest READ reply one frame carries (protocol sec 3.1). */ export const READ_MAX = 252; /** omega_hat_cps is csQ16, (counts/s) x 2^16 (firmware regions/telemetry.rs). */ @@ -34,11 +33,19 @@ export interface Span { fields: FieldInfo[]; } -/** One poll in device counts: `velocity` is counts/s, `t` seconds. */ +/** + * One poll in device counts: `velocity` and `goalVelocity` are counts/s, the + * duties q15, `modeActive` the `mode` enum's discriminant, `t` seconds. + */ export interface Sample { t: number; pos: number; goal: number; + goalVelocity: number; + goalCurrent: number; + goalDuty: number; + dutyApplied: number; + modeActive: number; velocity: number; current: number; vbus: number; @@ -54,8 +61,13 @@ export interface TelemetryConfig { } export const SAMPLE_REGISTERS: readonly string[] = [ + "goal_duty", "goal_position", + "goal_velocity", + "goal_current", + "mode_active", "omega_hat_cps", + "duty_applied_q15", "pos", "current", "vmotor_a", @@ -106,8 +118,9 @@ export function decodeSpan(span: Span, bytes: Uint8Array): ReadRegister { }; } +/** Bools and enums decode as their unsigned byte. */ function decodeField(view: DataView, offset: number, { name, width, kind }: FieldInfo): number { - if (kind !== "uint" && kind !== "int") throw new Error(`${name} is ${kind}, not a number`); + if (kind === "bytes") throw new Error(`${name} is ${kind}, not a number`); const signed = kind === "int"; switch (width) { case 1: @@ -127,6 +140,11 @@ export function decodeSample(span: Span, bytes: Uint8Array, t: number): Sample { t, pos: read("pos"), goal: read("goal_position"), + goalVelocity: read("goal_velocity"), + goalCurrent: read("goal_current"), + goalDuty: read("goal_duty"), + dutyApplied: read("duty_applied_q15"), + modeActive: read("mode_active"), velocity: read("omega_hat_cps") / Q16, current: read("current"), vbus: read("vbus_raw"), diff --git a/src/routes/live.tsx b/src/routes/live.tsx index e0010a3..5e498d0 100644 --- a/src/routes/live.tsx +++ b/src/routes/live.tsx @@ -1,21 +1,66 @@ +import type { Descriptor, Field } from "@openservocore/client"; import { createFileRoute, Link } from "@tanstack/react-router"; -import { useEffect, useMemo, useState } from "react"; +import { Pause, Play, TriangleAlert } from "lucide-react"; +import { useEffect, useId, useMemo, useRef, useState, type KeyboardEvent } from "react"; import type uPlot from "uplot"; import { Chart, type ChartOptions } from "@/components/uplot"; -import { Card, CardContent, CardDescription, CardHeader, CardTitle } from "@/components/ui/card"; +import { Alert, AlertDescription, AlertTitle } from "@/components/ui/alert"; +import { Button } from "@/components/ui/button"; +import { + Card, + CardAction, + CardContent, + CardDescription, + CardHeader, + CardTitle, +} from "@/components/ui/card"; +import { Input } from "@/components/ui/input"; +import { Label } from "@/components/ui/label"; +import { + Select, + SelectContent, + SelectItem, + SelectTrigger, + SelectValue, +} from "@/components/ui/select"; +import { Slider } from "@/components/ui/slider"; import { Switch } from "@/components/ui/switch"; import { Tabs, TabsContent, TabsList, TabsTrigger } from "@/components/ui/tabs"; import { Tooltip, TooltipContent, TooltipTrigger } from "@/components/ui/tooltip"; import { useChartTokens, type ChartTokens } from "@/lib/chart-theme"; -import { inFlight } from "@/lib/in-flight"; +import { + clampGoal, + CONTROL_REGISTERS, + decodeControl, + dutyPercent, + GOAL_WRITE_GAP_MS, + goalOf, + goalSpec, + goalUnits, + isWindow, + LatestWins, + LIMIT_REGISTERS, + limitsFromTable, + modeLabel, + modeName, + WINDOWS_S, + type ControlState, + type GoalRegister, + type GoalUnits, + type Limits, + type ModeName, + type WindowS, +} from "@/lib/control"; import { isUnits, type Units } from "@/lib/prefs"; -import { useSession } from "@/lib/session"; +import { useSession, type Session } from "@/lib/session"; import { + decodeSpan, POLL_HZ, SampleRing, + spanOver, startTelemetry, - WINDOW_S, type Sample, + type Span, type TelemetryConfig, } from "@/lib/telemetry-poll"; import { @@ -53,33 +98,47 @@ interface PanelDef { } const DASH = [6, 4]; +/** The ring keeps the longest window, so a shorter one is a view over the same samples. */ +const RING_S: WindowS = 30; -const PANELS: readonly PanelDef[] = [ - { - key: "motion", - title: "Motion", - series: [ - { key: "position", label: "Position", token: "series1" }, - { key: "goal", label: "Goal", token: "series1", dash: DASH }, - { key: "velocity", label: "Velocity", token: "series2", right: true }, - ], - }, - { - key: "electrical", - title: "Electrical", - series: [ - { key: "current", label: "Current", token: "series3" }, - { key: "busVoltage", label: "Bus V", token: "ctx", right: true }, - { key: "motorVoltage", label: "Motor V", token: "ctx", dash: DASH, right: true }, - ], - }, - { - key: "temperature", - title: "Temperature", - series: [{ key: "temperature", label: "Temperature", token: "series2" }], - optional: true, - }, -]; +const POSITION: SeriesDef = { key: "position", label: "Position", token: "series1" }; +const VELOCITY: SeriesDef = { key: "velocity", label: "Velocity", token: "series2", right: true }; +const CURRENT: SeriesDef = { key: "current", label: "Current", token: "series3" }; +const BUS_V: SeriesDef = { key: "busVoltage", label: "Bus V", token: "ctx", right: true }; +const MOTOR_V: SeriesDef = { + key: "motorVoltage", + label: "Motor V", + token: "ctx", + dash: DASH, + right: true, +}; +const TEMPERATURE: SeriesDef = { key: "temperature", label: "Temperature", token: "series2" }; + +/** + * The dashed goal sits with the series it is the setpoint of, in that series' + * hue: nowhere in open loop, which has no duty axis. + */ +function panelsFor(mode: ModeName | undefined): PanelDef[] { + const goal = (of: SeriesDef): SeriesDef => ({ ...of, key: "goal", label: "Goal", dash: DASH }); + return [ + { + key: "motion", + title: "Motion", + series: [ + POSITION, + ...(mode === "Position" ? [goal(POSITION)] : []), + VELOCITY, + ...(mode === "Velocity" ? [goal(VELOCITY)] : []), + ], + }, + { + key: "electrical", + title: "Electrical", + series: [CURRENT, ...(mode === "Current" ? [goal(CURRENT)] : []), BUS_V, MOTOR_V], + }, + { key: "temperature", title: "Temperature", series: [TEMPERATURE], optional: true }, + ]; +} const OFF_BY_DEFAULT: readonly SeriesKey[] = ["motorVoltage", "temperature"]; /** Raw mode shows counts for these; the rest stay in real units. */ @@ -90,17 +149,22 @@ type Row = Record & { t: number }; function initialShown(): Shown { const shown = {} as Shown; - for (const panel of PANELS) { + for (const panel of panelsFor("Position")) { for (const s of panel.series) shown[s.key] = !OFF_BY_DEFAULT.includes(s.key); } return shown; } -function convert(s: Sample, { sense, cal, biases }: TelemetryConfig, raw: boolean): Row { +function convert( + s: Sample, + { sense, cal, biases }: TelemetryConfig, + raw: boolean, + goal: GoalUnits | undefined, +): Row { return { t: s.t, position: raw ? s.pos : positionDeg(s.pos, cal), - goal: raw ? s.goal : positionDeg(s.goal, cal), + goal: goal === undefined ? NaN : goal.toDisplay(goalOf(s, goal.register)), velocity: raw ? s.velocity : velocityDegPerS(s.velocity, cal), current: currentMa(s.current, biases.currentBiasCounts, sense), busVoltage: busV(s.vbus, sense), @@ -109,14 +173,23 @@ function convert(s: Sample, { sense, cal, biases }: TelemetryConfig, raw: boolea }; } -/** Rows with `t` relative to the newest sample, which sits at 0. */ -function toRows(samples: readonly Sample[], config: TelemetryConfig, raw: boolean): Row[] { +/** The last `windowS` seconds as rows with `t` relative to the newest sample, which sits at 0. */ +function toRows( + samples: readonly Sample[], + config: TelemetryConfig, + raw: boolean, + goal: GoalUnits | undefined, + windowS: number, +): Row[] { const last = samples.at(-1); if (last === undefined) return []; - return samples.map((s) => ({ ...convert(s, config, raw), t: s.t - last.t })); + return samples + .filter((s) => s.t >= last.t - windowS) + .map((s) => ({ ...convert(s, config, raw, goal), t: s.t - last.t })); } -function display(key: SeriesKey, raw: boolean): Display { +function display(key: SeriesKey, raw: boolean, goal: GoalUnits | undefined): Display { + if (key === "goal" && goal !== undefined) return { unit: goal.unit, digits: goal.digits }; if (!raw || !RAW_FAMILY.includes(key)) return DISPLAY[key]; if (key === "velocity") return { unit: `${DISPLAY.raw.unit}/s`, digits: DISPLAY.raw.digits }; return DISPLAY.raw; @@ -128,16 +201,21 @@ function format(value: number | undefined, d: Display): string { : `${value.toFixed(d.digits)} ${d.unit}`; } -function panelTitle(panel: PanelDef, raw: boolean): string { +function panelTitle(panel: PanelDef, raw: boolean, goal: GoalUnits | undefined): string { const unitOf = (right: boolean) => { const s = panel.series.find((x) => (x.right ?? false) === right); - return s === undefined ? undefined : display(s.key, raw).unit; + return s === undefined ? undefined : display(s.key, raw, goal).unit; }; const right = unitOf(true); return `${panel.title} ${unitOf(false) ?? ""}${right === undefined ? "" : ` | ${right}`}`; } -function panelOptions(panel: PanelDef, shown: Shown, tokens: ChartTokens): ChartOptions { +function panelOptions( + panel: PanelDef, + shown: Shown, + tokens: ChartTokens, + windowS: number, +): ChartOptions { const axis = { stroke: tokens.label, grid: { stroke: tokens.grid, width: 1 }, @@ -149,7 +227,7 @@ function panelOptions(panel: PanelDef, shown: Shown, tokens: ChartTokens): Chart return { legend: { show: false }, cursor: { drag: { x: false, y: false } }, - scales: { x: { time: false, range: [-WINDOW_S, 0] }, y: {}, ...(right ? { r: {} } : {}) }, + scales: { x: { time: false, range: [-windowS, 0] }, y: {}, ...(right ? { r: {} } : {}) }, axes: [ axis, { ...axis, scale: "y", size }, @@ -170,6 +248,10 @@ function panelOptions(panel: PanelDef, shown: Shown, tokens: ChartTokens): Chart }; } +function message(e: unknown): string { + return e instanceof Error ? e.message : String(e); +} + function LivePage() { const { status, selected } = useSession(); return ( @@ -198,22 +280,36 @@ function LivePage() { } function Telemetry({ id }: { id: number }) { - const { client, descriptor, descriptorError } = useSession(); + const { descriptor, descriptorError, run } = useSession(); const [config, setConfig] = useState(); const [samples, setSamples] = useState([]); + /** The samples on screen while paused; the poll keeps filling the ring behind them. */ + const [frozen, setFrozen] = useState(); + const [windowS, setWindowS] = useState(RING_S); const [error, setError] = useState(); const [shown, setShown] = useState(initialShown); + /** The servo's mode, switch and goals as last read back. */ + const [control, setControl] = useState(); const [unitsPref, setUnitsPref] = useUnitsPref(); const tokens = useChartTokens(); useEffect(() => { - if (client === undefined || descriptor === undefined) return; - const ring = new SampleRing(WINDOW_S); + if (descriptor === undefined) return; + const ring = new SampleRing(RING_S); + let pending = false; return startTelemetry({ fields: descriptor.fields(), - // Swap for the session's command queue once it lands: read through it, - // drop inFlight. - read: inFlight((addr, count) => client.read(id, addr, count)), + // A tick due while the last read is still queued is skipped, so a busy + // bus never piles reads up behind the writes. + read: async (addr, count) => { + if (pending) return undefined; + pending = true; + try { + return await run((c) => c.read(id, addr, count)); + } finally { + pending = false; + } + }, now: () => performance.now() / 1000, periodMs: 1000 / POLL_HZ, onConfig: setConfig, @@ -222,44 +318,103 @@ function Telemetry({ id }: { id: number }) { setSamples(ring.samples); }, onError: (e: unknown) => { - setError(e instanceof Error ? e.message : String(e)); + setError(message(e)); }, }); - }, [client, descriptor, id]); + }, [descriptor, id, run]); const calibrated = config !== undefined && calibrationStatus(config.cal).valid; const raw = !calibrated || unitsPref === "raw"; + const shownSamples = frozen ?? samples; + const latestSample = shownSamples.at(-1); + const modeField = useMemo( + () => descriptor?.fields().find((f) => f.name === "mode"), + [descriptor], + ); + // The mode register as read back, not mode_active: the servo republishes + // mode_active from it within one slow tick, and the simulated fleet never + // runs the kernel that would. + const mode = + modeField === undefined || control === undefined + ? undefined + : modeName(modeField.variants, control.mode); + const active = + modeField === undefined || latestSample === undefined + ? undefined + : modeName(modeField.variants, latestSample.modeActive); + const goal = useMemo( + () => + mode === undefined || config === undefined + ? undefined + : goalUnits(mode, { cal: config.cal, sense: config.sense, raw }), + [mode, config, raw], + ); + const panels = useMemo(() => panelsFor(mode), [mode]); const rows = useMemo( - () => (config === undefined ? [] : toRows(samples, config, raw)), - [samples, config, raw], + () => (config === undefined ? [] : toRows(shownSamples, config, raw, goal, windowS)), + [shownSamples, config, raw, goal, windowS], ); const latest = rows.at(-1); const problem = error ?? descriptorError; + const windowId = useId(); return (
Telemetry - - {POLL_HZ} Hz, last {WINDOW_S} s + + + + + + {problem !== undefined &&

{problem}

}
- {PANELS.filter((p) => !p.optional || p.series.some((s) => shown[s.key])).map( - (panel) => ( + {panels + .filter((p) => !p.optional || p.series.some((s) => shown[s.key])) + .map((panel) => ( - ), - )} + ))} {rows.length === 0 && (

waiting for data @@ -271,7 +426,21 @@ function Telemetry({ id }: { id: number }) {