also looking at this
feat: model owned VTOL flight transitions
#78
13 files
+1087
-33
Review threads live on the whole diff, not on one commit, so none are shown here - a thread's line means something in the branch's final form, and painting it into an intermediate step would put it on code it is not about.
| @@ -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,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 | } |
| @@ -77,6 +77,8 @@ export interface VehicleMissionSnapshot { | ||
| 77 | 77 | export type VehicleMissionAction = MissionAction |
| 78 | 78 | export interface VehicleControlOutput { |
| 79 | 79 | kind: 'simulated-vehicle-control' |
| 80 | /** One capture-bound response, consumed by the next mission evaluation for explicit mode plans. */ | |
| 81 | transition?: VehicleVtolTransitionObservation | null | |
| 80 | 82 | vehicle: VehicleKind |
| 81 | 83 | atMs: number |
| 82 | 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 |
| @@ -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 | }) | |