also looking at this
feat: add shared-clock scoped vehicle simulation sessions
#98
42 files
+2764
-127
| @@ -25,8 +25,30 @@ function captureMissionActions(input: unknown, maximumMs: number): ScheduledMiss | ||
| 25 | 25 | } |
| 26 | 26 | return result |
| 27 | 27 | } |
| 28 | /** Owned raw sensor -> estimator -> mission -> vehicle policy -> authority -> independent board loop. */ | |
| 29 | export function runVehicleSimulation(profileInput: VehicleProfile, options: { scenario?: string, seconds?: number, neutralTailMs?: number, missionActions?: readonly ScheduledMissionAction[] } = {}) { | |
| 28 | function captureLinkObservation(input: unknown, nowMs: number): { capturedAtMs: number, connected: boolean } { | |
| 29 | if (!input || typeof input !== 'object' || Object.getPrototypeOf(input) !== Object.prototype || Reflect.ownKeys(input).length !== 2) throw new TypeError('Unqualified vehicle link observation') | |
| 30 | const time = Object.getOwnPropertyDescriptor(input, 'capturedAtMs'), connected = Object.getOwnPropertyDescriptor(input, 'connected') | |
| 31 | if (!time?.enumerable || !Object.hasOwn(time, 'value') || !Number.isSafeInteger(time.value) || time.value < 0 || time.value > nowMs || !connected?.enumerable || !Object.hasOwn(connected, 'value') || typeof connected.value !== 'boolean') throw new TypeError('Unqualified vehicle link observation') | |
| 32 | return { capturedAtMs: time.value, connected: connected.value } | |
| 33 | } | |
| 34 | export interface VehicleSimulationOptions { | |
| 35 | scenario?: string | |
| 36 | seconds?: number | |
| 37 | neutralTailMs?: number | |
| 38 | missionActions?: readonly ScheduledMissionAction[] | |
| 39 | scope?: { bootId: bigint, sessionId: bigint } | |
| 40 | } | |
| 41 | function captureSimulationScope(input: unknown) { | |
| 42 | if (input === undefined) return { bootId: 55n, sessionId: 123n } | |
| 43 | if (!input || typeof input !== 'object' || ![Object.prototype, null].includes(Object.getPrototypeOf(input)) || Reflect.ownKeys(input).length !== 2) throw new TypeError('Invalid vehicle simulation scope') | |
| 44 | const boot = Object.getOwnPropertyDescriptor(input, 'bootId'), session = Object.getOwnPropertyDescriptor(input, 'sessionId') | |
| 45 | if (!boot?.enumerable || !Object.hasOwn(boot, 'value') || !session?.enumerable || !Object.hasOwn(session, 'value') || [boot.value, session.value].some(value => typeof value !== 'bigint' || value < 1n || value > 0xffffffffffffffffn)) throw new TypeError('Invalid vehicle simulation scope') | |
| 46 | return { bootId: boot.value as bigint, sessionId: session.value as bigint } | |
| 47 | } | |
| 48 | /** Retained owned loop, advanced at exactly 4 ms epochs from zero by its caller. */ | |
| 49 | export function createVehicleSimulationSession(profileInput: VehicleProfile, options: VehicleSimulationOptions = {}) { | |
| 50 | const scope = captureSimulationScope(options.scope), vehicle = profileInput.vehicle | |
| 51 | const runtimeFaults = profileInput.faults.bind(profileInput) | |
| 30 | 52 | const scenario = options.scenario ?? profileInput.scenario, seconds = options.seconds ?? 120 |
| 31 | 53 | const neutralTailMs = options.neutralTailMs ?? 0 |
| 32 | 54 | const positionPrediction = profileInput.positionPrediction |
| @@ -37,20 +59,23 @@ export function runVehicleSimulation(profileInput: VehicleProfile, options: { sc | ||
| 37 | 59 | const missionActions = captureMissionActions(options.missionActions, seconds * 1000) |
| 38 | 60 | const { plan, worstCaseMs, requiredWh } = validateVehicleMission(profileInput.mission) |
| 39 | 61 | const batteryModel = captureVehicleBatteryModel(profileInput.batteryModel, plan.power) |
| 40 | if (profileInput.vehicle !== plan.vehicle || plan.origin.latitudeDeg !== 0 || plan.origin.longitudeDeg !== 0 || plan.origin.ellipsoidHeightM !== 100) throw new TypeError('Vehicle fixture requires its declared 0,0,100 WGS84 origin') | |
| 62 | if (vehicle !== plan.vehicle || plan.origin.latitudeDeg !== 0 || plan.origin.longitudeDeg !== 0 || plan.origin.ellipsoidHeightM !== 100) throw new TypeError('Vehicle fixture requires its declared 0,0,100 WGS84 origin') | |
| 41 | 63 | const hasTransitions = plan.tasks.some(task => task.kind === 'vtol-mode') |
| 42 | 64 | const captureMotion = plan.tasks.some(task => task.kind === 'vtol-mode' && task.approach === 'bounded-approach-v1') |
| 43 | 65 | let transitionObservation: VehicleVtolTransitionObservation | null = null |
| 44 | 66 | const actuators = structuredClone(profileInput.actuators), fence = structuredClone(profileInput.authorityFence) |
| 45 | 67 | const session = profileInput.createSession(), navigation = session.createNavigation?.() |
| 46 | if (profileInput.vehicle === 'subsea' && !navigation) throw new TypeError('Subsea profile requires owned pressure/DVL navigation') | |
| 68 | const linkProvider = session.linkObservation, renderMeasurement = session.renderMeasurement.bind(session) | |
| 69 | const observeIndependent = session.observeIndependent?.bind(session), updateNavigation = navigation?.update.bind(navigation) | |
| 70 | if (vehicle === 'subsea' && !navigation) throw new TypeError('Subsea profile requires owned pressure/DVL navigation') | |
| 47 | 71 | const engine = createVehicleMissionEngine(plan), plant = session.createPlant(), controller = session.createController() |
| 48 | const supervisor = createVehicleAuthoritySupervisor({ bootId: 55n, sessionId: 123n, vehicle: profileInput.vehicle, actuators, fence }) | |
| 49 | const board = createVehicleOutputWatchdog({ bootId: 55n, sessionId: 123n, vehicle: profileInput.vehicle, actuators }) | |
| 50 | const feed = createPhysicsSensorFeed(0), estimator = createEstimator({ origin: plan.origin, gravityCorrection: 'initialization-only', positionPrediction }) | |
| 51 | const qualifier = createVehicleMeasurementQualifier(profileInput.vehicle, batteryModel ?? undefined) | |
| 72 | const plantSnapshot = plant.snapshot.bind(plant), stepPlant = plant.step.bind(plant), stepController = controller.step.bind(controller) | |
| 73 | const supervisor = createVehicleAuthoritySupervisor({ ...scope, vehicle: vehicle, actuators, fence }) | |
| 74 | const board = createVehicleOutputWatchdog({ ...scope, vehicle: vehicle, actuators }) | |
| 75 | const feed = createPhysicsSensorFeed(0, scope.bootId), estimator = createEstimator({ origin: plan.origin, gravityCorrection: 'initialization-only', positionPrediction }) | |
| 76 | const qualifier = createVehicleMeasurementQualifier(vehicle, batteryModel ?? undefined) | |
| 52 | 77 | const observedSensors = { imu: 0, gnss: 0 }, sensorSequences = { imu: 0, gnss: 0 } |
| 53 | let truth = plant.snapshot(), mission = engine.snapshot() | |
| 78 | let truth = structuredClone(plantSnapshot()), mission = engine.snapshot() | |
| 54 | 79 | let estimate: EstimatorSnapshot = estimator.snapshot(0) |
| 55 | 80 | let measurement: VehicleMeasurement | null = null, measurementPose: VehicleMeasurementPose | null = null |
| 56 | 81 | let batteryObservation: VehicleBatteryObservation | null = null |
| @@ -61,98 +86,165 @@ export function runVehicleSimulation(profileInput: VehicleProfile, options: { sc | ||
| 61 | 86 | const transitions: { atMs: number, state: string, taskId: string, reason: string }[] = [] |
| 62 | 87 | let previousState = '', previousTask = '' |
| 63 | 88 | const payload = session.createPayload?.() |
| 64 | let finalAtMs = 0 | |
| 65 | try { | |
| 66 | for (let nowMs = 0; nowMs <= seconds * 1000; nowMs += 4) { | |
| 89 | const observePayload = payload?.observe?.bind(payload), pollPayload = payload?.poll.bind(payload), requestPayload = payload?.request.bind(payload), closePayload = payload?.close.bind(payload) | |
| 90 | let finalAtMs = 0, nextAtMs = 0, policyAtMs: number | null = null, hostEndAtMs: number | null = null | |
| 91 | let terminal = false, stepping = false | |
| 92 | const interrupted = Symbol('vehicle-session-closed') | |
| 93 | function owned<T>(work: () => T): T { | |
| 94 | if (terminal) throw interrupted | |
| 95 | const value = work() | |
| 96 | if (terminal) throw interrupted | |
| 97 | return value | |
| 98 | } | |
| 99 | function captured<T>(work: () => T): T { return owned(() => structuredClone(work())) } | |
| 100 | function stepHost(nowMs: number) { | |
| 101 | const faults = captured(() => runtimeFaults(scenario, nowMs, 'host')), boardState = board.tick(nowMs) | |
| 102 | if (nowMs > 600) truth = captured(() => stepPlant(boardState.actuators, 0.004, { windMps: faults.windMps ?? [0, 0, 0], currentMps: faults.currentMps ?? [0, 0, 0] })) | |
| 103 | if (faults.hostLost) hostLostAt ??= nowMs | |
| 104 | if (nowMs >= 600) owned(() => observeIndependent?.(structuredClone(truth), nowMs, [...boardState.actuators], hostLostAt === null)) | |
| 105 | if (hostLostAt !== null) { | |
| 106 | if (nowMs - hostLostAt >= 100) { status = 'host-lost'; fault = board.snapshot().fault ?? 'host-lost'; return false } | |
| 107 | return true | |
| 108 | } | |
| 109 | feed.capture(truth, nowMs, faults.dropImu, vehicle === 'subsea' || faults.dropPosition) | |
| 110 | const sensors = feed.advanceTo(nowMs) | |
| 111 | for (const kind of ['imu', 'gnss'] as const) { | |
| 112 | const sequence = sensors.sensors.find(sensor => sensor.kind === kind)?.observation?.sequence ?? 0 | |
| 113 | if (sequence > sensorSequences[kind]) { observedSensors[kind]++; sensorSequences[kind] = sequence } | |
| 114 | } | |
| 115 | const baseEstimate = estimator.update(sensors) | |
| 116 | estimate = updateNavigation ? captured(() => updateNavigation(baseEstimate, measurement, nowMs)) : baseEstimate | |
| 117 | if (nowMs % 20 !== 0) return true | |
| 118 | const reference = vehicleReference(estimate) | |
| 119 | const imuObservation = sensors.sensors.find(s => s.kind === 'imu')?.observation | |
| 120 | const capturePose = predictVehicleMeasurementPose(estimate, imuObservation, nowMs, { includeMotion: captureMotion }) | |
| 121 | if ((capturePose || vehicle === 'subsea') && !faults.dropVehicleSensor) { | |
| 122 | if (queue.length >= 8) throw new RangeError('Vehicle sensor queue exhausted') | |
| 123 | const packet = captured(() => renderMeasurement(truth, nowMs, reference)) | |
| 124 | if (packet.capturedAtMs !== nowMs || !Number.isSafeInteger(packet.receivedAtMs) || packet.receivedAtMs < nowMs + 20 || packet.receivedAtMs > nowMs + 100) throw new TypeError('Vehicle renderer must declare capture and bounded receipt epochs') | |
| 125 | queue.push({ measurement: packet, pose: capturePose }) | |
| 126 | queue.sort((a, b) => a.measurement.receivedAtMs - b.measurement.receivedAtMs || a.measurement.sequence - b.measurement.sequence) | |
| 127 | } | |
| 128 | while (queue[0] && queue[0].measurement.receivedAtMs <= nowMs) { const entry = queue.shift()!; measurement = entry.measurement; measurementPose = entry.pose } | |
| 129 | let qualified: VehicleMeasurement | null = null | |
| 130 | try { qualified = qualifier.qualify(measurement, nowMs, reference) } | |
| 131 | catch (error) { fault ??= error instanceof Error ? error.message : 'vehicle-sensor-envelope'; measurementPose = null } | |
| 132 | measurement = qualified | |
| 133 | if (batteryModel && qualified) { | |
| 134 | batteryObservation = observeVehicleBattery(qualified, batteryModel, nowMs) | |
| 135 | if (batteryObservation.cutoff) fault ??= 'modeled-battery-supply-cutoff' | |
| 136 | } | |
| 137 | if (updateNavigation) estimate = captured(() => updateNavigation(baseEstimate, qualified, nowMs)) | |
| 138 | let link = linkProvider === undefined ? { capturedAtMs: nowMs, connected: !faults.linkLost } : { capturedAtMs: 0, connected: false } | |
| 139 | if (typeof linkProvider === 'function') { | |
| 140 | try { link = captured(() => captureLinkObservation(linkProvider.call(session, nowMs), nowMs)) } | |
| 141 | catch (error) { if (error === interrupted || terminal) throw error; /* An installed adapter failure is disconnected, never a replacement fixture. */ } | |
| 142 | } | |
| 143 | if (faults.linkLost) link.connected = false | |
| 144 | if (nowMs < 600) return true | |
| 145 | const b = board.snapshot() | |
| 146 | const batteryPercent = batteryModel ? qualified && batteryObservation && !batteryObservation.cutoff ? batteryObservation.remainingPercentLowerBound : 0 : 100 - plan.power.maxW * Math.max(0, nowMs - 600) / 3600000 / plan.power.availableWh * 100 | |
| 147 | const observation = { kind: 'simulation' as const, nowMs, estimate, | |
| 148 | runtime: { bootId: b.bootId, capturedAtMs: nowMs, watchdogHealthy: b.fault === null }, | |
| 149 | rc: { capturedAtMs: nowMs, connected: true, armSwitch: true, override: faults.rcOverride ?? false }, | |
| 150 | interlocks: { capturedAtMs: qualified?.capturedAtMs ?? nowMs - 1000, killActive: faults.kill ?? false, onGround: qualified?.values.onSurface === true, motorsStopped: qualified?.values.stopped === true }, | |
| 151 | battery: { capturedAtMs: batteryModel ? qualified?.capturedAtMs ?? nowMs - 1000 : nowMs, remainingPercent: batteryPercent }, link } | |
| 152 | const send = (action: 'arm' | 'engage' | 'renew' | 'disarm', targetM = mission.targetM) => { | |
| 153 | const s = supervisor.snapshot() | |
| 154 | return supervisor.update(observation, { version: 1, sessionId: s.sessionId, bootId: s.bootId, epoch: s.epoch, sequence: s.sequence + 1, issuedAtMs: nowMs, action, mode: 'position', leaseMs: 100, target: { positionM: targetM ?? [0, 0, 0], yawRad: 0 } }) | |
| 155 | } | |
| 156 | supervisor.update(observation) | |
| 157 | if (nowMs === 600) { send('arm'); send('engage', [0, 0, 0]) } | |
| 158 | const context: VehicleControllerContext = { nowMs, enabled: supervisor.snapshot().state === 'autonomous', estimate, measurement, measurementPose, mission, | |
| 159 | ...(vehicle === 'vtol' || vehicle === 'quadcopter' ? { controlRate: capturePose ? { evidence: 'modeled-qualified-imu' as const, reference: capturePose.reference, capturedAtMs: capturePose.gyroCapturedAtMs, angularRateRadS: [...(imuObservation!.value as ImuMeasurement).angularRateRadS] as SensorVector } : null } : {}), | |
| 160 | } | |
| 161 | owned(() => observePayload?.(structuredClone(context))) | |
| 162 | const ack = captured(() => pollPayload?.(nowMs)) ?? null | |
| 163 | const missionObservation = { ...observeMission({ estimate, authority: supervisor.snapshot().state, batteryPercent, onGround: qualified?.values.onSurface === true }, vehicle === 'subsea' ? 'LOCAL_WATER_SURFACE' : 'WGS84_ELLIPSOID'), ...(!estimate.localPosition ? { origin: structuredClone(plan.origin), positionM: null, velocityMps: null } : {}), payloadAck: ack, ...(hasTransitions ? { transition: transitionObservation } : {}) } | |
| 164 | transitionObservation = null | |
| 165 | let action: VehicleMissionAction | undefined = nowMs === 600 ? 'start' : undefined | |
| 166 | const scheduled = missionActions[missionActionIndex] | |
| 167 | if (scheduled?.atMs === nowMs) { action = scheduled.action; missionActionIndex++ } | |
| 168 | mission = engine.step(missionObservation, action) | |
| 169 | if (mission.payloadFailure?.triggeredAtMs === nowMs) owned(() => observePayload?.(structuredClone({ ...context, mission }))) | |
| 170 | if (mission.payload && !requested.has(mission.payload.eventId)) { | |
| 171 | if (!payload) { fault ??= 'missing-vehicle-payload-port'; send('disarm') } | |
| 172 | else { owned(() => observePayload?.(structuredClone({ ...context, mission }))); owned(() => requestPayload!(structuredClone(mission.payload!))); requested.add(mission.payload.eventId) } | |
| 173 | } | |
| 174 | const active = mission.state === 'running' || mission.state === 'paused' | |
| 175 | if (active && mission.targetM && !fault) send('renew') | |
| 176 | else { send('disarm'); board.revoke() } | |
| 177 | const output: VehicleControlOutput = captured(() => stepController({ ...context, enabled: supervisor.snapshot().state === 'autonomous', mission })) | |
| 178 | if (hasTransitions && active && !output.inhibited && supervisor.snapshot().state === 'autonomous') transitionObservation = output.transition ? structuredClone(output.transition) : null | |
| 179 | if (active && output.inhibited && supervisor.snapshot().state === 'autonomous') { fault ??= output.reason; send('disarm'); board.revoke() } | |
| 180 | const frame = supervisor.gate(output, nowMs) | |
| 181 | if (frame && !terminal) board.accept(frame, nowMs) | |
| 182 | else board.revoke() | |
| 183 | policyAtMs = nowMs | |
| 184 | path.push({ atMs: nowMs, positionM: estimate.localPosition ? [...estimate.localPosition.positionM] as SensorVector : null, truthPositionM: [...truth.positionM], mission: structuredClone(mission), measurement: measurement ? structuredClone(measurement) : null, measurementPose: measurementPose ? structuredClone(measurementPose) : null, actuators: [...board.snapshot().actuators], ...(hasTransitions ? { transitionObservation: transitionObservation ? structuredClone(transitionObservation) : null } : {}), ...(output.backCapture ? { backCapture: structuredClone(output.backCapture) } : {}) }) | |
| 185 | if (mission.state !== previousState || mission.taskId !== previousTask) { transitions.push({ atMs: nowMs, state: mission.state, taskId: mission.taskId, reason: mission.reason }); previousState = mission.state; previousTask = mission.taskId } | |
| 186 | if (!['ready', 'running', 'paused'].includes(mission.state)) { status = mission.state === 'completed' ? 'completed' : 'blocked'; fault ??= mission.state === 'completed' ? null : mission.reason; return false } | |
| 187 | return true | |
| 188 | } | |
| 189 | function finishHost() { | |
| 190 | if (hostEndAtMs !== null) return | |
| 191 | hostEndAtMs = finalAtMs | |
| 192 | try { closePayload?.() } | |
| 193 | finally { if (status !== 'host-lost') board.revoke() } | |
| 194 | } | |
| 195 | function close() { | |
| 196 | if (terminal) return | |
| 197 | terminal = true | |
| 198 | status = 'blocked'; fault ??= 'simulation-session-closed' | |
| 199 | try { finishHost() } | |
| 200 | finally { board.revoke() } | |
| 201 | } | |
| 202 | function progress(nowMs: number, phase: 'host' | 'independent-tail') { | |
| 203 | return { atMs: nowMs, phase, hostAvailable: phase === 'host' && hostLostAt === null && !terminal, terminal, policyAtMs, reference: vehicleReference(estimate), mission: structuredClone(mission), authority: supervisor.snapshot(), board: board.snapshot() } | |
| 204 | } | |
| 205 | function step(nowMs: number) { | |
| 206 | if (terminal) throw new Error('Vehicle simulation session is closed') | |
| 207 | if (stepping) throw new Error('Vehicle simulation session is already stepping') | |
| 208 | if (!Number.isSafeInteger(nowMs) || nowMs !== nextAtMs) throw new RangeError('Vehicle simulation requires its next 4 ms epoch') | |
| 209 | const phase = hostEndAtMs === null ? 'host' as const : 'independent-tail' as const | |
| 210 | stepping = true | |
| 211 | try { | |
| 67 | 212 | finalAtMs = nowMs |
| 68 | const faults = profileInput.faults(scenario, nowMs, 'host'), boardState = board.tick(nowMs) | |
| 69 | if (nowMs > 600) truth = plant.step(boardState.actuators, 0.004, { windMps: faults.windMps ?? [0, 0, 0], currentMps: faults.currentMps ?? [0, 0, 0] }) | |
| 70 | if (faults.hostLost) hostLostAt ??= nowMs | |
| 71 | if (nowMs >= 600) session.observeIndependent?.(structuredClone(truth), nowMs, [...boardState.actuators], hostLostAt === null) | |
| 72 | if (hostLostAt !== null) { | |
| 73 | if (nowMs - hostLostAt >= 100) { status = 'host-lost'; fault = board.snapshot().fault ?? 'host-lost'; break } | |
| 74 | continue | |
| 75 | } | |
| 76 | feed.capture(truth, nowMs, faults.dropImu, profileInput.vehicle === 'subsea' || faults.dropPosition) | |
| 77 | const sensors = feed.advanceTo(nowMs) | |
| 78 | for (const kind of ['imu', 'gnss'] as const) { | |
| 79 | const sequence = sensors.sensors.find(sensor => sensor.kind === kind)?.observation?.sequence ?? 0 | |
| 80 | if (sequence > sensorSequences[kind]) { observedSensors[kind]++; sensorSequences[kind] = sequence } | |
| 81 | } | |
| 82 | const baseEstimate = estimator.update(sensors) | |
| 83 | estimate = navigation ? navigation.update(baseEstimate, measurement, nowMs) : baseEstimate | |
| 84 | if (nowMs % 20 !== 0) continue | |
| 85 | const reference = vehicleReference(estimate) | |
| 86 | const imuObservation = sensors.sensors.find(s => s.kind === 'imu')?.observation | |
| 87 | const capturePose = predictVehicleMeasurementPose(estimate, imuObservation, nowMs, { includeMotion: captureMotion }) | |
| 88 | if ((capturePose || profileInput.vehicle === 'subsea') && !faults.dropVehicleSensor) { | |
| 89 | if (queue.length >= 8) throw new RangeError('Vehicle sensor queue exhausted') | |
| 90 | const packet = structuredClone(session.renderMeasurement(truth, nowMs, reference)) | |
| 91 | if (packet.capturedAtMs !== nowMs || !Number.isSafeInteger(packet.receivedAtMs) || packet.receivedAtMs < nowMs + 20 || packet.receivedAtMs > nowMs + 100) throw new TypeError('Vehicle renderer must declare capture and bounded receipt epochs') | |
| 92 | queue.push({ measurement: packet, pose: capturePose }) | |
| 93 | queue.sort((a, b) => a.measurement.receivedAtMs - b.measurement.receivedAtMs || a.measurement.sequence - b.measurement.sequence) | |
| 94 | } | |
| 95 | while (queue[0] && queue[0].measurement.receivedAtMs <= nowMs) { const entry = queue.shift()!; measurement = entry.measurement; measurementPose = entry.pose } | |
| 96 | let qualified: VehicleMeasurement | null = null | |
| 97 | try { qualified = qualifier.qualify(measurement, nowMs, reference) } | |
| 98 | catch (error) { fault ??= error instanceof Error ? error.message : 'vehicle-sensor-envelope'; measurementPose = null } | |
| 99 | measurement = qualified | |
| 100 | if (batteryModel && qualified) { | |
| 101 | batteryObservation = observeVehicleBattery(qualified, batteryModel, nowMs) | |
| 102 | if (batteryObservation.cutoff) fault ??= 'modeled-battery-supply-cutoff' | |
| 213 | if (phase === 'host') { | |
| 214 | if (!stepHost(nowMs) || nowMs === seconds * 1000) finishHost() | |
| 215 | if (hostEndAtMs !== null && neutralTailMs === 0) terminal = true | |
| 103 | 216 | } |
| 104 | if (navigation) estimate = navigation.update(baseEstimate, qualified, nowMs) | |
| 105 | if (nowMs < 600) continue | |
| 106 | const b = board.snapshot() | |
| 107 | const batteryPercent = batteryModel ? qualified && batteryObservation && !batteryObservation.cutoff ? batteryObservation.remainingPercentLowerBound : 0 : 100 - plan.power.maxW * Math.max(0, nowMs - 600) / 3600000 / plan.power.availableWh * 100 | |
| 108 | const observation = { kind: 'simulation' as const, nowMs, estimate, | |
| 109 | runtime: { bootId: b.bootId, capturedAtMs: nowMs, watchdogHealthy: b.fault === null }, | |
| 110 | rc: { capturedAtMs: nowMs, connected: true, armSwitch: true, override: faults.rcOverride ?? false }, | |
| 111 | interlocks: { capturedAtMs: qualified?.capturedAtMs ?? nowMs - 1000, killActive: faults.kill ?? false, onGround: qualified?.values.onSurface === true, motorsStopped: qualified?.values.stopped === true }, | |
| 112 | battery: { capturedAtMs: batteryModel ? qualified?.capturedAtMs ?? nowMs - 1000 : nowMs, remainingPercent: batteryPercent }, link: { capturedAtMs: nowMs, connected: !faults.linkLost } } | |
| 113 | const send = (action: 'arm' | 'engage' | 'renew' | 'disarm', targetM = mission.targetM) => { | |
| 114 | const s = supervisor.snapshot() | |
| 115 | return supervisor.update(observation, { version: 1, sessionId: s.sessionId, bootId: s.bootId, epoch: s.epoch, sequence: s.sequence + 1, issuedAtMs: nowMs, action, mode: 'position', leaseMs: 100, target: { positionM: targetM ?? [0, 0, 0], yawRad: 0 } }) | |
| 217 | else { | |
| 218 | // No host sensor, estimation, mission or control work runs in this phase. | |
| 219 | const faults = captured(() => runtimeFaults(scenario, nowMs, 'independent-tail')), boardState = board.tick(nowMs) | |
| 220 | truth = captured(() => stepPlant(boardState.actuators, .004, { windMps: faults.windMps ?? [0, 0, 0], currentMps: faults.currentMps ?? [0, 0, 0] })) | |
| 221 | owned(() => observeIndependent?.(structuredClone(truth), nowMs, [...boardState.actuators], false)) | |
| 222 | if (nowMs === hostEndAtMs! + neutralTailMs) terminal = true | |
| 116 | 223 | } |
| 117 | supervisor.update(observation) | |
| 118 | if (nowMs === 600) { send('arm'); send('engage', [0, 0, 0]) } | |
| 119 | const context: VehicleControllerContext = { nowMs, enabled: supervisor.snapshot().state === 'autonomous', estimate, measurement, measurementPose, mission, | |
| 120 | ...(profileInput.vehicle === 'vtol' ? { controlRate: capturePose ? { evidence: 'modeled-qualified-imu' as const, reference: capturePose.reference, capturedAtMs: capturePose.gyroCapturedAtMs, angularRateRadS: [...(imuObservation!.value as ImuMeasurement).angularRateRadS] as SensorVector } : null } : {}), | |
| 121 | } | |
| 122 | payload?.observe?.(structuredClone(context)) | |
| 123 | const ack = payload?.poll(nowMs) ?? null | |
| 124 | const missionObservation = { ...observeMission({ estimate, authority: supervisor.snapshot().state, batteryPercent, onGround: qualified?.values.onSurface === true }, profileInput.vehicle === 'subsea' ? 'LOCAL_WATER_SURFACE' : 'WGS84_ELLIPSOID'), ...(!estimate.localPosition ? { origin: structuredClone(plan.origin), positionM: null, velocityMps: null } : {}), payloadAck: ack, ...(hasTransitions ? { transition: transitionObservation } : {}) } | |
| 125 | transitionObservation = null | |
| 126 | let action: VehicleMissionAction | undefined = nowMs === 600 ? 'start' : undefined | |
| 127 | const scheduled = missionActions[missionActionIndex] | |
| 128 | if (scheduled?.atMs === nowMs) { action = scheduled.action; missionActionIndex++ } | |
| 129 | mission = engine.step(missionObservation, action) | |
| 130 | if (mission.payload && !requested.has(mission.payload.eventId)) { | |
| 131 | if (!payload) { fault ??= 'missing-vehicle-payload-port'; send('disarm') } | |
| 132 | else { payload.observe?.(structuredClone({ ...context, mission })); payload.request(structuredClone(mission.payload)); requested.add(mission.payload.eventId) } | |
| 133 | } | |
| 134 | const active = mission.state === 'running' || mission.state === 'paused' | |
| 135 | if (active && mission.targetM && !fault) send('renew') | |
| 136 | else { send('disarm'); board.revoke() } | |
| 137 | const output: VehicleControlOutput = controller.step({ ...context, enabled: supervisor.snapshot().state === 'autonomous', mission }) | |
| 138 | if (hasTransitions && active && !output.inhibited && supervisor.snapshot().state === 'autonomous') transitionObservation = output.transition ? structuredClone(output.transition) : null | |
| 139 | if (active && output.inhibited && supervisor.snapshot().state === 'autonomous') { fault ??= output.reason; send('disarm'); board.revoke() } | |
| 140 | const frame = supervisor.gate(output, nowMs) | |
| 141 | if (frame) board.accept(frame, nowMs) | |
| 142 | else board.revoke() | |
| 143 | path.push({ atMs: nowMs, positionM: estimate.localPosition ? [...estimate.localPosition.positionM] as SensorVector : null, truthPositionM: [...truth.positionM], mission: structuredClone(mission), measurement: measurement ? structuredClone(measurement) : null, measurementPose: measurementPose ? structuredClone(measurementPose) : null, actuators: [...board.snapshot().actuators], ...(hasTransitions ? { transitionObservation: transitionObservation ? structuredClone(transitionObservation) : null } : {}), ...(output.backCapture ? { backCapture: structuredClone(output.backCapture) } : {}) }) | |
| 144 | if (mission.state !== previousState || mission.taskId !== previousTask) { transitions.push({ atMs: nowMs, state: mission.state, taskId: mission.taskId, reason: mission.reason }); previousState = mission.state; previousTask = mission.taskId } | |
| 145 | if (!['ready', 'running', 'paused'].includes(mission.state)) { status = mission.state === 'completed' ? 'completed' : 'blocked'; fault ??= mission.state === 'completed' ? null : mission.reason; break } | |
| 224 | nextAtMs += 4 | |
| 225 | return progress(nowMs, phase) | |
| 226 | } | |
| 227 | catch (error) { | |
| 228 | if (error === interrupted) return progress(nowMs, phase) | |
| 229 | terminal = true; status = 'blocked'; fault ??= 'simulation-step-failed' | |
| 230 | try { finishHost() } | |
| 231 | catch (cleanupError) { throw new AggregateError([error, cleanupError], 'Vehicle simulation step and cleanup failed') } | |
| 232 | finally { board.revoke() } | |
| 233 | throw error | |
| 146 | 234 | } |
| 235 | finally { stepping = false } | |
| 147 | 236 | } |
| 148 | finally { payload?.close(); if (status !== 'host-lost') board.revoke() } | |
| 149 | // Opt-in material/actuator settling has no host sensor, estimation, mission or control work. | |
| 150 | const hostEndAtMs = finalAtMs | |
| 151 | for (let elapsed = 4; elapsed <= neutralTailMs; elapsed += 4) { | |
| 152 | const nowMs = hostEndAtMs + elapsed, faults = profileInput.faults(scenario, nowMs, 'independent-tail'), boardState = board.tick(nowMs) | |
| 153 | truth = plant.step(boardState.actuators, .004, { windMps: faults.windMps ?? [0, 0, 0], currentMps: faults.currentMps ?? [0, 0, 0] }) | |
| 154 | session.observeIndependent?.(structuredClone(truth), nowMs, [...boardState.actuators], false) | |
| 155 | finalAtMs = nowMs | |
| 237 | function result() { | |
| 238 | if (!terminal) throw new Error('Vehicle simulation session is still active') | |
| 239 | return structuredClone({ version: 1, evidence: 'owned-vehicle-physics-simulation' as const, physicalFlightEnabled: false as const, boardOutputsInhibited: true as const, vehicle: vehicle, scenario, status, fault, navigationEvidence: { ...(positionPrediction ? { positionPrediction: { ...fixtureInertialModel } } : {}), observedSensors, datum: estimate.localPosition?.altitudeDatum ?? null, quality: [...estimate.quality], localPositionHealthy: estimate.health.localPosition }, mission, trace: engine.trace(), path, transitions, budget: { worstCaseMs, requiredWh }, authority: supervisor.snapshot(), board: board.snapshot(), finalTruthPositionM: [...truth.positionM] as SensorVector, ...(batteryModel ? { energy: { evidence: 'modeled-vehicle-battery-v1' as const, model: batteryModel, lastObservation: batteryObservation } } : {}) }) | |
| 240 | } | |
| 241 | return { step, result, close } | |
| 242 | } | |
| 243 | /** Run the same retained session synchronously to its natural terminal result. */ | |
| 244 | export function runVehicleSimulation(profileInput: VehicleProfile, options: VehicleSimulationOptions = {}) { | |
| 245 | const session = createVehicleSimulationSession(profileInput, options) | |
| 246 | try { | |
| 247 | for (let nowMs = 0; ; nowMs += 4) { if (session.step(nowMs).terminal) return session.result() } | |
| 156 | 248 | } |
| 157 | return { version: 1, evidence: 'owned-vehicle-physics-simulation' as const, physicalFlightEnabled: false as const, boardOutputsInhibited: true as const, vehicle: profileInput.vehicle, scenario, status, fault, navigationEvidence: { ...(positionPrediction ? { positionPrediction: { ...fixtureInertialModel } } : {}), observedSensors, datum: estimate.localPosition?.altitudeDatum ?? null, quality: [...estimate.quality], localPositionHealthy: estimate.health.localPosition }, mission, trace: engine.trace(), path, transitions, budget: { worstCaseMs, requiredWh }, authority: supervisor.snapshot(), board: board.snapshot(), finalTruthPositionM: [...truth.positionM] as SensorVector, ...(batteryModel ? { energy: { evidence: 'modeled-vehicle-battery-v1' as const, model: batteryModel, lastObservation: batteryObservation } } : {}) } | |
| 249 | finally { session.close() } | |
| 158 | 250 | } |