also looking at this
feat: correlate VTOL hover mode observations
#77
10 files
+507
-28
| @@ -1,4 +1,4 @@ | ||
| 1 | import type { VehicleMissionAction, VehicleMissionObservation, VehicleMissionPlan, VehicleMissionSnapshot, VehicleMissionTask, VehiclePayloadAck, VehiclePayloadCommand } from '../vehicles/types' | |
| 1 | import type { VehicleMissionAction, VehicleMissionObservation, VehicleMissionPlan, VehicleMissionSnapshot, VehicleMissionTask, VehiclePayloadAck, VehiclePayloadCommand, VehicleVtolTransitionCommand, VehicleVtolTransitionObservation } from '../vehicles/types' | |
| 2 | 2 | import type { MissionState } from './engine' |
| 3 | 3 | import { authorityStates } from '../authority/supervisor' |
| 4 | 4 | import { validVector } from '../controls/model' |
| @@ -26,6 +26,11 @@ export function validateVehicleMission(input: unknown): { plan: VehicleMissionPl | ||
| 26 | 26 | let fields = ['id', 'kind'], edges: string[] = [], durationMs = 20 |
| 27 | 27 | if (t.kind === 'start') { fields.push('next'); if (t.id !== p.start || !id(t.next)) invalid(); edges = [t.next] } |
| 28 | 28 | else if (t.kind === 'abort') { /* Terminal release, never an emergency landing. */ } |
| 29 | else if (t.kind === 'vtol-mode') { | |
| 30 | fields.push('mode', 'headingRad', 'timeoutMs', 'next') | |
| 31 | if (p.vehicle !== 'vtol' || !['hover', 'cruise'].includes(t.mode) || !number(t.headingRad, -Math.PI, Math.PI) || !Number.isSafeInteger(t.timeoutMs) || !number(t.timeoutMs, 100, 15000) || !id(t.next)) invalid() | |
| 32 | durationMs = t.timeoutMs; edges = [t.next] | |
| 33 | } | |
| 29 | 34 | else if (t.kind === 'branch') { |
| 30 | 35 | fields.push('batteryAtLeast', 'then', 'otherwise') |
| 31 | 36 | if (!number(t.batteryAtLeast, p.power.reservePercent + 1, 100) || !id(t.then) || !id(t.otherwise)) invalid() |
| @@ -65,18 +70,29 @@ function validAck(value: unknown): value is VehiclePayloadAck { | ||
| 65 | 70 | if (value.metadata !== undefined && (!object(value.metadata) || Object.keys(value.metadata).length > 32 || Object.entries(value.metadata).some(([key, item]) => !/^[a-zA-Z][a-zA-Z0-9_-]{0,39}$/.test(key) || !(typeof item === 'boolean' || (typeof item === 'number' && Number.isFinite(item)) || (typeof item === 'string' && item.length <= 160))))) return false |
| 66 | 71 | return true |
| 67 | 72 | } |
| 73 | function validTransition(value: unknown): value is VehicleVtolTransitionObservation { | |
| 74 | return object(value) && keys(value, ['evidence', 'eventId', 'taskId', 'mode', 'reference', 'capturedAtMs', 'measurementCapturedAtMs', 'phase', 'outcome', 'reason']) | |
| 75 | && value.evidence === 'modeled-vtol-controller-observation' && typeof value.eventId === 'string' && value.eventId.length <= 256 && id(value.taskId) && ['hover', 'cruise'].includes(value.mode as string) | |
| 76 | && typeof value.reference === 'string' && value.reference.length <= 120 && Number.isSafeInteger(value.capturedAtMs) && (value.capturedAtMs as number) >= 0 | |
| 77 | && Number.isSafeInteger(value.measurementCapturedAtMs) && (value.measurementCapturedAtMs as number) >= 0 && (value.measurementCapturedAtMs as number) <= (value.capturedAtMs as number) | |
| 78 | && ['hover', 'front-transition', 'cruise', 'back-transition'].includes(value.phase as string) && ['progress', 'confirmed', 'aborted'].includes(value.outcome as string) | |
| 79 | && typeof value.reason === 'string' && /^[a-z][a-z0-9-]{0,79}$/.test(value.reason) | |
| 80 | } | |
| 68 | 81 | /** V2 mission policy shared by all vehicle profiles. It has no transport or arming capability. */ |
| 69 | 82 | export function createVehicleMissionEngine(input: unknown) { |
| 70 | 83 | const { plan, worstCaseMs } = validateVehicleMission(input) |
| 71 | 84 | const tasks = new Map(plan.tasks.map(t => [t.id, t])) |
| 85 | const hasTransitions = plan.tasks.some(t => t.kind === 'vtol-mode') | |
| 72 | 86 | let state: MissionState = 'ready', taskId = plan.start, reason = 'awaiting-operator-start' |
| 73 | 87 | let lastNow: number | null = null, startedAt: number | null = null, reference: string | null = null, taskElapsedMs = 0, settledMs = 0 |
| 74 | 88 | let targetM: VehicleMissionSnapshot['targetM'] = null, holdM: VehicleMissionSnapshot['targetM'] = null, pending: VehiclePayloadCommand | null = null |
| 89 | let transition: VehicleVtolTransitionCommand | null = null | |
| 90 | let transitionObservation: { capturedAtMs: number, measurementCapturedAtMs: number, signature: string } | null = null | |
| 75 | 91 | const rows: VehicleMissionTrace['rows'] = [] |
| 76 | function snapshot(): VehicleMissionSnapshot { return { version: 2, vehicle: plan.vehicle, evidence: 'owned-vehicle-mission-simulation', physicalFlightEnabled: false, state, taskId, taskKind: tasks.get(taskId)!.kind, targetM: targetM ? [...targetM] : null, payload: pending ? structuredClone(pending) : null, reason, elapsedMs: startedAt === null || lastNow === null ? 0 : lastNow - startedAt } } | |
| 77 | function block(why: string) { state = 'blocked'; reason = why; targetM = null; pending = null } | |
| 92 | function snapshot(): VehicleMissionSnapshot { return { version: 2, vehicle: plan.vehicle, evidence: 'owned-vehicle-mission-simulation', physicalFlightEnabled: false, state, taskId, taskKind: tasks.get(taskId)!.kind, targetM: targetM ? [...targetM] : null, payload: pending ? structuredClone(pending) : null, reason, elapsedMs: startedAt === null || lastNow === null ? 0 : lastNow - startedAt, ...(hasTransitions ? { transition: transition ? structuredClone(transition) : null } : {}) } } | |
| 93 | function block(why: string) { state = 'blocked'; reason = why; targetM = null; pending = null; transition = null; transitionObservation = null } | |
| 78 | 94 | function advance(next: string) { |
| 79 | taskId = next; taskElapsedMs = 0; settledMs = 0; pending = null; reason = 'task-entered' | |
| 95 | taskId = next; taskElapsedMs = 0; settledMs = 0; pending = null; transition = null; transitionObservation = null; reason = 'task-entered' | |
| 80 | 96 | const task = tasks.get(next)! |
| 81 | 97 | const goal = vehicleTaskTarget(plan, task) |
| 82 | 98 | if (goal) targetM = goal as VehicleMissionSnapshot['targetM'] |
| @@ -90,7 +106,7 @@ export function createVehicleMissionEngine(input: unknown) { | ||
| 90 | 106 | let o: VehicleMissionObservation |
| 91 | 107 | try { o = structuredClone(inputObservation) } |
| 92 | 108 | catch { block('invalid-observation-or-clock'); throw new TypeError('Invalid vehicle mission observation') } |
| 93 | if (!object(o) || !keys(o, ['kind', 'nowMs', 'reference', 'origin', 'healthy', 'positionM', 'velocityMps', 'authority', 'batteryPercent', 'onGround', 'payloadAck']) || Object.keys(o).length !== 11 || o.kind !== 'simulation' || !Number.isSafeInteger(o.nowMs) || o.nowMs < 0 || (lastNow !== null && o.nowMs <= lastNow) || typeof o.reference !== 'string' || o.reference.length > 120 || !/^[1-9][0-9]{0,19}:[0-9]+,[0-9]+,[0-9]+,[0-9]+,[0-9]+$/.test(o.reference) || !object(o.origin) || !keys(o.origin, ['latitudeDeg', 'longitudeDeg', 'ellipsoidHeightM']) || o.origin.latitudeDeg !== plan.origin.latitudeDeg || o.origin.longitudeDeg !== plan.origin.longitudeDeg || o.origin.ellipsoidHeightM !== plan.origin.ellipsoidHeightM || typeof o.healthy !== 'boolean' || typeof o.onGround !== 'boolean' || !validVector(o.positionM) || !validVector(o.velocityMps) || !authorityStates.includes(o.authority) || !number(o.batteryPercent, 0, 100) || (o.payloadAck !== null && !validAck(o.payloadAck)) || (action !== undefined && !['start', 'pause', 'resume', 'abort', 'recover'].includes(action))) { block('invalid-observation-or-clock'); throw new TypeError('Invalid vehicle mission observation') } | |
| 109 | if (!object(o) || !keys(o, ['kind', 'nowMs', 'reference', 'origin', 'healthy', 'positionM', 'velocityMps', 'authority', 'batteryPercent', 'onGround', 'payloadAck', ...(hasTransitions ? ['transition'] : [])]) || Object.keys(o).length !== (hasTransitions ? 12 : 11) || o.kind !== 'simulation' || !Number.isSafeInteger(o.nowMs) || o.nowMs < 0 || (lastNow !== null && o.nowMs <= lastNow) || typeof o.reference !== 'string' || o.reference.length > 120 || !/^[1-9][0-9]{0,19}:[0-9]+,[0-9]+,[0-9]+,[0-9]+,[0-9]+$/.test(o.reference) || !object(o.origin) || !keys(o.origin, ['latitudeDeg', 'longitudeDeg', 'ellipsoidHeightM']) || o.origin.latitudeDeg !== plan.origin.latitudeDeg || o.origin.longitudeDeg !== plan.origin.longitudeDeg || o.origin.ellipsoidHeightM !== plan.origin.ellipsoidHeightM || typeof o.healthy !== 'boolean' || typeof o.onGround !== 'boolean' || !validVector(o.positionM) || !validVector(o.velocityMps) || !authorityStates.includes(o.authority) || !number(o.batteryPercent, 0, 100) || (o.payloadAck !== null && !validAck(o.payloadAck)) || (hasTransitions && o.transition !== null && !validTransition(o.transition)) || (action !== undefined && !['start', 'pause', 'resume', 'abort', 'recover'].includes(action))) { block('invalid-observation-or-clock'); throw new TypeError('Invalid vehicle mission observation') } | |
| 94 | 110 | const dt = lastNow === null ? 0 : o.nowMs - lastNow, previousState = state |
| 95 | 111 | lastNow = o.nowMs |
| 96 | 112 | if (active(state)) { |
| @@ -102,15 +118,17 @@ export function createVehicleMissionEngine(input: unknown) { | ||
| 102 | 118 | else if (o.batteryPercent <= plan.power.reservePercent) block('mission-battery-reserve') |
| 103 | 119 | else if (o.nowMs - startedAt! > worstCaseMs) block('mission-endurance-exhausted') |
| 104 | 120 | else if (o.payloadAck !== null && pending === null) block('unsolicited-payload-ack') |
| 121 | else if (o.transition && transition === null) block('unsolicited-vtol-transition') | |
| 105 | 122 | } |
| 106 | if (action === 'abort' && (active(state) || state === 'ready')) { state = 'aborted'; reason = 'operator-abort'; targetM = null; pending = null } | |
| 123 | if (action === 'abort' && (active(state) || state === 'ready')) { state = 'aborted'; reason = 'operator-abort'; targetM = null; pending = null; transition = null; transitionObservation = null } | |
| 107 | 124 | else if (action === 'start' && state === 'ready') { |
| 108 | 125 | const needed = plan.power.reservePercent + 100 * plan.power.maxW * worstCaseMs / 3600000 / plan.power.availableWh |
| 109 | if (!o.healthy || o.authority !== 'autonomous' || !o.onGround || Math.hypot(...o.velocityMps) > 0.1 || !vehicleMissionContains(plan, o.positionM) || o.batteryPercent < needed || o.payloadAck !== null) block('mission-start-interlocks') | |
| 126 | if (!o.healthy || o.authority !== 'autonomous' || !o.onGround || Math.hypot(...o.velocityMps) > 0.1 || !vehicleMissionContains(plan, o.positionM) || o.batteryPercent < needed || o.payloadAck !== null || o.transition) block('mission-start-interlocks') | |
| 110 | 127 | else { state = 'running'; startedAt = o.nowMs; reference = o.reference; holdM = [...o.positionM]; targetM = [...o.positionM]; reason = 'operator-start' } |
| 111 | 128 | } |
| 112 | 129 | else if ((action === 'pause' && state === 'running') || (action === 'recover' && active(state))) { |
| 113 | 130 | if (pending) block('payload-recovery-uncertain') |
| 131 | else if (transition) block('vtol-transition-recovery-uncertain') | |
| 114 | 132 | else { state = 'paused'; holdM = [...o.positionM]; settledMs = 0; reason = action === 'recover' ? 'checkpoint-recovered-awaiting-resume' : 'operator-pause' } |
| 115 | 133 | } |
| 116 | 134 | else if (action === 'resume' && state === 'paused') { state = 'running'; settledMs = 0; reason = 'operator-resume' } |
| @@ -118,10 +136,36 @@ export function createVehicleMissionEngine(input: unknown) { | ||
| 118 | 136 | if (state === 'running') { |
| 119 | 137 | const t = tasks.get(taskId)!, executionDt = previousState === 'ready' || previousState === 'paused' ? 0 : dt |
| 120 | 138 | taskElapsedMs += executionDt |
| 121 | if ('timeoutMs' in t && taskElapsedMs >= t.timeoutMs) block(t.kind === 'payload' ? 'payload-outcome-uncertain' : 'task-timeout') | |
| 139 | if ('timeoutMs' in t && taskElapsedMs >= t.timeoutMs) block(t.kind === 'payload' ? 'payload-outcome-uncertain' : t.kind === 'vtol-mode' ? 'vtol-transition-outcome-uncertain' : 'task-timeout') | |
| 122 | 140 | else if (t.kind === 'abort') { state = 'aborted'; reason = 'mission-abort-task'; targetM = null } |
| 123 | 141 | else if (t.kind === 'start') advance(t.next) |
| 124 | 142 | else if (t.kind === 'branch') { advance(o.batteryPercent >= t.batteryAtLeast ? t.then : t.otherwise); reason = 'battery-branch' } |
| 143 | else if (t.kind === 'vtol-mode') { | |
| 144 | if (!transition) { | |
| 145 | transition = { eventId: `${plan.id}:${reference}:${t.id}`, taskId: t.id, mode: t.mode, headingRad: t.headingRad, issuedAtMs: o.nowMs, expiresAtMs: o.nowMs + t.timeoutMs - taskElapsedMs, positionM: [...o.positionM], reference: reference! } | |
| 146 | reason = 'vtol-transition-requested' | |
| 147 | } | |
| 148 | targetM = [...transition.positionM] | |
| 149 | const observed = o.transition | |
| 150 | if (observed) { | |
| 151 | const phases = transition.mode === 'hover' ? ['hover', 'back-transition'] : ['front-transition', 'cruise'] | |
| 152 | if (observed.eventId !== transition.eventId || observed.taskId !== transition.taskId || observed.mode !== transition.mode || observed.reference !== transition.reference | |
| 153 | || observed.capturedAtMs < transition.issuedAtMs || observed.capturedAtMs >= transition.expiresAtMs || observed.capturedAtMs > o.nowMs || o.nowMs - observed.capturedAtMs > 120 | |
| 154 | || observed.measurementCapturedAtMs < transition.issuedAtMs || o.nowMs - observed.measurementCapturedAtMs > 120 | |
| 155 | || observed.outcome === 'confirmed' && observed.phase !== transition.mode || observed.outcome === 'progress' && !phases.includes(observed.phase)) block('vtol-transition-outcome-uncertain') | |
| 156 | else { | |
| 157 | // Wire object order is not evidence identity; all accepted fields are scalar and closed above. | |
| 158 | const signature = JSON.stringify(Object.entries(observed).sort(([a], [b]) => a < b ? -1 : a > b ? 1 : 0)) | |
| 159 | if (transitionObservation && (observed.capturedAtMs < transitionObservation.capturedAtMs || observed.measurementCapturedAtMs < transitionObservation.measurementCapturedAtMs || observed.capturedAtMs === transitionObservation.capturedAtMs && signature !== transitionObservation.signature)) block('vtol-transition-observation-replayed') | |
| 160 | else { | |
| 161 | transitionObservation = { capturedAtMs: observed.capturedAtMs, measurementCapturedAtMs: observed.measurementCapturedAtMs, signature } | |
| 162 | if (observed.outcome === 'aborted') block('vtol-transition-aborted') | |
| 163 | else if (observed.outcome === 'confirmed') { advance(t.next); reason = 'vtol-transition-confirmed' } | |
| 164 | else reason = 'vtol-transition-progress' | |
| 165 | } | |
| 166 | } | |
| 167 | } | |
| 168 | } | |
| 125 | 169 | else if (t.kind === 'payload') { |
| 126 | 170 | pending ??= { eventId: `${plan.id}:${reference}:${t.id}`, taskId: t.id, operation: t.operation, containerId: t.containerId, issuedAtMs: o.nowMs, expiresAtMs: o.nowMs + t.timeoutMs - taskElapsedMs, positionM: [...o.positionM], reference: reference! } |
| 127 | 171 | const ack = o.payloadAck |
| @@ -169,13 +213,13 @@ export function recoverVehicleMission(trace: VehicleMissionTrace, observation: V | ||
| 169 | 213 | if (!active(engine.snapshot().state) || engine.step(observation, 'recover').state !== 'paused') throw new TypeError('Vehicle checkpoint cannot recover without supervised compatible authority') |
| 170 | 214 | return engine |
| 171 | 215 | } |
| 172 | /** Verify the whole recording before selecting an exact active, settled-payload prefix. */ | |
| 216 | /** Verify the whole recording before selecting an active prefix without pending payload or transition work. */ | |
| 173 | 217 | export function checkpointVehicleMission(trace: VehicleMissionTrace, atMs: number): VehicleMissionTrace { |
| 174 | 218 | const verified = replayVehicleMission(trace).trace() |
| 175 | 219 | if (!Number.isSafeInteger(atMs) || atMs < 0 || !verified.rows.some(row => row.observation.nowMs === atMs)) throw new RangeError('Checkpoint epoch must match a recorded observation') |
| 176 | 220 | verified.rows = verified.rows.filter(row => row.observation.nowMs <= atMs) |
| 177 | 221 | const snapshot = replayVehicleMission(verified).snapshot() |
| 178 | if (!active(snapshot.state) || snapshot.payload !== null) throw new TypeError('Vehicle mission checkpoint is not resumable or has an uncertain payload') | |
| 222 | if (!active(snapshot.state) || snapshot.payload !== null || snapshot.transition) throw new TypeError('Vehicle mission checkpoint is not resumable or has an uncertain payload') | |
| 179 | 223 | return verified |
| 180 | 224 | } |
| 181 | 225 | /** Operator resume never clears authority faults or retries a pending payload. */ |