also looking at this
feat: correlate VTOL hover mode observations
#77
10 files
+507
-28
| @@ -37,7 +37,9 @@ The hover demonstration commands the pusher and three surfaces to zero. Calibrat | ||
| 37 | 37 | IMU/GNSS samples feed the owned estimator. The controller receives estimated pose, a current qualified |
| 38 | 38 | raw gyro capture and delayed contact measurements. Plant truth is used for sensor synthesis and |
| 39 | 39 | independent evaluation, never as controller input. Initial north heading and a flat contact plane |
| 40 | are declared simulation assumptions. Landing uses a 0.1 m below-plane target to obtain finite | |
| 40 | are declared simulation assumptions. A fictional skid friction coefficient of 0.5 supplies bounded | |
| 41 | horizontal braking from normal support at contact. It does not model impact impulses, gear deformation, | |
| 42 | tipping, damage or uneven terrain. Airborne motion receives no contact friction. Landing uses a 0.1 m below-plane target to obtain finite | |
| 41 | 43 | contact; mission completion still requires estimated arrival, low speed, settling and fresh contact. |
| 42 | 44 | |
| 43 | 45 | Contact and paused-ground neutral decisions occur after estimator/rate validation. A stale gyro |
| @@ -51,10 +53,21 @@ fictional hover experiment. They are not identified electrical power, measured e | ||
| 51 | 53 | complete hybrid energy model. Neutral outputs during an airborne fault do not establish a safe |
| 52 | 54 | emergency landing. Physical flight commands remain disabled. |
| 53 | 55 | |
| 56 | ## Explicit hover observation in library plans | |
| 57 | ||
| 58 | Library plans can declare a VTOL-only `vtol-mode` task with `mode: 'hover'`, an explicit heading | |
| 59 | and a finite timeout. The held position must equal the request position. The mission emits a task, reference and epoch-bound request. The hover adapter | |
| 60 | requires fresh post-request contact, estimated pose and raw gyro sources, with a 200 ms stability | |
| 61 | dwell on both the monotonic decision clock and source coverage. It reports the oldest source capture epoch. The shared runtime delivers each controller | |
| 62 | response once on the next mission tick and retains it for replay. Initial contact holds all outputs | |
| 63 | neutral until observation is qualified. Unobserved headings expire, and this hover adapter reports | |
| 64 | cruise requests as aborted. This is a library prerequisite, not front/back transition control or a | |
| 65 | new CLI scenario. Existing hover-only recordings keep their prior shapes. | |
| 66 | ||
| 54 | 67 | ## Remaining implementation |
| 55 | 68 | |
| 56 | [Issue #12](https://github.com/stacksjs/drone/issues/12) remains open for explicit correlated | |
| 57 | transition requests and observed outcomes, front/cruise/back control, airspeed and attitude | |
| 69 | [Issue #12](https://github.com/stacksjs/drone/issues/12) remains open for full correlated front/back | |
| 70 | transition control and observed outcomes, airspeed and attitude | |
| 58 | 71 | qualification, bounded transition abort/recovery, complete hybrid power/reserve accounting and |
| 59 | 72 | the common mission workspace. Full TECS/soaring, tiltrotor/tailsitter mechanisms, helicopters and |
| 60 | 73 | other airframe families remain separate work. Native board control, measured airframe identification, |
| @@ -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. */ |
| @@ -1,6 +1,6 @@ | ||
| 1 | 1 | import type { EstimatorSnapshot } from '../estimation/estimator' |
| 2 | 2 | import type { ImuMeasurement, SensorVector } from '../sensors/types' |
| 3 | import type { VehicleControlOutput, VehicleControllerContext, VehicleMeasurement, VehicleMeasurementPose, VehicleProfile } from './types' | |
| 3 | import type { VehicleControlOutput, VehicleControllerContext, VehicleMeasurement, VehicleMeasurementPose, VehicleProfile, VehicleVtolTransitionObservation } from './types' | |
| 4 | 4 | import { createPhysicsSensorFeed } from '../controls/sensors' |
| 5 | 5 | import { createEstimator } from '../estimation/estimator' |
| 6 | 6 | import { observeMission } from '../missions/engine' |
| @@ -15,6 +15,8 @@ export function runVehicleSimulation(profileInput: VehicleProfile, options: { sc | ||
| 15 | 15 | if (!Number.isInteger(seconds) || seconds < 2 || seconds > 180 || scenario !== profileInput.scenario || !profileInput.scenarios.includes(scenario) || profileInput.evidence !== 'modeled-vehicle-profile' || profileInput.physicalFlightEnabled !== false) throw new RangeError('Invalid vehicle simulation profile, scenario or duration') |
| 16 | 16 | const { plan, worstCaseMs, requiredWh } = validateVehicleMission(profileInput.mission) |
| 17 | 17 | 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') |
| 18 | const hasTransitions = plan.tasks.some(task => task.kind === 'vtol-mode') | |
| 19 | let transitionObservation: VehicleVtolTransitionObservation | null = null | |
| 18 | 20 | const actuators = structuredClone(profileInput.actuators), fence = structuredClone(profileInput.authorityFence) |
| 19 | 21 | const session = profileInput.createSession(), navigation = session.createNavigation?.() |
| 20 | 22 | if (profileInput.vehicle === 'subsea' && !navigation) throw new TypeError('Subsea profile requires owned pressure/DVL navigation') |
| @@ -30,7 +32,7 @@ export function runVehicleSimulation(profileInput: VehicleProfile, options: { sc | ||
| 30 | 32 | const queue: { measurement: VehicleMeasurement, pose: VehicleMeasurementPose | null }[] = [] |
| 31 | 33 | const requested = new Set<string>() |
| 32 | 34 | let status: 'completed' | 'blocked' | 'host-lost' | 'duration-exhausted' = 'duration-exhausted', fault: string | null = null, hostLostAt: number | null = null |
| 33 | const path: { atMs: number, positionM: SensorVector | null, truthPositionM: SensorVector, mission: typeof mission, measurement: VehicleMeasurement | null, measurementPose: VehicleMeasurementPose | null, actuators: number[] }[] = [] | |
| 35 | const path: { atMs: number, positionM: SensorVector | null, truthPositionM: SensorVector, mission: typeof mission, measurement: VehicleMeasurement | null, measurementPose: VehicleMeasurementPose | null, actuators: number[], transitionObservation?: VehicleVtolTransitionObservation | null }[] = [] | |
| 34 | 36 | const transitions: { atMs: number, state: string, taskId: string, reason: string }[] = [] |
| 35 | 37 | let previousState = '', previousTask = '' |
| 36 | 38 | const payload = session.createPayload?.() |
| @@ -89,7 +91,8 @@ export function runVehicleSimulation(profileInput: VehicleProfile, options: { sc | ||
| 89 | 91 | } |
| 90 | 92 | payload?.observe?.(structuredClone(context)) |
| 91 | 93 | const ack = payload?.poll(nowMs) ?? null |
| 92 | const missionObservation = { ...observeMission({ estimate, authority: supervisor.snapshot().state, batteryPercent, onGround: qualified?.values.onSurface === true }, profileInput.vehicle === 'subsea' ? 'LOCAL_WATER_SURFACE' : 'WGS84_ELLIPSOID'), payloadAck: ack } | |
| 94 | const missionObservation = { ...observeMission({ estimate, authority: supervisor.snapshot().state, batteryPercent, onGround: qualified?.values.onSurface === true }, profileInput.vehicle === 'subsea' ? 'LOCAL_WATER_SURFACE' : 'WGS84_ELLIPSOID'), payloadAck: ack, ...(hasTransitions ? { transition: transitionObservation } : {}) } | |
| 95 | transitionObservation = null | |
| 93 | 96 | mission = engine.step(missionObservation, nowMs === 600 ? 'start' : undefined) |
| 94 | 97 | if (mission.payload && !requested.has(mission.payload.eventId)) { |
| 95 | 98 | if (!payload) { fault ??= 'missing-vehicle-payload-port'; send('disarm') } |
| @@ -99,11 +102,12 @@ export function runVehicleSimulation(profileInput: VehicleProfile, options: { sc | ||
| 99 | 102 | if (active && mission.targetM && !fault) send('renew') |
| 100 | 103 | else { send('disarm'); board.revoke() } |
| 101 | 104 | const output: VehicleControlOutput = controller.step({ ...context, enabled: supervisor.snapshot().state === 'autonomous', mission }) |
| 105 | if (hasTransitions && active && !output.inhibited && supervisor.snapshot().state === 'autonomous') transitionObservation = output.transition ? structuredClone(output.transition) : null | |
| 102 | 106 | if (active && output.inhibited && supervisor.snapshot().state === 'autonomous') { fault ??= output.reason; send('disarm'); board.revoke() } |
| 103 | 107 | const frame = supervisor.gate(output, nowMs) |
| 104 | 108 | if (frame) board.accept(frame, nowMs) |
| 105 | 109 | else board.revoke() |
| 106 | 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] }) | |
| 110 | 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 } : {}) }) | |
| 107 | 111 | 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 } |
| 108 | 112 | if (!['ready', 'running', 'paused'].includes(mission.state)) { status = mission.state === 'completed' ? 'completed' : 'blocked'; fault ??= mission.state === 'completed' ? null : mission.reason; break } |
| 109 | 113 | } |
| @@ -31,8 +31,12 @@ export type VehiclePayloadOperation = 'camera-still' | 'material-drop' | 'water- | ||
| 31 | 31 | export interface VehiclePayloadCommand { eventId: string, taskId: string, operation: VehiclePayloadOperation, containerId: string | null, issuedAtMs: number, expiresAtMs: number, positionM: SensorVector, reference: string } |
| 32 | 32 | export interface VehiclePayloadAck { eventId: string, taskId: string, operation: VehiclePayloadOperation, containerId: string | null, capturedAtMs: number, reference: string, positionM: SensorVector, outcome: 'confirmed' | 'rejected' | 'uncertain', metadata?: Record<string, number | string | boolean> } |
| 33 | 33 | export interface VehiclePayloadPort { observe?(context: VehicleControllerContext): void, request(command: VehiclePayloadCommand): void, poll(nowMs: number): VehiclePayloadAck | null, close(): void } |
| 34 | export type VehicleVtolMode = 'hover' | 'cruise' | |
| 35 | export interface VehicleVtolTransitionCommand { eventId: string, taskId: string, mode: VehicleVtolMode, headingRad: number, issuedAtMs: number, expiresAtMs: number, positionM: SensorVector, reference: string } | |
| 36 | export interface VehicleVtolTransitionObservation { evidence: 'modeled-vtol-controller-observation', eventId: string, taskId: string, mode: VehicleVtolMode, reference: string, capturedAtMs: number, measurementCapturedAtMs: number, phase: 'hover' | 'front-transition' | 'cruise' | 'back-transition', outcome: 'progress' | 'confirmed' | 'aborted', reason: string } | |
| 34 | 37 | export type VehicleMissionTask = |
| 35 | 38 | | { id: string, kind: 'start', next: string } |
| 39 | | { id: string, kind: 'vtol-mode', mode: VehicleVtolMode, headingRad: number, timeoutMs: number, next: string } | |
| 36 | 40 | | { id: string, kind: 'takeoff' | 'waypoint' | 'land' | 'stop', positionM: SensorVector, acceptanceM: number, maxArrivalSpeedMps: number, settleMs: number, timeoutMs: number, next?: string } |
| 37 | 41 | | { id: string, kind: 'return', acceptanceM: number, maxArrivalSpeedMps: number, settleMs: number, timeoutMs: number, next: string } |
| 38 | 42 | | { id: string, kind: 'loiter', positionM: SensorVector, acceptanceM: number, maxArrivalSpeedMps: number, settleMs: number, timeoutMs: number, next: string } |
| @@ -54,7 +58,7 @@ export interface VehicleMissionPlan { | ||
| 54 | 58 | start: string |
| 55 | 59 | tasks: VehicleMissionTask[] |
| 56 | 60 | } |
| 57 | export interface VehicleMissionObservation extends MissionObservation { payloadAck: VehiclePayloadAck | null } | |
| 61 | export interface VehicleMissionObservation extends MissionObservation { payloadAck: VehiclePayloadAck | null, transition?: VehicleVtolTransitionObservation | null } | |
| 58 | 62 | export interface VehicleMissionSnapshot { |
| 59 | 63 | version: 2 |
| 60 | 64 | vehicle: VehicleKind |
| @@ -65,12 +69,16 @@ export interface VehicleMissionSnapshot { | ||
| 65 | 69 | taskKind: VehicleMissionTask['kind'] |
| 66 | 70 | targetM: SensorVector | null |
| 67 | 71 | payload: VehiclePayloadCommand | null |
| 72 | /** Present only for plans with explicit VTOL mode tasks; legacy recording shapes stay unchanged. */ | |
| 73 | transition?: VehicleVtolTransitionCommand | null | |
| 68 | 74 | reason: string |
| 69 | 75 | elapsedMs: number |
| 70 | 76 | } |
| 71 | 77 | export type VehicleMissionAction = MissionAction |
| 72 | 78 | export interface VehicleControlOutput { |
| 73 | 79 | kind: 'simulated-vehicle-control' |
| 80 | /** One capture-bound response, consumed by the next mission evaluation for explicit mode plans. */ | |
| 81 | transition?: VehicleVtolTransitionObservation | null | |
| 74 | 82 | vehicle: VehicleKind |
| 75 | 83 | atMs: number |
| 76 | 84 | actuators: number[] |
| @@ -1,11 +1,13 @@ | ||
| 1 | 1 | import type { VehicleController, VehicleControllerContext, VehicleControlOutput } from '../types' |
| 2 | 2 | import { createVehicleMeasurementQualifier, vehicleReference } from '../measurements' |
| 3 | import { numericPacket } from './packets' | |
| 3 | 4 | import { createVtolHoverController } from './hover-controller' |
| 4 | 5 | |
| 5 | 6 | /** Hover-only adapter on the shared mission/authority runtime. No wing transition policy. */ |
| 6 | 7 | export function createVtolVehicleHoverController(): VehicleController { |
| 7 | 8 | const hover = createVtolHoverController({ headingAlignment: 'initial-simulation-north' }), qualifier = createVehicleMeasurementQualifier('vtol') |
| 8 | 9 | let fault = '', hasFlown = false |
| 10 | let modeEvent = '', modeSignature = '', hoverStableSince: number | null = null, observedStableSince: number | null = null, confirmedEvent = '', headingRad = 0 | |
| 9 | 11 | function output(c: VehicleControllerContext, actuators: number[], reason: string, inhibited: boolean): VehicleControlOutput { |
| 10 | 12 | return { kind: 'simulated-vehicle-control', vehicle: 'vtol', atMs: c.nowMs, actuators, reason, inhibited, physicalFlightEnabled: false, boardOutputsInhibited: true } |
| 11 | 13 | } |
| @@ -18,13 +20,41 @@ export function createVtolVehicleHoverController(): VehicleController { | ||
| 18 | 20 | if (!measurement || c.mission.vehicle !== 'vtol' || !c.controlRate || c.controlRate.evidence !== 'modeled-qualified-imu' || c.controlRate.reference !== vehicleReference(c.estimate)) fault ||= 'vtol-control-observation-unqualified' |
| 19 | 21 | if (fault) return output(c, [0, 0, 0, 0, 0, 0, 0, 0], fault, true) |
| 20 | 22 | if (measurement!.values.onSurface === false) hasFlown = true |
| 23 | let target: [number, number, number] | null = null | |
| 24 | try { target = c.mission.targetM ? numericPacket(c.mission.targetM, 3) as [number, number, number] : null } | |
| 25 | catch { fault = 'vtol-mission-target-unqualified' } | |
| 26 | if (!target) { fault ||= 'vtol-mission-target-missing'; return output(c, [0, 0, 0, 0, 0, 0, 0, 0], fault, true) } | |
| 27 | if (fault) return output(c, [0, 0, 0, 0, 0, 0, 0, 0], fault, true) | |
| 28 | const request = c.mission.transition | |
| 29 | if (request) { | |
| 30 | let position: number[] = [] | |
| 31 | try { position = numericPacket(request.positionM, 3) } | |
| 32 | catch { fault = 'vtol-mode-request-unqualified' } | |
| 33 | if (c.mission.taskKind !== 'vtol-mode' || request.taskId !== c.mission.taskId || typeof request.eventId !== 'string' || !request.eventId || request.eventId.length > 256 || request.reference !== vehicleReference(c.estimate) || !['hover', 'cruise'].includes(request.mode) || !Number.isFinite(request.headingRad) || Math.abs(request.headingRad) > Math.PI || position.length !== 3 || position.some((n, i) => n !== target![i]) || !Number.isSafeInteger(request.issuedAtMs) || !Number.isSafeInteger(request.expiresAtMs) || request.issuedAtMs > c.nowMs || request.expiresAtMs <= c.nowMs || request.expiresAtMs <= request.issuedAtMs) fault = 'vtol-mode-request-unqualified' | |
| 34 | const signature = JSON.stringify(request) | |
| 35 | if (modeEvent === request.eventId && modeSignature !== signature) fault = 'vtol-mode-request-changed' | |
| 36 | if (modeEvent !== request.eventId) { modeEvent = request.eventId; modeSignature = signature; hoverStableSince = null; observedStableSince = null; headingRad = request.headingRad } | |
| 37 | } | |
| 38 | if (fault) return output(c, [0, 0, 0, 0, 0, 0, 0, 0], fault, true) | |
| 21 | 39 | const landing = c.mission.state === 'running' && c.mission.taskKind === 'land' |
| 22 | const target = c.mission.targetM ? [...c.mission.targetM] as [number, number, number] : null | |
| 23 | if (!target) { fault = 'vtol-mission-target-missing'; return output(c, [0, 0, 0, 0, 0, 0, 0, 0], fault, true) } | |
| 24 | 40 | // A declared 0.1 m plane intercept permits finite contact rather than asymptotic height holding. |
| 25 | 41 | if (landing) target[2] += 0.1 |
| 26 | const result = hover.step({ kind: 'simulation', enabled: true, nowMs: c.nowMs, estimate: c.estimate, angularRateRadS: c.controlRate!.angularRateRadS, rateCapturedAtMs: c.controlRate!.capturedAtMs, target: { positionM: target, yawRad: 0, issuedAtMs: c.nowMs } }) | |
| 27 | if (result.inhibited) fault = result.reason | |
| 42 | const result = hover.step({ kind: 'simulation', enabled: true, nowMs: c.nowMs, estimate: c.estimate, angularRateRadS: c.controlRate!.angularRateRadS, rateCapturedAtMs: c.controlRate!.capturedAtMs, target: { positionM: target, yawRad: headingRad, issuedAtMs: c.nowMs } }) | |
| 43 | if (result.inhibited) fault ||= result.reason | |
| 44 | if (fault) return output(c, [0, 0, 0, 0, 0, 0, 0, 0], fault, true) | |
| 45 | if (c.mission.taskKind === 'vtol-mode') { | |
| 46 | // The hover development adapter never certifies wing-borne flight or a back transition. | |
| 47 | const response = output(c, measurement!.values.onSurface === true ? [0, 0, 0, 0, 0, 0, 0, 0] : [...result.motorsN, 0, 0, 0, 0], 'hover-mode-observing', false) | |
| 48 | const sourceCapturedAtMs = Math.min(measurement!.capturedAtMs, c.controlRate!.capturedAtMs, c.estimate.attitude!.capturedAtMs!, c.estimate.localPosition!.capturedAtMs!) | |
| 49 | if (!request || sourceCapturedAtMs < request.issuedAtMs || confirmedEvent === request.eventId) return response | |
| 50 | const stable = Math.hypot(...c.estimate.localPosition!.velocityMps) <= 0.2 && Math.hypot(...c.controlRate!.angularRateRadS) <= 0.1 && Math.abs(c.estimate.attitude!.eulerRad[0]) <= 0.1 && Math.abs(c.estimate.attitude!.eulerRad[1]) <= 0.1 && Math.abs(Math.atan2(Math.sin(c.estimate.attitude!.eulerRad[2] - headingRad), Math.cos(c.estimate.attitude!.eulerRad[2] - headingRad))) <= 0.1 && Math.hypot(...target.map((n, i) => n - c.estimate.localPosition!.positionM[i]!)) <= 0.3 && (measurement!.values.onSurface === false || measurement!.values.stopped === true) | |
| 51 | if (!stable) { hoverStableSince = null; observedStableSince = null } | |
| 52 | else { hoverStableSince ??= sourceCapturedAtMs; observedStableSince ??= c.nowMs } | |
| 53 | const confirmed = request.mode === 'hover' && stable && sourceCapturedAtMs - hoverStableSince! >= 200 && c.nowMs - observedStableSince! >= 200 | |
| 54 | response.transition = { evidence: 'modeled-vtol-controller-observation', eventId: request.eventId, taskId: request.taskId, mode: request.mode, reference: request.reference, capturedAtMs: c.nowMs, measurementCapturedAtMs: sourceCapturedAtMs, phase: 'hover', outcome: request.mode === 'cruise' ? 'aborted' : confirmed ? 'confirmed' : 'progress', reason: request.mode === 'cruise' ? 'hover-only-controller' : confirmed ? 'hover-qualified' : 'hover-qualifying' } | |
| 55 | if (confirmed || request.mode === 'cruise') confirmedEvent = request.eventId | |
| 56 | return response | |
| 57 | } | |
| 28 | 58 | if (!result.inhibited && c.mission.state === 'paused' && measurement!.values.onSurface === true) return output(c, [0, 0, 0, 0, 0, 0, 0, 0], 'ground-paused', false) |
| 29 | 59 | if (!result.inhibited && landing && hasFlown && measurement!.values.onSurface === true) return output(c, [0, 0, 0, 0, 0, 0, 0, 0], 'vertical-contact', false) |
| 30 | 60 | return output(c, [...result.motorsN, 0, 0, 0, 0], result.reason, result.inhibited) |
| @@ -8,7 +8,7 @@ import { airAerodynamics, airModel } from '../air/plant' | ||
| 8 | 8 | import { numericPacket } from './packets' |
| 9 | 9 | |
| 10 | 10 | /** Fictional lift-and-cruise model. No measured hardware coefficients or flight qualification. */ |
| 11 | export const vtolModel = Object.freeze({ massKg: 2.5, gravityMps2: 9.80665, maxRotorThrustN: 12, rotorTimeConstantS: 0.06, rotorSlewNPerS: 120, rotorArmProjectionM: 0.4, reactionTorqueM: 0.02, inertiaKgM2: Object.freeze([0.3, 0.4, 0.6] as const), maxPusherThrustN: airModel.maxThrustN, pusherTimeConstantS: 0.08, pusherSlewNPerS: 60, wing: airModel }) | |
| 11 | export const vtolModel = Object.freeze({ massKg: 2.5, surfaceFrictionCoefficient: 0.5, gravityMps2: 9.80665, maxRotorThrustN: 12, rotorTimeConstantS: 0.06, rotorSlewNPerS: 120, rotorArmProjectionM: 0.4, reactionTorqueM: 0.02, inertiaKgM2: Object.freeze([0.3, 0.4, 0.6] as const), maxPusherThrustN: airModel.maxThrustN, pusherTimeConstantS: 0.08, pusherSlewNPerS: 60, wing: airModel }) | |
| 12 | 12 | |
| 13 | 13 | /** Independent lift rotors and the owned trainer wing/pusher share one rigid body. */ |
| 14 | 14 | export function createVtolPlant(initial: { positionM?: SensorVector, velocityMps?: SensorVector, quaternionWxyz?: Quaternion, rotorsN?: [number, number, number, number], pusherThrustN?: number } = {}): VehiclePlant { |
| @@ -54,10 +54,21 @@ export function createVtolPlant(initial: { positionM?: SensorVector, velocityMps | ||
| 54 | 54 | const forceN = rotateBodyToNed(nextQuaternionWxyz, [aero.forceN[0], aero.forceN[1], aero.forceN[2] - nextRotorsN.reduce((a, b) => a + b, 0)]) |
| 55 | 55 | const nextAccelerationMps2 = forceN.map((n, i) => n / vtolModel.massKg + (i === 2 ? vtolModel.gravityMps2 : 0)) as SensorVector |
| 56 | 56 | const nextVelocityMps = velocityMps.map((n, i) => n + nextAccelerationMps2[i]! * dtS) as SensorVector |
| 57 | const nextOnSurface = positionM[2] + nextVelocityMps[2] * dtS >= 0 | |
| 58 | if (nextOnSurface) { | |
| 59 | // Fictional flat skids: bounded Coulomb friction from the modeled normal support. | |
| 60 | // No impact impulse, terrain, gear deformation, tipping or damage model is implied. | |
| 61 | const speedMps = Math.hypot(nextVelocityMps[0], nextVelocityMps[1]) | |
| 62 | const brakingMps = Math.min(speedMps, vtolModel.surfaceFrictionCoefficient * Math.max(0, nextAccelerationMps2[2]) * dtS) | |
| 63 | if (speedMps > 0) for (const i of [0, 1]) { | |
| 64 | nextVelocityMps[i] = nextVelocityMps[i]! * (1 - brakingMps / speedMps) | |
| 65 | nextAccelerationMps2[i] = (nextVelocityMps[i]! - velocityMps[i]!) / dtS | |
| 66 | } | |
| 67 | nextVelocityMps[2] = Math.min(0, nextVelocityMps[2]); nextAccelerationMps2[2] = 0 | |
| 68 | } | |
| 57 | 69 | const nextPositionM = positionM.map((n, i) => n + nextVelocityMps[i]! * dtS) as SensorVector |
| 70 | if (nextOnSurface) nextPositionM[2] = 0 | |
| 58 | 71 | if (![...nextAccelerationMps2, ...nextVelocityMps, ...nextPositionM].every(Number.isFinite)) throw new RangeError('VTOL rotor model diverged') |
| 59 | const nextOnSurface = nextPositionM[2] >= 0 | |
| 60 | if (nextOnSurface) { nextPositionM[2] = 0; nextVelocityMps[2] = Math.min(0, nextVelocityMps[2]); nextAccelerationMps2[2] = 0 } | |
| 61 | 72 | // Reject the entire integration before committing any retained state. |
| 62 | 73 | windMps = nextWindMps |
| 63 | 74 | rotorsN = nextRotorsN |
| @@ -0,0 +1,163 @@ | ||
| 1 | import { expect, test } from 'bun:test' | |
| 2 | import type { VehicleMissionObservation, VehicleMissionPlan, VehicleVtolTransitionCommand, VehicleVtolTransitionObservation } from '../../resources/functions/drone/vehicles/types' | |
| 3 | import { checkpointVehicleMission, createVehicleMissionEngine, replayVehicleMission, validateVehicleMission } from '../../resources/functions/drone/missions/vehicle' | |
| 4 | import { createVtolHoverProfile } from '../../resources/functions/drone/vehicles/vtol' | |
| 5 | ||
| 6 | // Public mission seam catches requested/observed ordering and replay, not controller dynamics or hardware. | |
| 7 | test('an explicit VTOL hover request waits for its correlated observed outcome before takeoff', () => { | |
| 8 | const input = { | |
| 9 | ...createVtolHoverProfile().mission, | |
| 10 | id: 'vtol-mode-policy', | |
| 11 | tasks: [ | |
| 12 | { id: 'start', kind: 'start', next: 'confirm-hover' }, | |
| 13 | { id: 'confirm-hover', kind: 'vtol-mode', mode: 'hover', headingRad: 0, timeoutMs: 1000, next: 'depart' }, | |
| 14 | { id: 'depart', kind: 'takeoff', positionM: [0, 0, -5], acceptanceM: 0.2, maxArrivalSpeedMps: 0.2, settleMs: 200, timeoutMs: 2000, next: 'land' }, | |
| 15 | { id: 'land', kind: 'land', positionM: [0, 0, 0], acceptanceM: 0.3, maxArrivalSpeedMps: 0.2, settleMs: 200, timeoutMs: 2000 }, | |
| 16 | ], | |
| 17 | } satisfies VehicleMissionPlan | |
| 18 | const reference = '55:0,0,0,0,0' | |
| 19 | const observation = (nowMs: number): VehicleMissionObservation => ({ kind: 'simulation', nowMs, reference, origin: input.origin, healthy: true, positionM: [0, 0, 0], velocityMps: [0, 0, 0], authority: 'autonomous', batteryPercent: 100, onGround: true, payloadAck: null, transition: null }) | |
| 20 | const engine = createVehicleMissionEngine(input) | |
| 21 | expect(engine.step(observation(1000), 'start').taskId).toBe('confirm-hover') | |
| 22 | const requested = engine.step(observation(1020)) | |
| 23 | expect(requested).toMatchObject({ taskId: 'confirm-hover', targetM: [0, 0, 0], transition: { | |
| 24 | eventId: 'vtol-mode-policy:55:0,0,0,0,0:confirm-hover', taskId: 'confirm-hover', mode: 'hover', headingRad: 0, | |
| 25 | issuedAtMs: 1020, expiresAtMs: 2000, positionM: [0, 0, 0], reference, | |
| 26 | } }) | |
| 27 | expect(engine.step(observation(1040))).toMatchObject({ state: 'running', taskId: 'confirm-hover', targetM: [0, 0, 0], transition: requested.transition }) | |
| 28 | const confirmed = engine.step({ ...observation(1060), transition: { | |
| 29 | evidence: 'modeled-vtol-controller-observation', eventId: 'vtol-mode-policy:55:0,0,0,0,0:confirm-hover', taskId: 'confirm-hover', mode: 'hover', reference, | |
| 30 | capturedAtMs: 1060, measurementCapturedAtMs: 1040, phase: 'hover', outcome: 'confirmed', reason: 'hover-qualified', | |
| 31 | } }) | |
| 32 | expect(confirmed).toMatchObject({ state: 'running', taskId: 'depart', reason: 'vtol-transition-confirmed', targetM: [0, 0, -5], transition: null, physicalFlightEnabled: false }) | |
| 33 | expect(replayVehicleMission(engine.trace()).snapshot()).toEqual(confirmed) | |
| 34 | }) | |
| 35 | ||
| 36 | function awaitingHover() { | |
| 37 | const input = createVtolHoverProfile().mission | |
| 38 | if (input.tasks[0]!.kind !== 'start') throw new Error('Expected the declared VTOL start task') | |
| 39 | input.tasks[0]!.next = 'confirm-hover' | |
| 40 | input.tasks.splice(1, 0, { id: 'confirm-hover', kind: 'vtol-mode', mode: 'hover', headingRad: 0, timeoutMs: 1000, next: 'depart' }) | |
| 41 | const reference = '55:0,0,0,0,0' | |
| 42 | const observation = (nowMs: number): VehicleMissionObservation => ({ kind: 'simulation', nowMs, reference, origin: input.origin, healthy: true, positionM: [0, 0, 0], velocityMps: [0, 0, 0], authority: 'autonomous', batteryPercent: 100, onGround: true, payloadAck: null, transition: null }) | |
| 43 | const engine = createVehicleMissionEngine(input) | |
| 44 | engine.step(observation(1000), 'start'); engine.step(observation(1020)) | |
| 45 | const command = engine.snapshot().transition! | |
| 46 | const confirmed = (capturedAtMs: number, measurementCapturedAtMs: number): VehicleVtolTransitionObservation => ({ evidence: 'modeled-vtol-controller-observation', eventId: command.eventId, taskId: command.taskId, mode: command.mode, reference: command.reference, capturedAtMs, measurementCapturedAtMs, phase: 'hover', outcome: 'confirmed', reason: 'hover-qualified' }) | |
| 47 | return { engine, observation, confirmed } | |
| 48 | } | |
| 49 | ||
| 50 | test('older VTOL phase evidence cannot confirm a request after newer progress or regain the blocked mission', () => { | |
| 51 | const { engine, observation, confirmed } = awaitingHover() | |
| 52 | expect(engine.step({ ...observation(1060), transition: { ...confirmed(1060, 1040), outcome: 'progress', reason: 'hover-qualifying' } }).taskId).toBe('confirm-hover') | |
| 53 | const replayed = engine.step({ ...observation(1080), transition: confirmed(1040, 1020) }) | |
| 54 | expect(replayed).toMatchObject({ state: 'blocked', reason: 'vtol-transition-observation-replayed', targetM: null, transition: null }) | |
| 55 | expect(engine.step(observation(1100), 'resume').state).toBe('blocked') | |
| 56 | }) | |
| 57 | ||
| 58 | test('VTOL mode outcomes require matching identity, reference, post-request capture and the requested phase', () => { | |
| 59 | for (const change of [ | |
| 60 | { eventId: 'foreign-event' }, { taskId: 'foreign-task' }, { mode: 'cruise' }, { reference: '56:0,0,0,0,0' }, | |
| 61 | { capturedAtMs: 1000, measurementCapturedAtMs: 1000 }, { capturedAtMs: 1080 }, { measurementCapturedAtMs: 1000 }, { phase: 'cruise' }, | |
| 62 | ] satisfies Partial<VehicleVtolTransitionObservation>[]) { | |
| 63 | const { engine, observation, confirmed } = awaitingHover() | |
| 64 | const refused = engine.step({ ...observation(1060), transition: { ...confirmed(1060, 1040), ...change } }) | |
| 65 | expect(refused).toMatchObject({ state: 'blocked', reason: 'vtol-transition-outcome-uncertain', targetM: null, transition: null }) | |
| 66 | expect(engine.step(observation(1080), 'resume').state).toBe('blocked') | |
| 67 | } | |
| 68 | }) | |
| 69 | ||
| 70 | test('identical retained progress is order independent but altered same-epoch confirmation is a replay', () => { | |
| 71 | const { engine, observation, confirmed } = awaitingHover() | |
| 72 | const progress: VehicleVtolTransitionObservation = { ...confirmed(1060, 1040), outcome: 'progress', reason: 'hover-qualifying' } | |
| 73 | engine.step({ ...observation(1060), transition: progress }) | |
| 74 | const reordered: VehicleVtolTransitionObservation = { | |
| 75 | reason: progress.reason, outcome: progress.outcome, phase: progress.phase, measurementCapturedAtMs: progress.measurementCapturedAtMs, capturedAtMs: progress.capturedAtMs, | |
| 76 | reference: progress.reference, mode: progress.mode, taskId: progress.taskId, eventId: progress.eventId, evidence: progress.evidence, | |
| 77 | } | |
| 78 | expect(engine.step({ ...observation(1080), transition: reordered }).taskId).toBe('confirm-hover') | |
| 79 | expect(engine.step({ ...observation(1100), transition: confirmed(1060, 1040) })).toMatchObject({ state: 'blocked', reason: 'vtol-transition-observation-replayed', transition: null }) | |
| 80 | }) | |
| 81 | ||
| 82 | test('a pending VTOL transition expires at its original task deadline and cannot be checkpointed or silently recovered', () => { | |
| 83 | for (const action of ['pause', 'recover'] as const) { | |
| 84 | const { engine, observation } = awaitingHover() | |
| 85 | expect(() => checkpointVehicleMission(engine.trace(), 1020)).toThrow('not resumable') | |
| 86 | expect(engine.step(observation(1040), action)).toMatchObject({ state: 'blocked', reason: 'vtol-transition-recovery-uncertain', targetM: null, transition: null }) | |
| 87 | expect(engine.step(observation(1060), 'resume').state).toBe('blocked') | |
| 88 | } | |
| 89 | const { engine, observation, confirmed } = awaitingHover() | |
| 90 | for (let now = 1040; now < 2000; now += 20) expect(engine.step(observation(now)).taskId).toBe('confirm-hover') | |
| 91 | expect(engine.step({ ...observation(2000), transition: confirmed(1980, 1960) })).toMatchObject({ state: 'blocked', reason: 'vtol-transition-outcome-uncertain', targetM: null, transition: null }) | |
| 92 | }) | |
| 93 | ||
| 94 | test('malformed VTOL outcome packets release the mission rather than supplying a partial confirmation', () => { | |
| 95 | for (const mutate of [ | |
| 96 | (p: VehicleVtolTransitionObservation) => { Reflect.deleteProperty(p, 'measurementCapturedAtMs') }, | |
| 97 | (p: VehicleVtolTransitionObservation) => { Reflect.set(p, 'capturedAtMs', NaN) }, | |
| 98 | (p: VehicleVtolTransitionObservation) => { Object.assign(p, { extra: true }) }, | |
| 99 | (p: VehicleVtolTransitionObservation) => { Object.assign(p, { extra: () => true }) }, | |
| 100 | (p: VehicleVtolTransitionObservation) => { p.reason = 'not a bounded reason' }, | |
| 101 | ]) { | |
| 102 | const { engine, observation, confirmed } = awaitingHover(), packet = confirmed(1060, 1040) | |
| 103 | mutate(packet) | |
| 104 | expect(() => engine.step({ ...observation(1060), transition: packet })).toThrow('Invalid vehicle mission observation') | |
| 105 | expect(engine.snapshot()).toMatchObject({ state: 'blocked', targetM: null, transition: null }) | |
| 106 | } | |
| 107 | }) | |
| 108 | ||
| 109 | test('old hover recordings keep their exact observation and snapshot keys and other vehicle plans reject VTOL tasks', () => { | |
| 110 | const input = createVtolHoverProfile().mission, engine = createVehicleMissionEngine(input) | |
| 111 | const before = engine.snapshot() | |
| 112 | const old: VehicleMissionObservation = { kind: 'simulation', nowMs: 1000, reference: '55:0,0,0,0,0', origin: input.origin, healthy: true, positionM: [0, 0, 0], velocityMps: [0, 0, 0], authority: 'autonomous', batteryPercent: 100, onGround: true, payloadAck: null } | |
| 113 | engine.step(old, 'start') | |
| 114 | expect(Object.keys(before).sort()).toEqual(['version', 'vehicle', 'evidence', 'physicalFlightEnabled', 'state', 'taskId', 'taskKind', 'targetM', 'payload', 'reason', 'elapsedMs'].sort()) | |
| 115 | expect(Object.keys(engine.snapshot()).sort()).toEqual(Object.keys(before).sort()) | |
| 116 | expect(Object.keys(engine.trace().rows[0]!.observation).sort()).toEqual(['kind', 'nowMs', 'reference', 'origin', 'healthy', 'positionM', 'velocityMps', 'authority', 'batteryPercent', 'onGround', 'payloadAck'].sort()) | |
| 117 | expect(replayVehicleMission(engine.trace()).snapshot()).toEqual(engine.snapshot()) | |
| 118 | for (const vehicle of ['fixed-wing', 'ground', 'surface', 'subsea'] as const) { | |
| 119 | const base: VehicleMissionPlan = { ...input, vehicle, tasks: [{ id: 'start', kind: 'start', next: 'abort' }, { id: 'abort', kind: 'abort' }] } | |
| 120 | expect(() => validateVehicleMission(base)).not.toThrow() | |
| 121 | base.tasks = [{ id: 'start', kind: 'start', next: 'hover' }, { id: 'hover', kind: 'vtol-mode', mode: 'hover', headingRad: 0, timeoutMs: 1000, next: 'abort' }, { id: 'abort', kind: 'abort' }] | |
| 122 | expect(() => validateVehicleMission(base)).toThrow('unsupported profile') | |
| 123 | } | |
| 124 | }) | |
| 125 | ||
| 126 | test('front and back requests retain distinct identities and require their observed destination phases in replay', () => { | |
| 127 | const input: VehicleMissionPlan = { ...createVtolHoverProfile().mission, tasks: [ | |
| 128 | { id: 'start', kind: 'start', next: 'depart' }, | |
| 129 | { id: 'depart', kind: 'takeoff', positionM: [0, 0, -5], acceptanceM: 0.2, maxArrivalSpeedMps: 0.2, settleMs: 0, timeoutMs: 2000, next: 'front' }, | |
| 130 | { id: 'front', kind: 'vtol-mode', mode: 'cruise', headingRad: 0, timeoutMs: 2000, next: 'leg' }, | |
| 131 | { id: 'leg', kind: 'waypoint', positionM: [20, 0, -5], acceptanceM: 1, maxArrivalSpeedMps: 15, settleMs: 0, timeoutMs: 2000, next: 'back' }, | |
| 132 | { id: 'back', kind: 'vtol-mode', mode: 'hover', headingRad: 0, timeoutMs: 2000, next: 'land' }, | |
| 133 | { id: 'land', kind: 'land', positionM: [20, 0, 0], acceptanceM: 0.3, maxArrivalSpeedMps: 0.2, settleMs: 0, timeoutMs: 2000 }, | |
| 134 | ] } | |
| 135 | const observation = (nowMs: number, positionM: [number, number, number] = [0, 0, -5], onGround = false, velocityMps: [number, number, number] = [0, 0, 0]): VehicleMissionObservation => ({ kind: 'simulation', nowMs, reference: '55:0,0,0,0,0', origin: input.origin, healthy: true, positionM, velocityMps, authority: 'autonomous', batteryPercent: 100, onGround, payloadAck: null, transition: null }) | |
| 136 | const outcome = (request: VehicleVtolTransitionCommand, now: number, phase: VehicleVtolTransitionObservation['phase'], status: VehicleVtolTransitionObservation['outcome']): VehicleVtolTransitionObservation => ({ evidence: 'modeled-vtol-controller-observation', eventId: request.eventId, taskId: request.taskId, mode: request.mode, reference: request.reference, capturedAtMs: now, measurementCapturedAtMs: now - 20, phase, outcome: status, reason: status === 'confirmed' ? 'mode-qualified' : 'mode-progress' }) | |
| 137 | const engine = createVehicleMissionEngine(input) | |
| 138 | engine.step(observation(1000, [0, 0, 0], true), 'start'); engine.step(observation(1020)) | |
| 139 | const front = engine.step(observation(1040)).transition! | |
| 140 | expect(front.mode).toBe('cruise') | |
| 141 | expect(engine.step({ ...observation(1060), transition: outcome(front, 1060, 'front-transition', 'progress') }).taskId).toBe('front') | |
| 142 | expect(engine.step({ ...observation(1080), transition: outcome(front, 1080, 'cruise', 'confirmed') }).taskId).toBe('leg') | |
| 143 | engine.step(observation(1100, [20, 0, -5], false, [13, 0, 0])) | |
| 144 | const back = engine.step(observation(1120, [20.2, 0, -5], false, [13, 0, 0])).transition! | |
| 145 | expect(back.mode).toBe('hover'); expect(back.eventId).not.toBe(front.eventId) | |
| 146 | const foreign = replayVehicleMission(engine.trace()) | |
| 147 | expect(foreign.step({ ...observation(1140), transition: outcome(front, 1140, 'cruise', 'confirmed') }).reason).toBe('vtol-transition-outcome-uncertain') | |
| 148 | expect(engine.step({ ...observation(1140), transition: outcome(back, 1140, 'back-transition', 'progress') }).taskId).toBe('back') | |
| 149 | expect(engine.step({ ...observation(1160), transition: outcome(back, 1160, 'hover', 'confirmed') }).taskId).toBe('land') | |
| 150 | expect(engine.step(observation(1180, [20, 0, 0], true)).state).toBe('completed') | |
| 151 | expect(replayVehicleMission(engine.trace()).snapshot()).toEqual(engine.snapshot()) | |
| 152 | }) | |
| 153 | ||
| 154 | test('observed transition aborts and confirmations reused after consumption cannot advance or retry the mission', () => { | |
| 155 | const aborted = awaitingHover() | |
| 156 | expect(aborted.engine.step({ ...aborted.observation(1060), transition: { ...aborted.confirmed(1060, 1040), outcome: 'aborted', reason: 'transition-rejected' } })).toMatchObject({ state: 'blocked', reason: 'vtol-transition-aborted', transition: null, targetM: null }) | |
| 157 | expect(aborted.engine.step(aborted.observation(1080), 'resume').state).toBe('blocked') | |
| 158 | const consumed = awaitingHover(), packet = consumed.confirmed(1060, 1040) | |
| 159 | expect(consumed.engine.step({ ...consumed.observation(1060), transition: packet }).taskId).toBe('depart') | |
| 160 | expect(consumed.engine.step({ ...consumed.observation(1080), transition: packet })).toMatchObject({ state: 'blocked', reason: 'unsolicited-vtol-transition', transition: null, targetM: null }) | |
| 161 | const operator = awaitingHover() | |
| 162 | expect(operator.engine.step(operator.observation(1040), 'abort')).toMatchObject({ state: 'aborted', reason: 'operator-abort', transition: null, targetM: null }) | |
| 163 | }) | |
| @@ -23,8 +23,8 @@ test('the owned VTOL hover mission takes off vertically and lands under shared m | ||
| 23 | 23 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) |
| 24 | 24 | }) |
| 25 | 25 | |
| 26 | function observedControllerInputs() { | |
| 27 | const profile = createVtolHoverProfile(), createSession = profile.createSession, inputs: VehicleControllerContext[] = [] | |
| 26 | function observedControllerInputs(profile = createVtolHoverProfile(), expectedStatus: ReturnType<typeof runVehicleSimulation>['status'] = 'completed') { | |
| 27 | const createSession = profile.createSession, inputs: VehicleControllerContext[] = [] | |
| 28 | 28 | profile.createSession = () => { |
| 29 | 29 | const session = createSession(), createController = session.createController |
| 30 | 30 | return { ...session, createController() { |
| @@ -32,7 +32,7 @@ function observedControllerInputs() { | ||
| 32 | 32 | return { step(context) { inputs.push(structuredClone(context)); return controller.step(context) } } |
| 33 | 33 | } } |
| 34 | 34 | } |
| 35 | expect(runVehicleSimulation(profile, { seconds: 60 }).status).toBe('completed') | |
| 35 | expect(runVehicleSimulation(profile, { seconds: 60 }).status).toBe(expectedStatus) | |
| 36 | 36 | return inputs |
| 37 | 37 | } |
| 38 | 38 | |
| @@ -92,3 +92,185 @@ test('separate VTOL sessions retain independent controller faults and returned a | ||
| 92 | 92 | expect(unaffected.inhibited).toBe(false) |
| 93 | 93 | expect(unaffected.actuators.slice(0, 4).every(n => n >= 0 && n <= 12)).toBe(true) |
| 94 | 94 | }) |
| 95 | ||
| 96 | ||
| 97 | function initialModeProfile(mode: 'hover' | 'cruise' = 'hover', headingRad = 0) { | |
| 98 | const profile = createVtolHoverProfile() | |
| 99 | profile.mission.id = 'vtol-observed-initial-hover' | |
| 100 | if (profile.mission.tasks[0]!.kind !== 'start') throw new Error('Expected the declared start task') | |
| 101 | profile.mission.tasks[0]!.next = 'confirm-hover' | |
| 102 | profile.mission.tasks.splice(1, 0, { id: 'confirm-hover', kind: 'vtol-mode', mode, headingRad, timeoutMs: 1000, next: 'depart' }) | |
| 103 | return profile | |
| 104 | } | |
| 105 | ||
| 106 | test('an actual initial hover observation crosses the shared runtime once before vertical departure', () => { | |
| 107 | const profile = initialModeProfile(), createSession = profile.createSession | |
| 108 | const outputs: { context: VehicleControllerContext, output: ReturnType<ReturnType<typeof createVtolVehicleHoverController>['step']> }[] = [] | |
| 109 | profile.createSession = () => { | |
| 110 | const session = createSession(), createController = session.createController | |
| 111 | return { ...session, createController() { | |
| 112 | const controller = createController() | |
| 113 | return { step(context) { | |
| 114 | const output = controller.step(context) | |
| 115 | outputs.push({ context: structuredClone(context), output: structuredClone(output) }) | |
| 116 | return output | |
| 117 | } } | |
| 118 | } } | |
| 119 | } | |
| 120 | const report = runVehicleSimulation(profile, { seconds: 60 }) | |
| 121 | expect(report.status).toBe('completed') | |
| 122 | const confirmed = outputs.filter(row => row.output.transition?.outcome === 'confirmed') | |
| 123 | expect(confirmed).toHaveLength(1) | |
| 124 | const { context, output } = confirmed[0]!, request = context.mission.transition! | |
| 125 | expect(context.measurement!.capturedAtMs).toBeGreaterThanOrEqual(request.issuedAtMs) | |
| 126 | expect(output.transition).toMatchObject({ eventId: request.eventId, taskId: 'confirm-hover', mode: 'hover', phase: 'hover', outcome: 'confirmed', reference: request.reference, capturedAtMs: context.nowMs }) | |
| 127 | expect(output.transition!.measurementCapturedAtMs).toBeGreaterThanOrEqual(request.issuedAtMs + 200) | |
| 128 | const delivered = report.trace.rows.filter(row => row.observation.transition?.outcome === 'confirmed') | |
| 129 | expect(delivered).toHaveLength(1) | |
| 130 | expect(delivered[0]!.observation.nowMs).toBe(context.nowMs + 20) | |
| 131 | expect(delivered[0]!.observation.transition).toEqual(output.transition) | |
| 132 | expect(outputs.filter(row => row.context.mission.taskId === 'confirm-hover').every(row => row.output.actuators.every(n => n === 0))).toBe(true) | |
| 133 | for (const row of outputs.filter(row => row.output.transition)) { | |
| 134 | const epoch = row.output.transition!.measurementCapturedAtMs | |
| 135 | expect(epoch).toBeLessThanOrEqual(row.context.measurement!.capturedAtMs) | |
| 136 | expect(epoch).toBeLessThanOrEqual(row.context.controlRate!.capturedAtMs) | |
| 137 | expect(epoch).toBeLessThanOrEqual(row.context.estimate.attitude!.capturedAtMs!) | |
| 138 | expect(epoch).toBeLessThanOrEqual(row.context.estimate.localPosition!.capturedAtMs!) | |
| 139 | } | |
| 140 | expect(report.path.some(row => row.truthPositionM[2] < -4)).toBe(true) | |
| 141 | expect(report.path.at(-1)!.measurement!.values.onSurface).toBe(true) | |
| 142 | expect(report.board.expiresAtMs).toBeNull() | |
| 143 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 144 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) | |
| 145 | }) | |
| 146 | ||
| 147 | ||
| 148 | test('initial mode observation cannot survive real sensor or host loss before its qualifying dwell', () => { | |
| 149 | for (const drop of ['dropImu', 'dropPosition', 'dropVehicleSensor', 'hostLost'] as const) { | |
| 150 | const profile = initialModeProfile() | |
| 151 | profile.faults = (_scenario, atMs) => atMs >= 620 ? { [drop]: true } : {} | |
| 152 | const report = runVehicleSimulation(profile, { seconds: 2 }) | |
| 153 | expect(report.status).toBe(drop === 'hostLost' ? 'host-lost' : 'blocked') | |
| 154 | expect(report.trace.rows.some(row => row.observation.transition?.outcome === 'confirmed')).toBe(false) | |
| 155 | expect(report.trace.rows.some(row => row.result.taskId === 'depart')).toBe(false) | |
| 156 | expect(report.path.every(row => row.actuators.every(n => n === 0))).toBe(true) | |
| 157 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 158 | expect(report.board.expiresAtMs).toBeNull() | |
| 159 | if (drop === 'hostLost') expect(report.board.fault).toBe('output-lease-expired') | |
| 160 | else expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) | |
| 161 | } | |
| 162 | }) | |
| 163 | ||
| 164 | test('the hover adapter reports unsupported cruise as an abort and a mismatched initial heading times out', () => { | |
| 165 | const unsupported = runVehicleSimulation(initialModeProfile('cruise'), { seconds: 2 }) | |
| 166 | expect(unsupported.status).toBe('blocked') | |
| 167 | expect(unsupported.mission.reason).toBe('vtol-transition-aborted') | |
| 168 | const aborted = unsupported.trace.rows.find(row => row.observation.transition?.outcome === 'aborted')!.observation.transition! | |
| 169 | expect(aborted).toMatchObject({ mode: 'cruise', phase: 'hover', reason: 'hover-only-controller' }) | |
| 170 | expect(unsupported.trace.rows.some(row => row.result.taskId === 'depart')).toBe(false) | |
| 171 | const wrongHeading = runVehicleSimulation(initialModeProfile('hover', 1), { seconds: 2 }) | |
| 172 | expect(wrongHeading.status).toBe('blocked') | |
| 173 | expect(wrongHeading.mission.reason).toBe('vtol-transition-outcome-uncertain') | |
| 174 | expect(wrongHeading.trace.rows.some(row => row.observation.transition?.outcome === 'confirmed')).toBe(false) | |
| 175 | expect(wrongHeading.path.every(row => row.actuators.every(n => n === 0))).toBe(true) | |
| 176 | for (const report of [unsupported, wrongHeading]) { | |
| 177 | expect(report.mission.targetM).toBeNull() | |
| 178 | expect(report.mission.transition).toBeNull() | |
| 179 | expect(report.board.expiresAtMs).toBeNull() | |
| 180 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 181 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) | |
| 182 | } | |
| 183 | }) | |
| 184 | ||
| 185 | ||
| 186 | test('fresh ground contact cannot bypass current rate, request reference or expiry before mode confirmation', () => { | |
| 187 | const inputs = observedControllerInputs(initialModeProfile()) | |
| 188 | const index = inputs.findIndex(c => c.mission.transition && c.measurement!.capturedAtMs >= c.mission.transition.issuedAtMs) | |
| 189 | expect(index).toBeGreaterThan(0) | |
| 190 | for (const mutate of [ | |
| 191 | (c: VehicleControllerContext) => { c.controlRate!.capturedAtMs -= 1000 }, | |
| 192 | (c: VehicleControllerContext) => { c.controlRate!.reference = 'another-reference' }, | |
| 193 | (c: VehicleControllerContext) => { c.mission.transition!.reference = 'another-reference' }, | |
| 194 | (c: VehicleControllerContext) => { c.mission.transition!.expiresAtMs = c.nowMs }, | |
| 195 | (c: VehicleControllerContext) => { c.mission.transition!.issuedAtMs = c.nowMs + 20 }, | |
| 196 | (c: VehicleControllerContext) => { c.mission.transition!.headingRad = NaN }, | |
| 197 | ]) { | |
| 198 | const controller = createVtolVehicleHoverController() | |
| 199 | for (const c of inputs.slice(0, index)) expect(controller.step(c).inhibited).toBe(false) | |
| 200 | const invalid = structuredClone(inputs[index]!) | |
| 201 | mutate(invalid) | |
| 202 | const refused = controller.step(invalid) | |
| 203 | expect(refused.inhibited).toBe(true) | |
| 204 | expect(refused.transition).toBeUndefined() | |
| 205 | expect(refused.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 206 | expect(controller.step(inputs[index + 1]!).inhibited).toBe(true) | |
| 207 | } | |
| 208 | }) | |
| 209 | ||
| 210 | test('legacy hover runtime records and outputs retain their shapes when no explicit modes are declared', () => { | |
| 211 | const profile = createVtolHoverProfile(), createSession = profile.createSession | |
| 212 | let emitted = false | |
| 213 | profile.createSession = () => { | |
| 214 | const session = createSession(), controller = session.createController() | |
| 215 | return { ...session, createController: () => ({ step(c) { | |
| 216 | const result = controller.step(c) | |
| 217 | if (Object.hasOwn(result, 'transition')) emitted = true | |
| 218 | return result | |
| 219 | } }) } | |
| 220 | } | |
| 221 | const report = runVehicleSimulation(profile, { seconds: 60 }) | |
| 222 | expect(report.status).toBe('completed') | |
| 223 | expect(emitted).toBe(false) | |
| 224 | expect(report.path.every(row => !Object.hasOwn(row, 'transitionObservation') && !Object.hasOwn(row.mission, 'transition'))).toBe(true) | |
| 225 | expect(report.trace.rows.every(row => !Object.hasOwn(row.observation, 'transition') && !Object.hasOwn(row.result, 'transition'))).toBe(true) | |
| 226 | }) | |
| 227 | ||
| 228 | ||
| 229 | test('a first hover request for a different position cannot certify the held target or clear its refusal', () => { | |
| 230 | const inputs = observedControllerInputs(initialModeProfile()) | |
| 231 | const index = inputs.findIndex(c => c.mission.transition !== null) | |
| 232 | const invalid = structuredClone(inputs[index]!) | |
| 233 | expect(invalid.mission.targetM).toEqual([0, 0, 0]) | |
| 234 | invalid.mission.transition!.positionM = [10, 0, 0] | |
| 235 | const controller = createVtolVehicleHoverController(), result = controller.step(invalid) | |
| 236 | expect(result.inhibited).toBe(true) | |
| 237 | expect(result.transition).toBeUndefined() | |
| 238 | expect(result.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 239 | expect(controller.step(inputs[index + 1]!).inhibited).toBe(true) | |
| 240 | }) | |
| 241 | ||
| 242 | test('changing capture lag cannot shorten the observed hover stability dwell', () => { | |
| 243 | // Real ground sensor/estimator contexts from an unobserved-heading mission retain the actual | |
| 244 | // 100 ms GNSS and 60 ms contact cadence. A fresh public controller observes north instead. | |
| 245 | const inputs = observedControllerInputs(initialModeProfile('hover', 1), 'blocked') | |
| 246 | const controller = createVtolVehicleHoverController(), confirmed: number[] = [] | |
| 247 | const observed = inputs.filter(c => c.nowMs >= 900 && c.nowMs <= 1100) | |
| 248 | expect(observed).toHaveLength(11) | |
| 249 | expect(observed[0]!.estimate.localPosition!.capturedAtMs).toBe(800) | |
| 250 | for (const input of observed) { | |
| 251 | input.mission.transition!.headingRad = 0 | |
| 252 | const output = controller.step(input) | |
| 253 | expect(output.inhibited).toBe(false) | |
| 254 | if (output.transition?.outcome === 'confirmed') confirmed.push(input.nowMs) | |
| 255 | } | |
| 256 | expect(confirmed).toHaveLength(1) | |
| 257 | expect(confirmed[0]!).toBeGreaterThanOrEqual(1100) | |
| 258 | }) | |
| 259 | ||
| 260 | ||
| 261 | test('custom target iterators cannot certify a different position from its own numeric slots', () => { | |
| 262 | const inputs = observedControllerInputs(initialModeProfile('hover', 1), 'blocked'), controller = createVtolVehicleHoverController() | |
| 263 | let iteratorCalls = 0, confirmed = false | |
| 264 | for (const input of inputs.filter(c => c.nowMs >= 900 && c.nowMs <= 1120)) { | |
| 265 | const heldTarget: [number, number, number] = [10, 0, 0] | |
| 266 | Object.defineProperty(heldTarget, Symbol.iterator, { value: function* () { iteratorCalls++; yield 0; yield 0; yield 0 } }) | |
| 267 | input.mission.targetM = heldTarget | |
| 268 | input.mission.transition!.positionM = [10, 0, 0] | |
| 269 | input.mission.transition!.headingRad = 0 | |
| 270 | const result = controller.step(input) | |
| 271 | expect(result.inhibited).toBe(false) | |
| 272 | if (result.transition?.outcome === 'confirmed') confirmed = true | |
| 273 | } | |
| 274 | expect(confirmed).toBe(false) | |
| 275 | expect(iteratorCalls).toBe(0) | |
| 276 | }) | |
| @@ -171,3 +171,26 @@ test('inherited slots and custom iterators cannot disguise malformed lift torque | ||
| 171 | 171 | for (const torqueNm of [sparse, rewritten]) expect(() => allocateVtolLift({ thrustN: 24, torqueNm } as never)).toThrow() |
| 172 | 172 | expect(iteratorCalls).toBe(0) |
| 173 | 173 | }) |
| 174 | ||
| 175 | ||
| 176 | test('declared skid friction stops residual ground motion gradually without braking an airborne body', () => { | |
| 177 | const environment = { windMps: [0, 0, 0] as [number, number, number], currentMps: [0, 0, 0] as [number, number, number] }, neutral = [0, 0, 0, 0, 0, 0, 0, 0] | |
| 178 | const ground = createVtolPlant({ velocityMps: [0.24, 0, 0] }) | |
| 179 | const first = ground.step(neutral, 0.004, environment) | |
| 180 | expect(first.onSurface).toBe(true) | |
| 181 | expect(first.velocityMps[0]).toBeGreaterThan(0.20) | |
| 182 | expect(first.velocityMps[0]).toBeLessThan(0.23) | |
| 183 | let stopped = first | |
| 184 | for (let step = 1; step < 250; step++) stopped = ground.step(neutral, 0.004, environment) | |
| 185 | expect(Math.hypot(...stopped.velocityMps)).toBeLessThan(1e-8) | |
| 186 | expect(stopped.positionM[0]).toBeGreaterThan(0) | |
| 187 | expect(stopped.positionM[0]).toBeLessThan(0.02) | |
| 188 | const air = createVtolPlant({ positionM: [0, 0, -1], velocityMps: [0.24, 0, 0] }) | |
| 189 | const flying = air.step(neutral, 0.004, environment) | |
| 190 | expect(flying.onSurface).toBe(false) | |
| 191 | expect(flying.velocityMps[0]).toBeGreaterThan(0.239) | |
| 192 | const liftSupported = createVtolPlant({ velocityMps: [0.24, 0, 0], rotorsN: [6.12915625, 6.12915625, 6.12915625, 6.12915625] }) | |
| 193 | const unloaded = liftSupported.step([6.12915625, 6.12915625, 6.12915625, 6.12915625, 0, 0, 0, 0], 0.004, environment) | |
| 194 | expect(unloaded.velocityMps[0]).toBeGreaterThan(0.239) | |
| 195 | expect(unloaded.positionM[2]).toBeLessThan(0) | |
| 196 | }) | |