also looking at this
feat: supervise modeled VTOL mission actions
#79
13 files
+962
-24
| @@ -76,9 +76,11 @@ These profiles use owned raw sensors, estimation, mission policy, controls and a | ||
| 76 | 76 | outputs remain inhibited. Read the [shared contract](docs/vehicles.md), [air model](docs/vehicles-air.md), |
| 77 | 77 | [ground model](docs/vehicles-ground.md) and [surface corrections](docs/vehicles-surface.md) and [subsea pressure/DVL inspection](docs/vehicles-subsea.md). |
| 78 | 78 | |
| 79 | `./buddy vtol:hover` separately exercises [VTOL hover development](docs/vehicles-vtol.md): | |
| 80 | vertical takeoff, a nearby hover waypoint and contact-gated landing. Wing transitions, cruise, | |
| 81 | hybrid energy qualification and a selectable VTOL mission workspace remain open. | |
| 79 | `./buddy vtol:hover` separately exercises [VTOL development](docs/vehicles-vtol.md): | |
| 80 | vertical takeoff, a nearby hover waypoint and contact-gated landing. The TypeScript library also | |
| 81 | includes a bounded modeled front/cruise/back experiment with stable-phase pause and resume. The | |
| 82 | command stays hover-only. Controlled transition recovery and abort landing, complete hybrid energy | |
| 83 | qualification and a selectable VTOL mission workspace remain open. | |
| 82 | 84 | |
| 83 | 85 | ## Simulated carried load |
| 84 | 86 | |
| @@ -76,7 +76,7 @@ const config: BunPressOptions = { | ||
| 76 | 76 | items: [ |
| 77 | 77 | { text: 'Owned vehicle runtime', link: '/vehicles' }, |
| 78 | 78 | { text: 'Owned fixed-wing profile', link: '/vehicles-air' }, |
| 79 | { text: 'VTOL hover development', link: '/vehicles-vtol' }, | |
| 79 | { text: 'VTOL development', link: '/vehicles-vtol' }, | |
| 80 | 80 | { text: 'Owned ground vehicle profile', link: '/vehicles-ground' }, |
| 81 | 81 | { text: 'Owned surface vessel profile', link: '/vehicles-surface' }, |
| 82 | 82 | { text: 'Owned subsea inspection profile', link: '/vehicles-subsea' }, |
| @@ -56,8 +56,10 @@ surface and subsea profiles. [Optional external drivers](drivers.md) have | ||
| 56 | 56 | [acknowledged protocol services](driver-protocols.md); scripted peers are distinct |
| 57 | 57 | from [firmware SITL](sitl.md) and physical testing. |
| 58 | 58 | |
| 59 | The separate [VTOL hover development command](vehicles-vtol.md) exercises vertical takeoff, | |
| 60 | hover and landing. Front/cruise/back transitions and the common VTOL mission workspace remain open. | |
| 59 | The separate [VTOL development guide](vehicles-vtol.md) covers the hover-only command and an | |
| 60 | experimental TypeScript library profile for modeled front transition, cruise, back transition and | |
| 61 | vertical landing, with stable-phase pause and resume. Controlled transition recovery and abort | |
| 62 | landing, complete hybrid energy qualification and the common VTOL mission workspace remain open. | |
| 61 | 63 | |
| 62 | 64 | ## Explore a workflow |
| 63 | 65 | |
| @@ -16,7 +16,7 @@ means there is no native implementation yet. A protocol-compatible transport is | ||
| 16 | 16 | | Diagnostics, logs, replay, firmware update and configuration provenance | Owned runtime journal, loss accounting, replay, CLI/viewer and [protected signed preparation, owned candidates, host slots, observed image/session health and fresh-policy rollback](../runtime-updates.md); full mission correlation, key repair, complete storage faults and board install/boot/rollback qualification remain pending | Named parameter read/write with bounded readback in the SITL adapter; no log/FTP services, full-table synchronization or reboot persistence qualification | [#9](https://github.com/stacksjs/drone/issues/9) | |
| 17 | 17 | | Missions, tasks, waypoints, terrain, rally and resume | Owned versioned graph, flat-terrain/fence/rally/energy validation, physics execution, pause/resume, recorded recovery, CLI and stx viewer; correlated durable checkpoints, driver translation and hardware evidence pending | Bounded SITL waypoint upload/download and acknowledged item selection/resume; no arming, automatic flight-mode selection or owned-plan translation | [#10](https://github.com/stacksjs/drone/issues/10) | |
| 18 | 18 | | Avoidance, route planning, precision landing, follow and orbit | Owned modeled range detection, inflated route planning, terrain-clearance/RTL/selected-site landing physics; metric follow/orbit goals; physical calibration and dynamic obstacle campaigns pending | No avoidance/landing integration | [#11](https://github.com/stacksjs/drone/issues/11) | |
| 19 | | Fixed-wing, VTOL, heli, blimp and experimental airframes | Owned fixed-wing runway takeoff/survey/landing and [VTOL hover development](../vehicles-vtol.md) with vertical takeoff/hover/contact-gated landing; VTOL transitions/cruise, hybrid energy, helicopter, soaring, blimp and experimental families remain open | Current drivers reject non-quadcopter heartbeats | [#12](https://github.com/stacksjs/drone/issues/12) | | |
| 19 | | Fixed-wing, VTOL, heli, blimp and experimental airframes | Owned fixed-wing runway takeoff/survey/landing, hover-only VTOL command and [experimental VTOL library](../vehicles-vtol.md) with modeled front/cruise/back/vertical landing and stable-phase pause/resume; controlled transition recovery/abort landing, complete hybrid energy, native transition CLI/workspace, helicopter, soaring, blimp and other airframe families remain open | Current drivers reject non-quadcopter heartbeats | [#12](https://github.com/stacksjs/drone/issues/12) | | |
| 20 | 20 | | Rovers, mowing, logistics and materials handling | Owned differential traction/braking, encoder/GNSS odometry, guarded mowing coverage and scoped material-drop simulation; hardware and broader chassis/implements pending | Not exposed | [#13](https://github.com/stacksjs/drone/issues/13) | |
| 21 | 21 | | Surface vessels, bathymetry, water sampling and harbours | Owned planar hull/current/wind, collision inhibition, corrected single-beam export, scoped sample ACK and energy-return simulation; physical survey/sampling and broader hulls pending | Not exposed | [#14](https://github.com/stacksjs/drone/issues/14) | |
| 22 | 22 | | Subsea ROV, depth hold, tether and leak handling | Owned eight-thruster 6DOF physics, pressure/DVL local estimation, depth hold, tether/leak supervision and scoped synthetic camera/sonar evidence; hardware, full hydrodynamics and physical inspection pending | Not exposed | [#15](https://github.com/stacksjs/drone/issues/15) | |
| @@ -1,9 +1,10 @@ | ||
| 1 | # VTOL hover development | |
| 1 | # VTOL development | |
| 2 | 2 | |
| 3 | 3 | The owned lift-and-cruise model can execute a **hover-only** mission through the shared raw sensor, |
| 4 | 4 | estimator, mission, authority and independent output-lease runtime. It takes off vertically, reaches |
| 5 | 5 | a nearby hover waypoint and lands against fresh modeled contact evidence. Wing transitions and |
| 6 | cruise control are not implemented. This development profile is separate from the selectable | |
| 6 | cruise control are not implemented in this hover profile. A separate experimental library controller | |
| 7 | is described below. This development profile is separate from the selectable | |
| 7 | 8 | profiles in the `/missions` workspace. |
| 8 | 9 | |
| 9 | 10 | ```sh |
| @@ -64,14 +65,99 @@ neutral until observation is qualified. Unobserved headings expire, and this hov | ||
| 64 | 65 | cruise requests as aborted. This is a library prerequisite, not front/back transition control or a |
| 65 | 66 | new CLI scenario. Existing hover-only recordings keep their prior shapes. |
| 66 | 67 | |
| 68 | ## Experimental lift-and-cruise library controller | |
| 69 | ||
| 70 | `createVtolTransitionProfile()` in `resources/functions/drone/vehicles/vtol/transition-profile.ts` | |
| 71 | is a separate development experiment on the same owned runtime. Its nominal mission observes initial | |
| 72 | hover, takes off vertically to 15 m, requests a front transition, flies a cruise leg, requests back | |
| 73 | transition, returns to the back request's capture point, observes settled hover, and lands vertically. | |
| 74 | A turning leg also has an actual completed model campaign. This profile is not registered as a CLI | |
| 75 | scenario or selectable workspace profile. | |
| 76 | ||
| 77 | The controller receives measured relative airspeed, differential pressure, airflow angles and modeled | |
| 78 | rotor/pusher response, together with the qualified raw gyro and estimated navigation. Delayed airflow | |
| 79 | angles are transformed through their matching capture-time attitude. The measured NED airflow vector | |
| 80 | is held to the decision and then expressed in the current gyro-predicted body attitude. This is a | |
| 81 | bounded synthetic observation approximation, not identified wind estimation or physical air-data | |
| 82 | calibration. Banked navigation follows the route while coordinated yaw follows the measured air | |
| 83 | course. Surface torque and residual rotor torque are allocated together; measured wing lift reduces | |
| 84 | collective rather than adding full rotor weight support on top of the wing. | |
| 85 | ||
| 86 | Front confirmation requires sustained measured 12 to 14 m/s relative airflow, bounded angles, | |
| 87 | height/rate and less than 1 N of measured and commanded lift-rotor support. Both decision time and | |
| 88 | oldest-source coverage must qualify for 500 ms, including the attitude and gyro epochs used to | |
| 89 | transform delayed airflow. Ground speed or a timer alone cannot confirm cruise. | |
| 90 | A failed modeled pusher and an artificial co-moving-air fixture retain failed transition outcomes. | |
| 91 | ||
| 92 | Back transition turns the pusher off and decelerates under the declared aerodynamic model before | |
| 93 | returning to the original capture point at a horizontal target speed bounded to 3 m/s. The nominal | |
| 94 | model needs about 38 seconds for deceleration, return and hover qualification. Observed hover also | |
| 95 | requires wrapped heading error within 0.1 rad for the complete 500 ms dwell. Hover/back requests | |
| 96 | therefore have a finite 45-second ceiling; front requests retain 15 seconds. The original 15-second | |
| 97 | back budget still fails without retry or a claimed landing. This is a virtual task allowance, | |
| 98 | not a wall-clock test timeout. The complete declared mission budget is 131,520 ms, or 43.84 Wh at its | |
| 99 | 1200 W ceiling, with 100 Wh capacity and 30% reserve. These remain declared planning values, without | |
| 100 | a complete hybrid electrical-power law or measured battery identification. | |
| 101 | ||
| 102 | The experiment keeps the 2 m transition-height deviation, 0.6 rad roll, 0.5 rad pitch and 22 m/s | |
| 103 | speed guards. Its own target range is bounded to 400 m. The quadcopter and hover development kernel's | |
| 104 | existing 3 m/s horizontal target and 100 m target-radius guards are unchanged. Source, capture-pose, | |
| 105 | reference and clock faults latch. Real modeled sensor loss, RC override, kill and host-loss campaigns | |
| 106 | cover front transition, cruise, back transition and post-back landing through the independent output | |
| 107 | lease. Neutral outputs during an airborne failure do not demonstrate a safe recovery or landing. | |
| 108 | ||
| 109 | ## Operator actions in modeled sessions | |
| 110 | ||
| 111 | The shared runtime accepts a captured schedule of `pause`, `resume`, `recover` and `abort` actions. | |
| 112 | For example, from a TypeScript file in the project root: | |
| 113 | ||
| 114 | ```ts | |
| 115 | import { runVehicleSimulation } from './resources/functions/drone/vehicles/runtime' | |
| 116 | import { createVtolTransitionProfile } from './resources/functions/drone/vehicles/vtol/transition-profile' | |
| 117 | ||
| 118 | const report = runVehicleSimulation(createVtolTransitionProfile(), { | |
| 119 | seconds: 160, | |
| 120 | missionActions: [ | |
| 121 | { atMs: 20000, action: 'pause' }, | |
| 122 | { atMs: 40000, action: 'resume' }, | |
| 123 | ], | |
| 124 | }) | |
| 125 | ``` | |
| 126 | ||
| 127 | Each schedule record contains only `atMs` and `action`. Up to 32 events must have unique, | |
| 128 | strictly increasing 20 ms epochs after the automatic start at 600 ms and within the requested | |
| 129 | simulation duration. Validation and capture happen before session resources are created. Events | |
| 130 | are consumed once by the existing mission policy and retained in its replayable trace. An omitted | |
| 131 | or empty schedule preserves prior recordings and report fields. | |
| 132 | ||
| 133 | A qualified hover pause holds the captured estimated position. Wing-borne cruise uses a captured | |
| 134 | clockwise 60 m holding circle, with coordinated bank and radial course feedback. The complete circle | |
| 135 | must fit inside the declared 400 m horizontal envelope. Straight and banked-entry model campaigns | |
| 136 | hold the same 8 m radial-error bound after six seconds, maintain wing support and height, then resume | |
| 137 | the route and land. Post-back airborne pause also arrests descent around the captured position. | |
| 138 | Qualified stopped ground contact stays neutral during pause, with source and envelope checks applied | |
| 139 | before that return. Sensor loss, RC takeover, kill and host loss retain authority withdrawal. | |
| 140 | ||
| 141 | `recover` pauses the already running session until explicit `resume`. Fresh-process restoration after | |
| 142 | host loss or a changed estimator reference remains a separate gate. Pending initial-hover, front or | |
| 143 | back mode work retains uncertain-outcome refusal for pause/recover. Those refusals release modeled | |
| 144 | outputs; a controlled airborne recovery and observed landing still require an explicit route. | |
| 145 | `abort` terminates the modeled mission and releases outputs as well. | |
| 146 | ||
| 147 | Task execution time stops during pause. Global endurance and the declared energy charge continue, | |
| 148 | so a later scheduled resume can be preempted by reserve, endurance, health or authority refusal. | |
| 149 | Operator actions never extend the original planning allowance or rearm revoked authority. The | |
| 150 | transition library and these actions remain separate from the hover-only CLI and common workspace. | |
| 151 | ||
| 67 | 152 | ## Remaining implementation |
| 68 | 153 | |
| 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 | |
| 71 | qualification, bounded transition abort/recovery, complete hybrid power/reserve accounting and | |
| 72 | the common mission workspace. Full TECS/soaring, tiltrotor/tailsitter mechanisms, helicopters and | |
| 73 | other airframe families remain separate work. Native board control, measured airframe identification, | |
| 74 | HIL and physical flight require their own evidence. | |
| 154 | [Issue #12](https://github.com/stacksjs/drone/issues/12) remains open. The experimental nominal | |
| 155 | controller does not establish complete VTOL support. Controlled front/back recovery and fresh-process | |
| 156 | restoration, explicit bounded transition abort and landing, the complete hybrid power/reserve model, native transition CLI and | |
| 157 | the common mission workspace still require implementation and qualification. Wind campaigns and | |
| 158 | broader mission/envelope coverage remain necessary. Full TECS/soaring, tiltrotor/tailsitter mechanisms, | |
| 159 | helicopters and other airframe families remain separate work. Native board control, measured airframe | |
| 160 | identification, HIL and physical flight require their own evidence. | |
| 75 | 161 | |
| 76 | 162 | Read the [shared vehicle contract](vehicles.md), [fixed-wing model](vehicles-air.md) and |
| 77 | 163 | [authority contract](authority.md). |
| @@ -60,10 +60,12 @@ interlocks and host loss as applicable. Tests compare public decisions and physi | ||
| 60 | 60 | recompute mission replay, reject altered traces and exercise payload uncertainty and session reuse. |
| 61 | 61 | All evidence is owned physics simulation. The Pico neutral-only board image does not run these |
| 62 | 62 | TypeScript controllers. Native board execution, external firmware SITL, HIL and measured physical |
| 63 | vehicle acceptance remain separate gates. VTOL, helicopter, soaring, blimp and other vehicle | |
| 64 | families remain explicit roadmap work. A separate [VTOL hover development command](vehicles-vtol.md) | |
| 65 | now exercises vertical takeoff, a nearby hover waypoint and contact-gated landing. It does not | |
| 66 | implement wing transitions or add a VTOL selection to the common workspace. | |
| 63 | vehicle acceptance remain separate gates. A separate [VTOL development guide](vehicles-vtol.md) | |
| 64 | covers the hover-only command and an experimental lift-and-cruise library profile. The latter | |
| 65 | executes a bounded modeled front/cruise/back/vertical-landing route with stable-phase pause and | |
| 66 | resume. Controlled transition recovery and abort landing, complete hybrid energy qualification, | |
| 67 | a native transition command and a VTOL selection in the common workspace remain open. Helicopter, | |
| 68 | soaring, blimp and other airframe families remain explicit roadmap work. | |
| 67 | 69 | |
| 68 | 70 | Read [air](vehicles-air.md), [ground](vehicles-ground.md), [surface](vehicles-surface.md), [subsea](vehicles-subsea.md), |
| 69 | 71 | [mission semantics](missions.md) and [the replacement matrix](roadmap/feature-coverage.md). |
| @@ -28,7 +28,7 @@ export function validateVehicleMission(input: unknown): { plan: VehicleMissionPl | ||
| 28 | 28 | else if (t.kind === 'abort') { /* Terminal release, never an emergency landing. */ } |
| 29 | 29 | else if (t.kind === 'vtol-mode') { |
| 30 | 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() | |
| 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, t.mode === 'hover' ? 45000 : 15000) || !id(t.next)) invalid() | |
| 32 | 32 | durationMs = t.timeoutMs; edges = [t.next] |
| 33 | 33 | } |
| 34 | 34 | else if (t.kind === 'branch') { |
| @@ -1,18 +1,37 @@ | ||
| 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, VehicleVtolTransitionObservation } from './types' | |
| 3 | import type { VehicleControlOutput, VehicleControllerContext, VehicleMeasurement, VehicleMeasurementPose, VehicleMissionAction, 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' |
| 7 | 7 | import { createVehicleMissionEngine, validateVehicleMission } from '../missions/vehicle' |
| 8 | 8 | import { createVehicleAuthoritySupervisor, createVehicleOutputWatchdog } from '../authority/vehicle' |
| 9 | 9 | import { createVehicleMeasurementQualifier, predictVehicleMeasurementPose, vehicleReference } from './measurements' |
| 10 | type ScheduledMissionAction = { atMs: number, action: Exclude<VehicleMissionAction, 'start'> } | |
| 11 | function captureMissionActions(input: unknown, maximumMs: number): ScheduledMissionAction[] { | |
| 12 | function invalid(): never { throw new TypeError('Invalid vehicle mission action schedule') } | |
| 13 | if (input === undefined) return [] | |
| 14 | if (!Array.isArray(input) || input.length > 32 || Reflect.ownKeys(input).length !== input.length + 1) invalid() | |
| 15 | const result: ScheduledMissionAction[] = [] | |
| 16 | let previousMs = 600 | |
| 17 | for (let i = 0; i < input.length; i++) { | |
| 18 | const slot = Object.getOwnPropertyDescriptor(input, String(i)) | |
| 19 | if (!slot || !('value' in slot) || !slot.value || typeof slot.value !== 'object' || ![Object.prototype, null].includes(Object.getPrototypeOf(slot.value)) || Reflect.ownKeys(slot.value).length !== 2) invalid() | |
| 20 | const time = Object.getOwnPropertyDescriptor(slot.value, 'atMs'), action = Object.getOwnPropertyDescriptor(slot.value, 'action') | |
| 21 | if (!time || !('value' in time) || !Number.isSafeInteger(time.value) || time.value <= previousMs || time.value > maximumMs || time.value % 20 !== 0 || !action || !('value' in action) || !['pause', 'resume', 'recover', 'abort'].includes(action.value)) invalid() | |
| 22 | result.push({ atMs: time.value, action: action.value }) | |
| 23 | previousMs = time.value | |
| 24 | } | |
| 25 | return result | |
| 26 | } | |
| 10 | 27 | /** Owned raw sensor -> estimator -> mission -> vehicle policy -> authority -> independent board loop. */ |
| 11 | export function runVehicleSimulation(profileInput: VehicleProfile, options: { scenario?: string, seconds?: number, neutralTailMs?: number } = {}) { | |
| 28 | export function runVehicleSimulation(profileInput: VehicleProfile, options: { scenario?: string, seconds?: number, neutralTailMs?: number, missionActions?: readonly ScheduledMissionAction[] } = {}) { | |
| 12 | 29 | const scenario = options.scenario ?? profileInput.scenario, seconds = options.seconds ?? 120 |
| 13 | 30 | const neutralTailMs = options.neutralTailMs ?? 0 |
| 31 | let missionActionIndex = 0 | |
| 14 | 32 | if (!Number.isSafeInteger(neutralTailMs) || neutralTailMs < 0 || neutralTailMs > 15000 || neutralTailMs % 4 !== 0) throw new RangeError('Invalid independent vehicle neutral tail') |
| 15 | 33 | 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') |
| 34 | const missionActions = captureMissionActions(options.missionActions, seconds * 1000) | |
| 16 | 35 | const { plan, worstCaseMs, requiredWh } = validateVehicleMission(profileInput.mission) |
| 17 | 36 | 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 | 37 | const hasTransitions = plan.tasks.some(task => task.kind === 'vtol-mode') |
| @@ -93,7 +112,10 @@ export function runVehicleSimulation(profileInput: VehicleProfile, options: { sc | ||
| 93 | 112 | const ack = payload?.poll(nowMs) ?? null |
| 94 | 113 | 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 | 114 | transitionObservation = null |
| 96 | mission = engine.step(missionObservation, nowMs === 600 ? 'start' : undefined) | |
| 115 | let action: VehicleMissionAction | undefined = nowMs === 600 ? 'start' : undefined | |
| 116 | const scheduled = missionActions[missionActionIndex] | |
| 117 | if (scheduled?.atMs === nowMs) { action = scheduled.action; missionActionIndex++ } | |
| 118 | mission = engine.step(missionObservation, action) | |
| 97 | 119 | if (mission.payload && !requested.has(mission.payload.eventId)) { |
| 98 | 120 | if (!payload) { fault ??= 'missing-vehicle-payload-port'; send('disarm') } |
| 99 | 121 | else { payload.observe?.(structuredClone({ ...context, mission })); payload.request(structuredClone(mission.payload)); requested.add(mission.payload.eventId) } |
| @@ -0,0 +1,172 @@ | ||
| 1 | import type { VehicleController, VehicleControllerContext, VehicleControlOutput, VehicleVtolTransitionCommand, VehicleVtolTransitionObservation } from '../types' | |
| 2 | import type { SensorVector } from '../../sensors/types' | |
| 3 | import type { Quaternion } from '../../estimation/frames' | |
| 4 | import { createVehicleMeasurementQualifier, vehicleReference } from '../measurements' | |
| 5 | import { quaternionEuler, quaternionFromTilt, multiplyQuaternion, normalizeQuaternion } from '../../estimation/frames' | |
| 6 | import { rotateBodyToNed, rotateNedToBody } from '../../controls/physics' | |
| 7 | import { createBoundedPid } from '../../controls/pid' | |
| 8 | import { airAerodynamics, airModel } from '../air/plant' | |
| 9 | import { createVtolVehicleHoverController } from './controller' | |
| 10 | import { numericPacket } from './packets' | |
| 11 | import { allocateVtolLift } from './allocation' | |
| 12 | import { vtolModel } from './plant' | |
| 13 | ||
| 14 | const clamp = (n: number, low: number, high: number) => Math.max(low, Math.min(high, n)) | |
| 15 | const wrap = (n: number) => Math.atan2(Math.sin(n), Math.cos(n)) | |
| 16 | const pauseLoiterRadiusM = 60 | |
| 17 | ||
| 18 | /** Original fictional pressure/force-aware lift-and-cruise control, undergoing owned-model qualification. */ | |
| 19 | export function createVtolFlightController(): VehicleController { | |
| 20 | const qualifier = createVehicleMeasurementQualifier('vtol'), hover = createVtolVehicleHoverController() | |
| 21 | const ratePid = [0, 1, 2].map(i => createBoundedPid({ kp: i === 2 ? 0.96 : 1.6, ki: i === 2 ? 0.48 : 0.8, kd: 0.02, integralLimit: 1, outputLimit: i === 2 ? 2 : 6, derivativeTauS: 0.03, trackingGain: 4 })) | |
| 22 | const velocityPid = [0, 1, 2].map(() => createBoundedPid({ kp: 1.6, ki: 0.25, kd: 0, integralLimit: 1.5, outputLimit: 3, derivativeTauS: 0.03, trackingGain: 1 })) | |
| 23 | let hasWingControl = false, backSlow = false | |
| 24 | let loiterCenter: SensorVector | null = null | |
| 25 | let lastNow: number | null = null, identity: string | null = null, fault = '' | |
| 26 | let phase: VehicleVtolTransitionObservation['phase'] = 'hover', request: VehicleVtolTransitionCommand | null = null, signature = '', confirmedEvent = '' | |
| 27 | let stableSource: number | null = null, stableDecision: number | null = null | |
| 28 | function output(c: VehicleControllerContext, actuators: number[], reason: string, inhibited = false): VehicleControlOutput { return { kind: 'simulated-vehicle-control', vehicle: 'vtol', atMs: c.nowMs, actuators: [...actuators], reason, inhibited, physicalFlightEnabled: false, boardOutputsInhibited: true } } | |
| 29 | function neutral(c: VehicleControllerContext, reason: string) { fault ||= reason; return output(c, [0, 0, 0, 0, 0, 0, 0, 0], fault, true) } | |
| 30 | return { | |
| 31 | step(c) { | |
| 32 | if (!Number.isSafeInteger(c.nowMs) || c.nowMs < 0 || lastNow !== null && (c.nowMs <= lastNow || c.nowMs - lastNow > 40)) return neutral(c, 'vtol-flight-clock-gap') | |
| 33 | const dtS = lastNow === null ? 0.02 : (c.nowMs - lastNow) / 1000 | |
| 34 | lastNow = c.nowMs | |
| 35 | if (!c.enabled || fault) return output(c, [0, 0, 0, 0, 0, 0, 0, 0], fault || 'authority-disabled', true) | |
| 36 | let measurement, position: SensorVector, velocity: SensorVector, q: Quaternion, captureQ: Quaternion, rates: SensorVector, target: SensorVector | |
| 37 | try { | |
| 38 | measurement = qualifier.qualify(c.measurement, c.nowMs, vehicleReference(c.estimate)) | |
| 39 | const e = c.estimate, p = e.localPosition, a = e.attitude, rate = c.controlRate | |
| 40 | if (!measurement || c.mission.vehicle !== 'vtol' || e.evidence !== 'fixture' || e.physicalFlightEnabled !== false || typeof e.bootId !== 'bigint' || e.bootId <= 0n || !Object.values(e.resets).every(n => Number.isSafeInteger(n) && n >= 0) || e.nowMs !== c.nowMs || !e.health.tilt || !e.health.localPosition || !p || !a || !rate || rate.evidence !== 'modeled-qualified-imu' || rate.reference !== measurement.reference || a.reference !== 'GRAVITY_ALIGNED_RELATIVE_YAW' || p.frame !== 'LOCAL_NED' || p.altitudeDatum !== 'WGS84_ELLIPSOID' || p.predictedToMs !== c.nowMs || p.capturedAtMs === null || a.capturedAtMs === null || !Number.isSafeInteger(p.capturedAtMs) || !Number.isSafeInteger(a.capturedAtMs) || p.capturedAtMs > c.nowMs || c.nowMs - p.capturedAtMs > 200 || a.capturedAtMs > c.nowMs || c.nowMs - a.capturedAtMs > 60 || rate.capturedAtMs !== a.capturedAtMs) throw new TypeError('Unqualified navigation') | |
| 41 | position = numericPacket(p.positionM, 3) as SensorVector; velocity = numericPacket(p.velocityMps, 3) as SensorVector | |
| 42 | q = numericPacket(a.quaternionWxyz, 4) as Quaternion; rates = numericPacket(rate.angularRateRadS, 3) as SensorVector | |
| 43 | target = numericPacket(c.mission.targetM, 3) as SensorVector | |
| 44 | const pose = c.measurementPose | |
| 45 | if (!pose || pose.reference !== measurement.reference || pose.capturedAtMs !== measurement.capturedAtMs || pose.attitudePrediction !== 'qualified-gyro-hold' || pose.attitudePredictedToMs !== pose.capturedAtMs || pose.positionPredictedToMs !== pose.capturedAtMs || ![pose.attitudeCapturedAtMs, pose.gyroCapturedAtMs, pose.positionCapturedAtMs].every(n => Number.isSafeInteger(n) && n >= 0) || pose.attitudeCapturedAtMs !== pose.gyroCapturedAtMs || pose.gyroCapturedAtMs > pose.capturedAtMs || pose.capturedAtMs - pose.gyroCapturedAtMs > 60 || pose.positionCapturedAtMs > pose.capturedAtMs || pose.capturedAtMs - pose.positionCapturedAtMs > 200) throw new TypeError('Unmatched air capture pose') | |
| 46 | captureQ = numericPacket(pose.quaternionWxyz, 4) as Quaternion | |
| 47 | if (Math.abs(Math.hypot(...captureQ) - 1) > 1e-6) throw new TypeError('Invalid air capture attitude') | |
| 48 | if (Math.abs(Math.hypot(...q) - 1) > 1e-6 || Math.hypot(...rates) > 35 || Math.hypot(...target) > 400 || Math.hypot(...position) > 500 || !p.origin || ![p.origin.latitudeDeg, p.origin.longitudeDeg, p.origin.ellipsoidHeightM].every(Number.isFinite)) throw new TypeError('Invalid navigation values') | |
| 49 | const nextIdentity = JSON.stringify([String(e.bootId), e.resets, p.origin]) | |
| 50 | if (identity !== null && identity !== nextIdentity) throw new TypeError('Changed navigation reference') | |
| 51 | identity = nextIdentity | |
| 52 | const heldS = (c.nowMs - rate.capturedAtMs) / 1000, magnitude = Math.hypot(...rates), angle = magnitude * heldS | |
| 53 | const scale = magnitude < 1e-10 ? heldS / 2 : Math.sin(angle / 2) / magnitude | |
| 54 | q = normalizeQuaternion(multiplyQuaternion(q, [Math.cos(angle / 2), rates[0] * scale, rates[1] * scale, rates[2] * scale])) | |
| 55 | const v = measurement.values | |
| 56 | for (const key of ['airspeedMps', 'differentialPressurePa', 'alphaRad', 'betaRad', 'rotorThrustN', 'pusherThrustN']) if (typeof v[key] !== 'number' || !Number.isFinite(v[key])) throw new TypeError('Missing VTOL air measurement') | |
| 57 | if ((v.airspeedMps as number) < 0 || (v.airspeedMps as number) > 60 || (v.differentialPressurePa as number) < 0 || Math.abs(Math.sqrt(2 * (v.differentialPressurePa as number) / airModel.densityKgM3) - (v.airspeedMps as number)) > 0.02 || Math.abs(v.alphaRad as number) > Math.PI || Math.abs(v.betaRad as number) > Math.PI || (v.rotorThrustN as number) < 0 || (v.rotorThrustN as number) > 48 || (v.pusherThrustN as number) < 0 || (v.pusherThrustN as number) > 14) throw new TypeError('Invalid VTOL air measurement') | |
| 58 | } | |
| 59 | catch { return neutral(c, 'vtol-flight-observation-unqualified') } | |
| 60 | const incoming = c.mission.transition | |
| 61 | if (incoming) { | |
| 62 | let anchor: number[] | |
| 63 | try { anchor = numericPacket(incoming.positionM, 3) } | |
| 64 | catch { return neutral(c, 'vtol-flight-request-unqualified') } | |
| 65 | if (c.mission.taskKind !== 'vtol-mode' || incoming.taskId !== c.mission.taskId || typeof incoming.eventId !== 'string' || !incoming.eventId || incoming.eventId.length > 256 || incoming.reference !== measurement!.reference || !['hover', 'cruise'].includes(incoming.mode) || !Number.isFinite(incoming.headingRad) || Math.abs(incoming.headingRad) > Math.PI || anchor.some((n, i) => n !== target[i]) || !Number.isSafeInteger(incoming.issuedAtMs) || incoming.issuedAtMs < 0 || !Number.isSafeInteger(incoming.expiresAtMs) || incoming.issuedAtMs > c.nowMs || incoming.expiresAtMs <= c.nowMs || incoming.expiresAtMs <= incoming.issuedAtMs || incoming.expiresAtMs - incoming.issuedAtMs > (incoming.mode === 'hover' ? 45000 : 15000)) return neutral(c, 'vtol-flight-request-unqualified') | |
| 66 | const encoded = JSON.stringify(incoming) | |
| 67 | if (request?.eventId === incoming.eventId && signature !== encoded) return neutral(c, 'vtol-flight-request-changed') | |
| 68 | if (request?.eventId !== incoming.eventId && incoming.mode === 'cruise') { | |
| 69 | if (phase !== 'hover' || measurement!.values.onSurface) return neutral(c, 'vtol-front-requires-airborne-hover') | |
| 70 | request = structuredClone(incoming); signature = encoded; phase = 'front-transition'; hasWingControl = true; stableSource = null; stableDecision = null | |
| 71 | } | |
| 72 | if (request?.eventId !== incoming.eventId && incoming.mode === 'hover' && hasWingControl) { | |
| 73 | if (phase !== 'cruise') return neutral(c, 'vtol-back-requires-observed-cruise') | |
| 74 | request = structuredClone(incoming); signature = encoded; phase = 'back-transition'; backSlow = false; stableSource = null; stableDecision = null | |
| 75 | for (const pid of velocityPid) pid.reset() | |
| 76 | } | |
| 77 | } | |
| 78 | if (phase === 'hover' && !hasWingControl) return hover.step(c) | |
| 79 | const [roll, pitch, yaw] = quaternionEuler(q!), values = measurement!.values | |
| 80 | const speed = values.airspeedMps as number, alpha = values.alphaRad as number, beta = values.betaRad as number | |
| 81 | const landing = phase === 'hover' && c.mission.state === 'running' && c.mission.taskKind === 'land' | |
| 82 | const pausedContact = phase === 'hover' && c.mission.state === 'paused' && values.onSurface === true && values.stopped === true | |
| 83 | if (measurement!.values.onSurface && !landing && !pausedContact || speed > 22 || Math.abs(roll) > 0.6 || Math.abs(pitch) > 0.5 || speed > 8 && (Math.abs(alpha) > 0.32 || Math.abs(beta) > 0.4) || phase !== 'hover' && Math.abs(position![2] - request!.positionM[2]) > 2) return neutral(c, 'vtol-transition-envelope') | |
| 84 | if (landing && measurement!.values.onSurface || pausedContact) return output(c, [0, 0, 0, 0, 0, 0, 0, 0], pausedContact ? 'ground-paused' : 'vertical-contact') | |
| 85 | const rotary = phase === 'back-transition' || phase === 'hover' | |
| 86 | let heading = phase === 'front-transition' || rotary ? request!.headingRad : Math.atan2(target![1] - position![1], target![0] - position![0]) | |
| 87 | const groundSpeed = Math.hypot(velocity![0], velocity![1]) | |
| 88 | const course = groundSpeed > 3 ? Math.atan2(velocity![1], velocity![0]) : yaw | |
| 89 | const pausedCruise = phase === 'cruise' && c.mission.state === 'paused' | |
| 90 | if (!pausedCruise) loiterCenter = null | |
| 91 | else { | |
| 92 | loiterCenter ??= [position![0] - pauseLoiterRadiusM * Math.sin(course), position![1] + pauseLoiterRadiusM * Math.cos(course), target![2]] | |
| 93 | if (Math.hypot(loiterCenter[0], loiterCenter[1]) + pauseLoiterRadiusM > 400) return neutral(c, 'vtol-pause-loiter-outside-envelope') | |
| 94 | const radial = Math.atan2(position![1] - loiterCenter[1], position![0] - loiterCenter[0]) | |
| 95 | const distance = Math.hypot(position![0] - loiterCenter[0], position![1] - loiterCenter[1]) | |
| 96 | heading = radial + Math.PI / 2 + Math.atan2(distance - pauseLoiterRadiusM, 40) | |
| 97 | } | |
| 98 | const loiterBank = pausedCruise ? Math.atan2(groundSpeed ** 2, vtolModel.gravityMps2 * pauseLoiterRadiusM) : 0 | |
| 99 | let bankTarget = clamp(loiterBank + wrap(heading - course) * 0.8, -0.3, 0.3) | |
| 100 | let downAcceleration = clamp((target![2] - position![2]) * 1.2 - velocity![2] * 1.5, -3, 3) | |
| 101 | const capturedRelative: SensorVector = [speed * Math.cos(alpha) * Math.cos(beta), speed * Math.sin(beta), speed * Math.sin(alpha) * Math.cos(beta)] | |
| 102 | // Delayed BODY_FRD airflow belongs to its qualified capture pose. Hold that measured NED | |
| 103 | // airflow vector to this decision, then express it in the current gyro-predicted attitude. | |
| 104 | const airNed = rotateBodyToNed(captureQ!, capturedRelative) | |
| 105 | const relative = rotateNedToBody(q!, airNed) | |
| 106 | const gamma = Math.atan2(-airNed[2], Math.hypot(airNed[0], airNed[1])) | |
| 107 | const pressureArea = (values.differentialPressurePa as number) * airModel.areaM2 | |
| 108 | let requiredUpN = vtolModel.massKg * (vtolModel.gravityMps2 - downAcceleration) | |
| 109 | const alphaTarget = clamp((requiredUpN / Math.max(1, pressureArea * Math.cos(bankTarget)) - 0.25) / 4.8, -0.08, 0.18) | |
| 110 | let pitchTarget = clamp(gamma + alphaTarget, -0.2, 0.25) * clamp((speed - 10) / 2, 0, 1) | |
| 111 | const zero = airAerodynamics(relative, rates!, [0, 0, 0, 0, 0]) | |
| 112 | if (rotary) { | |
| 113 | backSlow ||= speed < 3 | |
| 114 | if (!backSlow && phase === 'back-transition') { | |
| 115 | // With the pusher off, high-speed braking is bounded by wing drag and the lift surplus. | |
| 116 | // The linear pre-stall lift slope gives the pitch that maximizes supported rotor braking. | |
| 117 | const optimalAlpha = Math.max(0, (requiredUpN / Math.max(1, pressureArea) - 0.25) / (2 * 4.8)) | |
| 118 | pitchTarget = clamp(gamma + optimalAlpha, -0.08, 0.3) | |
| 119 | bankTarget = 0 | |
| 120 | } | |
| 121 | else { | |
| 122 | const goal = [...target!] as SensorVector | |
| 123 | if (landing) goal[2] += 0.1 | |
| 124 | const desiredVelocity = goal.map((n, i) => n - position![i]!) as SensorVector | |
| 125 | const horizontal = Math.hypot(desiredVelocity[0], desiredVelocity[1]) | |
| 126 | if (horizontal > 3) { desiredVelocity[0] *= 3 / horizontal; desiredVelocity[1] *= 3 / horizontal } | |
| 127 | desiredVelocity[2] = clamp(desiredVelocity[2], -2, 2) | |
| 128 | const acceleration = desiredVelocity.map((n, i) => velocityPid[i]!.step(n - velocity![i]!, velocity![i]!, dtS)) as SensorVector | |
| 129 | const aHorizontal = Math.hypot(acceleration[0], acceleration[1]) | |
| 130 | if (aHorizontal > 3) { acceleration[0] *= 3 / aHorizontal; acceleration[1] *= 3 / aHorizontal } | |
| 131 | downAcceleration = acceleration[2]; requiredUpN = vtolModel.massKg * (vtolModel.gravityMps2 - downAcceleration) | |
| 132 | const zeroNed = rotateBodyToNed(q!, zero.forceN) | |
| 133 | const fx = vtolModel.massKg * acceleration[0] - zeroNed[0], fy = vtolModel.massKg * acceleration[1] - zeroNed[1] | |
| 134 | const forward = Math.cos(heading) * fx + Math.sin(heading) * fy, right = -Math.sin(heading) * fx + Math.cos(heading) * fy | |
| 135 | const up = Math.max(1, requiredUpN + zeroNed[2]) | |
| 136 | pitchTarget = clamp(Math.atan2(-forward, up), -0.4, 0.4) | |
| 137 | bankTarget = clamp(Math.atan2(right, Math.hypot(forward, up)), -0.4, 0.4) | |
| 138 | } | |
| 139 | } | |
| 140 | // Wing navigation banks toward the route while the nose follows the measured air course. | |
| 141 | // An instantaneous waypoint bearing is not a physically coordinated yaw command. | |
| 142 | const yawTarget = phase === 'cruise' ? Math.atan2(airNed[1], airNed[0]) : heading | |
| 143 | const desired = multiplyQuaternion([Math.cos(yawTarget / 2), 0, 0, Math.sin(yawTarget / 2)], quaternionFromTilt(bankTarget, pitchTarget)) | |
| 144 | const error = multiplyQuaternion([q![0], -q![1], -q![2], -q![3]], desired) | |
| 145 | const turnRate = phase === 'cruise' ? vtolModel.gravityMps2 * Math.tan(bankTarget) / Math.max(8, speed) : 0 | |
| 146 | const turnRates = rotateNedToBody(q!, [0, 0, turnRate]) | |
| 147 | const desiredRates = error.slice(1).map((n, i) => clamp(8 * n * (error[0] < 0 ? -1 : 1) + turnRates[i]!, i === 2 ? -1.5 : -3, i === 2 ? 1.5 : 3)) | |
| 148 | const torque = desiredRates.map((n, i) => ratePid[i]!.step(n - rates![i]!, rates![i]!, dtS)) as SensorVector | |
| 149 | const pusher = rotary ? 0 : clamp((-zero.forceN[0] + vtolModel.massKg * clamp((13 - speed) * 1.2, -2.5, 2.5)) / vtolModel.maxPusherThrustN, 0, 1) | |
| 150 | const coefficients = [pressureArea * airModel.spanM * 0.12, pressureArea * airModel.chordM * 0.8, pressureArea * airModel.spanM * 0.08] | |
| 151 | const surfaces = torque.map((n, i) => coefficients[i]! > 1e-6 ? clamp((n - zero.torqueNm[i]!) / coefficients[i]!, -0.4, 0.4) : 0) | |
| 152 | const aero = airAerodynamics(relative, rates!, [pusher, ...surfaces, 0]) | |
| 153 | const aeroNed = rotateBodyToNed(q!, aero.forceN) | |
| 154 | const collective = clamp((requiredUpN + aeroNed[2]) / Math.max(0.7, Math.cos(roll) * Math.cos(pitch)), 0, 48) | |
| 155 | const allocation = allocateVtolLift({ thrustN: collective, torqueNm: torque.map((n, i) => n - aero.torqueNm[i]!) as SensorVector }) | |
| 156 | for (let i = 0; i < 3; i++) ratePid[i]!.track(allocation.achieved.torqueNm[i]! + aero.torqueNm[i]! - torque[i]!, dtS) | |
| 157 | const response = output(c, [...allocation.rotorsN, pusher, ...surfaces], phase === 'front-transition' ? 'force-managed-front-transition' : phase === 'cruise' ? 'wing-borne-cruise' : phase === 'back-transition' ? 'force-managed-back-transition' : 'force-managed-hover') | |
| 158 | const source = Math.min(measurement!.capturedAtMs, c.measurementPose!.attitudeCapturedAtMs, c.measurementPose!.gyroCapturedAtMs, c.estimate.attitude!.capturedAtMs!, c.estimate.localPosition!.capturedAtMs!, c.controlRate!.capturedAtMs) | |
| 159 | if (incoming && incoming.eventId !== confirmedEvent && source >= incoming.issuedAtMs) { | |
| 160 | const stable = incoming.mode === 'cruise' | |
| 161 | ? speed >= 12 && speed <= 14 && Math.abs(alpha) < 0.15 && Math.abs(beta) < 0.15 && Math.abs(roll) < 0.25 && Math.abs(pitch) < 0.25 && Math.abs(velocity![2]) < 0.4 && Math.abs(position![2] - incoming.positionM[2]) < 1 && (values.rotorThrustN as number) < 1 && collective < 1 | |
| 162 | : Math.hypot(...velocity!) <= 0.2 && Math.hypot(...rates!) < 0.1 && Math.abs(wrap(incoming.headingRad - yaw)) <= 0.1 && Math.abs(roll) < 0.1 && Math.abs(pitch) < 0.1 && Math.hypot(...position!.map((n, i) => n - incoming.positionM[i]!)) < 0.3 && (values.rotorThrustN as number) > 20 && (values.rotorThrustN as number) < 30 | |
| 163 | if (!stable) { stableSource = null; stableDecision = null } | |
| 164 | else { stableSource ??= source; stableDecision ??= c.nowMs } | |
| 165 | const confirmed = stable && source - stableSource! >= 500 && c.nowMs - stableDecision! >= 500 | |
| 166 | if (confirmed) { phase = incoming.mode; confirmedEvent = incoming.eventId } | |
| 167 | response.transition = { evidence: 'modeled-vtol-controller-observation', eventId: incoming.eventId, taskId: incoming.taskId, mode: incoming.mode, reference: incoming.reference, capturedAtMs: c.nowMs, measurementCapturedAtMs: source, phase, outcome: confirmed ? 'confirmed' : 'progress', reason: confirmed ? incoming.mode === 'cruise' ? 'wing-support-qualified' : 'hover-support-qualified' : incoming.mode === 'cruise' ? 'wing-support-observing' : 'hover-support-observing' } | |
| 168 | } | |
| 169 | return response | |
| 170 | }, | |
| 171 | } | |
| 172 | } | |
| @@ -0,0 +1,38 @@ | ||
| 1 | import type { VehicleProfile } from '../types' | |
| 2 | import { createVtolHoverProfile } from './index' | |
| 3 | import { createVtolPlant } from './plant' | |
| 4 | import { createVtolFlightController } from './flight-controller' | |
| 5 | ||
| 6 | /** Declared fictional transition experiment, undergoing controller qualification. No physical IO. */ | |
| 7 | export function createVtolTransitionProfile(): VehicleProfile { | |
| 8 | const base = createVtolHoverProfile() | |
| 9 | return { | |
| 10 | ...base, scenario: 'nominal', scenarios: ['nominal'], name: 'Owned VTOL lift-and-cruise development', | |
| 11 | authorityFence: { radiusM: 400, minDownM: -40, maxDownM: 1 }, | |
| 12 | mission: { ...base.mission, id: 'vtol-transition-development', fence: { radiusM: 400, minDownM: -40, maxDownM: 1 }, rallyM: [0, 0, -15], | |
| 13 | power: { availableWh: 100, maxW: 1200, reservePercent: 30 }, | |
| 14 | tasks: [ | |
| 15 | { id: 'start', kind: 'start', next: 'initial-hover' }, | |
| 16 | { id: 'initial-hover', kind: 'vtol-mode', mode: 'hover', headingRad: 0, timeoutMs: 1500, next: 'depart' }, | |
| 17 | { id: 'depart', kind: 'takeoff', positionM: [0, 0, -15], acceptanceM: 0.25, maxArrivalSpeedMps: 0.2, settleMs: 200, timeoutMs: 20000, next: 'front' }, | |
| 18 | { id: 'front', kind: 'vtol-mode', mode: 'cruise', headingRad: 0, timeoutMs: 15000, next: 'cruise-leg' }, | |
| 19 | { id: 'cruise-leg', kind: 'waypoint', positionM: [180, 0, -15], acceptanceM: 8, maxArrivalSpeedMps: 15, settleMs: 0, timeoutMs: 25000, next: 'back' }, | |
| 20 | { id: 'back', kind: 'vtol-mode', mode: 'hover', headingRad: 0, timeoutMs: 45000, next: 'land' }, | |
| 21 | { id: 'land', kind: 'land', positionM: [180, 0, 0], acceptanceM: 0.3, maxArrivalSpeedMps: 0.2, settleMs: 200, timeoutMs: 25000 }, | |
| 22 | ], | |
| 23 | }, | |
| 24 | createSession() { | |
| 25 | return { createPlant: createVtolPlant, createController: createVtolFlightController, | |
| 26 | renderMeasurement(state, atMs, reference) { | |
| 27 | const rounded = (n: number) => Math.round(n * 1000000) / 1000000 | |
| 28 | return { version: 1, evidence: 'modeled-vehicle-sensors', vehicle: 'vtol', sequence: Math.floor(atMs / 20) + 1, reference, capturedAtMs: atMs, receivedAtMs: atMs + 60, | |
| 29 | values: { onSurface: state.onSurface, stopped: Math.hypot(...state.velocityMps) <= 0.2, | |
| 30 | airspeedMps: rounded(state.details.airspeedMps as number), differentialPressurePa: rounded(state.details.dynamicPressurePa as number), | |
| 31 | alphaRad: rounded(state.details.alphaRad as number), betaRad: rounded(state.details.betaRad as number), | |
| 32 | rotorThrustN: rounded(state.details.rotorThrustN as number), pusherThrustN: rounded(state.details.pusherThrustN as number) } } | |
| 33 | }, | |
| 34 | } | |
| 35 | }, | |
| 36 | faults() { return {} }, | |
| 37 | } | |
| 38 | } | |
| @@ -77,6 +77,7 @@ | ||
| 77 | 77 | "./unit/vtol-runtime.test.ts", |
| 78 | 78 | "./unit/vtol-cli.test.ts", |
| 79 | 79 | "./unit/vtol-mode-mission.test.ts", |
| 80 | "./unit/vtol-transition.test.ts", | |
| 80 | 81 | "./unit/ground.test.ts", |
| 81 | 82 | "./unit/surface.test.ts", |
| 82 | 83 | "./unit/mission-workspace.test.ts", |
| @@ -114,6 +115,7 @@ | ||
| 114 | 115 | "./unit/gimbal-authority.test.ts", |
| 115 | 116 | "./unit/cinema-managed.test.ts", |
| 116 | 117 | "./unit/cinema-counted-encoder.test.ts", |
| 117 | "./unit/cinema-counted-translation.test.ts" | |
| 118 | "./unit/cinema-counted-translation.test.ts", | |
| 119 | "./unit/vehicle-lifecycle.test.ts" | |
| 118 | 120 | ] |
| 119 | 121 | } |
| @@ -0,0 +1,277 @@ | ||
| 1 | import { expect, test } from 'bun:test' | |
| 2 | import { createVtolHoverProfile } from '../../resources/functions/drone/vehicles/vtol' | |
| 3 | import { runVehicleSimulation } from '../../resources/functions/drone/vehicles/runtime' | |
| 4 | import { replayVehicleMission } from '../../resources/functions/drone/missions/vehicle' | |
| 5 | import { createVtolTransitionProfile } from '../../resources/functions/drone/vehicles/vtol/transition-profile' | |
| 6 | import type { VehiclePlantState } from '../../resources/functions/drone/vehicles/types' | |
| 7 | ||
| 8 | // Agreed shared-runtime seam: actual sensor/estimator/mission/controller/independent lease | |
| 9 | // execution. This verifies operator lifecycle delivery, not physical pause or recovery. | |
| 10 | test('scheduled operator pause and resume reach the real hover loop and its replay without withdrawing authority', () => { | |
| 11 | const report = runVehicleSimulation(createVtolHoverProfile(), { seconds: 60, missionActions: [ | |
| 12 | { atMs: 4000, action: 'pause' }, | |
| 13 | { atMs: 6000, action: 'resume' }, | |
| 14 | ] }) | |
| 15 | const paused = report.path.filter(row => row.mission.state === 'paused') | |
| 16 | expect(paused).toHaveLength(100) | |
| 17 | expect(paused[0]!.atMs).toBe(4000) | |
| 18 | expect(paused.at(-1)!.atMs).toBe(5980) | |
| 19 | expect(paused.every(row => row.mission.reason === 'operator-pause' && row.mission.targetM?.every((n, i) => n === paused[0]!.mission.targetM![i]))).toBe(true) | |
| 20 | expect(paused.every(row => row.actuators.slice(0, 4).some(n => n > 0))).toBe(true) | |
| 21 | expect(report.trace.rows.filter(row => row.action === 'pause' || row.action === 'resume').map(row => [row.observation.nowMs, row.action])).toEqual([[4000, 'pause'], [6000, 'resume']]) | |
| 22 | expect(report.status).toBe('completed') | |
| 23 | expect(report.mission.reason).toBe('landed') | |
| 24 | expect(report.finalTruthPositionM[2]).toBe(0) | |
| 25 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 26 | expect(report.board.expiresAtMs).toBeNull() | |
| 27 | expect(report.physicalFlightEnabled).toBe(false) | |
| 28 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) | |
| 29 | }) | |
| 30 | ||
| 31 | ||
| 32 | test('invalid operator schedules are refused before creating a session or invoking data accessors', () => { | |
| 33 | let accessorCalls = 0 | |
| 34 | const getterEvent = Object.defineProperty({ action: 'pause' }, 'atMs', { enumerable: true, get() { accessorCalls++; return 1000 } }) | |
| 35 | const cases: [string, unknown][] = [ | |
| 36 | ['off-grid', [{ atMs: 1001, action: 'pause' }]], | |
| 37 | ['null', null], | |
| 38 | ['not-an-array', {}], | |
| 39 | ['sparse', new Array(1)], | |
| 40 | ['too-many', Array.from({ length: 33 }, () => ({ atMs: 1000, action: 'pause' }))], | |
| 41 | ['automatic-start-collision', [{ atMs: 600, action: 'pause' }]], | |
| 42 | ['before-start', [{ atMs: 580, action: 'pause' }]], | |
| 43 | ['past-duration', [{ atMs: 2020, action: 'pause' }]], | |
| 44 | ['non-finite', [{ atMs: NaN, action: 'pause' }]], | |
| 45 | ['unsafe-integer', [{ atMs: Number.MAX_SAFE_INTEGER + 1, action: 'pause' }]], | |
| 46 | ['duplicate', [{ atMs: 1000, action: 'pause' }, { atMs: 1000, action: 'resume' }]], | |
| 47 | ['reversed', [{ atMs: 1020, action: 'pause' }, { atMs: 1000, action: 'resume' }]], | |
| 48 | ['second-start', [{ atMs: 1000, action: 'start' }]], | |
| 49 | ['unknown-action', [{ atMs: 1000, action: 'fly' }]], | |
| 50 | ['extra-field', [{ atMs: 1000, action: 'pause', retry: true }]], | |
| 51 | ['accessor', [getterEvent]], | |
| 52 | ] | |
| 53 | const violations: { label: string, created: number, message: string }[] = [] | |
| 54 | for (const [label, missionActions] of cases) { | |
| 55 | const profile = createVtolHoverProfile(), createSession = profile.createSession | |
| 56 | let created = 0, message = '' | |
| 57 | profile.createSession = () => { created++; return createSession() } | |
| 58 | try { runVehicleSimulation(profile, { seconds: 2, missionActions: missionActions as never }) } | |
| 59 | catch (error) { message = error instanceof Error ? error.message : String(error) } | |
| 60 | if (created !== 0 || message !== 'Invalid vehicle mission action schedule') violations.push({ label, created, message }) | |
| 61 | } | |
| 62 | expect(violations).toEqual([]) | |
| 63 | expect(accessorCalls).toBe(0) | |
| 64 | }) | |
| 65 | ||
| 66 | ||
| 67 | test('validated lifecycle events are independent of caller mutation during session creation', () => { | |
| 68 | const first = { atMs: 4000, action: 'pause' as const }, second = { atMs: 6000, action: 'resume' as const } | |
| 69 | const missionActions = [first, second], profile = createVtolHoverProfile(), createSession = profile.createSession | |
| 70 | profile.createSession = () => { | |
| 71 | first.atMs = 2000; second.atMs = 2020; missionActions.reverse() | |
| 72 | return createSession() | |
| 73 | } | |
| 74 | const report = runVehicleSimulation(profile, { seconds: 60, missionActions }) | |
| 75 | expect(report.trace.rows.filter(row => row.action === 'pause' || row.action === 'resume').map(row => [row.observation.nowMs, row.action])).toEqual([[4000, 'pause'], [6000, 'resume']]) | |
| 76 | expect(report.status).toBe('completed') | |
| 77 | expect(report.finalTruthPositionM[2]).toBe(0) | |
| 78 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) | |
| 79 | }) | |
| 80 | ||
| 81 | ||
| 82 | test('an empty lifecycle schedule preserves every legacy hover recording and report field', () => { | |
| 83 | const legacy = runVehicleSimulation(createVtolHoverProfile(), { seconds: 60 }) | |
| 84 | const empty = runVehicleSimulation(createVtolHoverProfile(), { seconds: 60, missionActions: [] }) | |
| 85 | expect(empty).toEqual(legacy) | |
| 86 | expect(Object.keys(empty)).toEqual(Object.keys(legacy)) | |
| 87 | expect(empty.trace.rows.every(row => row.action === null || row.action === 'start')).toBe(true) | |
| 88 | }) | |
| 89 | ||
| 90 | ||
| 91 | for (const bankedEntry of [false, true]) test(`a paused wing-borne VTOL flies a bounded clockwise 60 m loiter from ${bankedEntry ? 'banked' : 'straight'} entry before resuming and landing`, () => { | |
| 92 | const profile = createVtolTransitionProfile(), createSession = profile.createSession | |
| 93 | const destination = bankedEntry ? [150, 100] : [180, 0] | |
| 94 | if (bankedEntry) for (const task of profile.mission.tasks) { | |
| 95 | if ((task.id === 'cruise-leg' || task.id === 'land') && 'positionM' in task) { task.positionM[0] = destination[0]!; task.positionM[1] = destination[1]! } | |
| 96 | if (task.id === 'back' && task.kind === 'vtol-mode') task.headingRad = 1 | |
| 97 | } | |
| 98 | const observed: { atMs: number, state: VehiclePlantState }[] = [] | |
| 99 | profile.createSession = () => { | |
| 100 | const session = createSession() | |
| 101 | return { ...session, observeIndependent(state, atMs) { | |
| 102 | if (atMs % 20 === 0 && atMs >= 20000 && atMs < 40000) observed.push({ atMs, state: structuredClone(state) }) | |
| 103 | } } | |
| 104 | } | |
| 105 | const report = runVehicleSimulation(profile, { seconds: 160, missionActions: [{ atMs: 20000, action: 'pause' }, { atMs: 40000, action: 'resume' }] }) | |
| 106 | const entry = report.trace.rows.find(row => row.observation.nowMs === 20000)!.observation | |
| 107 | const course = Math.atan2(entry.velocityMps[1], entry.velocityMps[0]) | |
| 108 | const center = [entry.positionM[0] - 60 * Math.sin(course), entry.positionM[1] + 60 * Math.cos(course)] | |
| 109 | const settled = observed.filter(row => row.atMs >= 26000) | |
| 110 | const radialError = Math.max(...settled.map(row => Math.abs(Math.hypot(row.state.positionM[0] - center[0]!, row.state.positionM[1] - center[1]!) - 60))) | |
| 111 | const paused = report.path.filter(row => row.mission.state === 'paused') | |
| 112 | console.log('VTOL_PAUSE_LOITER', JSON.stringify({ bankedEntry, status: report.status, fault: report.fault, pausedRows: paused.length, settledRows: settled.length, maximumRadialErrorM: radialError })) | |
| 113 | expect(paused).toHaveLength(1000) | |
| 114 | expect(settled.length).toBeGreaterThan(600) | |
| 115 | expect(radialError).toBeLessThan(8) | |
| 116 | expect(settled.every(row => Math.abs(row.state.positionM[2] + 15) < 2)).toBe(true) | |
| 117 | expect(settled.every(row => (row.state.details.airspeedMps as number) >= 12 && (row.state.details.airspeedMps as number) <= 14)).toBe(true) | |
| 118 | expect(settled.every(row => (row.state.details.rotorThrustN as number) < 1)).toBe(true) | |
| 119 | expect(report.status).toBe('completed') | |
| 120 | expect(report.finalTruthPositionM[2]).toBe(0) | |
| 121 | expect(Math.hypot(report.finalTruthPositionM[0] - destination[0]!, report.finalTruthPositionM[1] - destination[1]!)).toBeLessThan(0.3) | |
| 122 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 123 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) | |
| 124 | }) | |
| 125 | ||
| 126 | ||
| 127 | test('a qualified post-back landing contact can pause with neutral outputs and explicitly resume', () => { | |
| 128 | const baseline = runVehicleSimulation(createVtolTransitionProfile(), { seconds: 160 }) | |
| 129 | expect(baseline.status).toBe('completed') | |
| 130 | const contact = baseline.path.find(row => row.mission.state === 'running' && row.mission.taskKind === 'land' && row.measurement?.values.onSurface === true) | |
| 131 | expect(contact).toBeDefined() | |
| 132 | const pauseAtMs = contact!.atMs | |
| 133 | const report = runVehicleSimulation(createVtolTransitionProfile(), { seconds: 160, missionActions: [{ atMs: pauseAtMs, action: 'pause' }, { atMs: pauseAtMs + 2000, action: 'resume' }] }) | |
| 134 | const paused = report.path.filter(row => row.mission.state === 'paused') | |
| 135 | console.log('VTOL_CONTACT_PAUSE', JSON.stringify({ pauseAtMs, status: report.status, fault: report.fault, pausedRows: paused.length })) | |
| 136 | expect(paused).toHaveLength(100) | |
| 137 | expect(paused.every(row => row.measurement!.values.onSurface === true && row.truthPositionM[2] === 0 && row.actuators.every(n => n === 0))).toBe(true) | |
| 138 | expect(report.status).toBe('completed') | |
| 139 | expect(report.fault).toBeNull() | |
| 140 | expect(report.mission.reason).toBe('landed') | |
| 141 | expect(report.finalTruthPositionM[2]).toBe(0) | |
| 142 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 143 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) | |
| 144 | }) | |
| 145 | ||
| 146 | ||
| 147 | test('neutral post-back ground pause cannot bypass lost IMU qualification or resume authority', () => { | |
| 148 | const baseline = runVehicleSimulation(createVtolTransitionProfile(), { seconds: 160 }) | |
| 149 | expect(baseline.status).toBe('completed') | |
| 150 | const pauseAtMs = baseline.path.find(row => row.mission.state === 'running' && row.mission.taskKind === 'land' && row.measurement?.values.onSurface === true)!.atMs | |
| 151 | const failedAtMs = pauseAtMs + 200, profile = createVtolTransitionProfile() | |
| 152 | profile.faults = (_scenario, nowMs) => nowMs >= failedAtMs ? { dropImu: true } : {} | |
| 153 | const report = runVehicleSimulation(profile, { seconds: 160, missionActions: [{ atMs: pauseAtMs, action: 'pause' }, { atMs: pauseAtMs + 2000, action: 'resume' }] }) | |
| 154 | console.log('VTOL_CONTACT_PAUSE_IMU', JSON.stringify({ failedAtMs, status: report.status, fault: report.fault, finalAtMs: report.path.at(-1)!.atMs })) | |
| 155 | expect(report.path.some(row => row.mission.state === 'paused' && row.measurement?.values.onSurface === true)).toBe(true) | |
| 156 | expect(report.status).toBe('blocked') | |
| 157 | expect(report.path.at(-1)!.atMs).toBeLessThanOrEqual(failedAtMs + 120) | |
| 158 | expect(report.mission.state).toBe('blocked') | |
| 159 | expect(report.trace.rows.some(row => row.action === 'resume')).toBe(false) | |
| 160 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 161 | expect(report.board.expiresAtMs).toBeNull() | |
| 162 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) | |
| 163 | }) | |
| 164 | ||
| 165 | ||
| 166 | for (const [taskId, atMs] of [['initial-hover', 700], ['front', 13000], ['back', 30000]] as const) { | |
| 167 | for (const action of ['pause', 'recover'] as const) { | |
| 168 | test(`an uncertain ${taskId} refuses ${action} instead of inventing transition recovery`, () => { | |
| 169 | const report = runVehicleSimulation(createVtolTransitionProfile(), { seconds: 60, missionActions: [{ atMs, action }] }) | |
| 170 | expect(report.path.filter(row => row.atMs < atMs).at(-1)!.mission.taskId).toBe(taskId) | |
| 171 | expect(report.status).toBe('blocked') | |
| 172 | expect(report.mission.reason).toBe('vtol-transition-recovery-uncertain') | |
| 173 | expect(report.mission.targetM).toBeNull() | |
| 174 | expect(report.mission.transition).toBeNull() | |
| 175 | expect(report.trace.rows.at(-1)!.observation.nowMs).toBe(atMs) | |
| 176 | expect(report.trace.rows.at(-1)!.action).toBe(action) | |
| 177 | expect(report.trace.rows.filter(row => row.action === 'start')).toHaveLength(1) | |
| 178 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 179 | expect(report.board.expiresAtMs).toBeNull() | |
| 180 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) | |
| 181 | }) | |
| 182 | } | |
| 183 | } | |
| 184 | ||
| 185 | ||
| 186 | test('same-session operator recovery remains paused until explicit resume with continuous authority', () => { | |
| 187 | const report = runVehicleSimulation(createVtolHoverProfile(), { seconds: 60, missionActions: [{ atMs: 4000, action: 'recover' }, { atMs: 6000, action: 'resume' }] }) | |
| 188 | const paused = report.path.filter(row => row.mission.state === 'paused') | |
| 189 | expect(paused).toHaveLength(100) | |
| 190 | expect(paused.every(row => row.mission.reason === 'checkpoint-recovered-awaiting-resume')).toBe(true) | |
| 191 | expect(report.trace.rows.filter(row => row.action === 'recover' || row.action === 'resume').map(row => [row.observation.nowMs, row.action])).toEqual([[4000, 'recover'], [6000, 'resume']]) | |
| 192 | expect(report.authority.events.filter(event => event.to === 'autonomous')).toHaveLength(1) | |
| 193 | expect(report.status).toBe('completed') | |
| 194 | expect(report.finalTruthPositionM[2]).toBe(0) | |
| 195 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) | |
| 196 | }) | |
| 197 | ||
| 198 | ||
| 199 | test('a paused hover still consumes the declared energy and global endurance allowance', () => { | |
| 200 | const report = runVehicleSimulation(createVtolHoverProfile(), { seconds: 80, missionActions: [{ atMs: 4000, action: 'pause' }] }) | |
| 201 | const paused = report.trace.rows.filter(row => row.result.state === 'paused') | |
| 202 | expect(paused.length).toBeGreaterThan(2000) | |
| 203 | expect(paused.at(-1)!.observation.batteryPercent).toBeLessThan(paused[0]!.observation.batteryPercent) | |
| 204 | expect(report.status).toBe('blocked') | |
| 205 | expect(report.mission.reason).toBe('mission-endurance-exhausted') | |
| 206 | expect(report.mission.elapsedMs).toBeGreaterThan(report.budget.worstCaseMs) | |
| 207 | expect(report.mission.elapsedMs).toBeLessThanOrEqual(report.budget.worstCaseMs + 20) | |
| 208 | expect(report.mission.taskId).toBe('depart') | |
| 209 | expect(report.path.some(row => row.mission.taskKind === 'land')).toBe(false) | |
| 210 | expect(report.finalTruthPositionM[2]).toBeLessThan(-1) | |
| 211 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 212 | expect(report.board.expiresAtMs).toBeNull() | |
| 213 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) | |
| 214 | }) | |
| 215 | ||
| 216 | ||
| 217 | for (const failure of ['dropImu', 'dropPosition', 'dropVehicleSensor', 'rcOverride', 'kill', 'hostLost'] as const) { | |
| 218 | test(`paused wing-borne VTOL cannot retain or regain output authority after ${failure}`, () => { | |
| 219 | const profile = createVtolTransitionProfile() | |
| 220 | profile.faults = (_scenario, nowMs) => nowMs >= 20500 ? { [failure]: true } : {} | |
| 221 | const report = runVehicleSimulation(profile, { seconds: 160, missionActions: [{ atMs: 20000, action: 'pause' }, { atMs: 22000, action: 'resume' }] }) | |
| 222 | expect(report.path.filter(row => row.atMs < 20500).at(-1)!.mission.state).toBe('paused') | |
| 223 | expect(report.status).toBe(failure === 'hostLost' ? 'host-lost' : 'blocked') | |
| 224 | expect(report.trace.rows.some(row => row.action === 'resume')).toBe(false) | |
| 225 | expect(report.authority.events.filter(event => event.to === 'autonomous')).toHaveLength(1) | |
| 226 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 227 | expect(report.board.expiresAtMs).toBeNull() | |
| 228 | expect(report.finalTruthPositionM[2]).toBeLessThan(-1) | |
| 229 | if (failure === 'hostLost') { | |
| 230 | expect(report.board.fault).toBe('output-lease-expired') | |
| 231 | expect(report.authority.lastCheckedAtMs).toBeLessThan(20500) | |
| 232 | } | |
| 233 | else { | |
| 234 | expect(report.mission.state).toBe('blocked') | |
| 235 | expect(report.mission.targetM).toBeNull() | |
| 236 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) | |
| 237 | } | |
| 238 | if (failure === 'rcOverride') expect(report.authority.events.some(event => event.to === 'manual')).toBe(true) | |
| 239 | if (failure === 'kill') expect(report.authority.events.some(event => event.to === 'killed')).toBe(true) | |
| 240 | }) | |
| 241 | } | |
| 242 | ||
| 243 | ||
| 244 | for (const action of ['pause', 'recover'] as const) { | |
| 245 | test(`post-back airborne ${action} holds estimated position until explicit resume and vertical landing`, () => { | |
| 246 | const report = runVehicleSimulation(createVtolTransitionProfile(), { seconds: 160, missionActions: [{ atMs: 68000, action }, { atMs: 70000, action: 'resume' }] }) | |
| 247 | const paused = report.path.filter(row => row.mission.state === 'paused') | |
| 248 | expect(paused).toHaveLength(100) | |
| 249 | const held = paused[0]!.mission.targetM! | |
| 250 | expect(Math.max(...paused.map(row => Math.hypot(...row.truthPositionM.map((n, i) => n - held[i]!))))).toBeLessThan(1.5) | |
| 251 | expect(paused.every(row => row.mission.taskKind === 'land' && row.measurement?.values.onSurface === false && row.actuators.slice(0, 4).some(n => n > 0))).toBe(true) | |
| 252 | expect(report.trace.rows.filter(row => row.observation.transition?.outcome === 'confirmed').map(row => row.observation.transition!.phase)).toEqual(['hover', 'cruise', 'hover']) | |
| 253 | expect(report.authority.events.filter(event => event.to === 'autonomous')).toHaveLength(1) | |
| 254 | expect(report.status).toBe('completed') | |
| 255 | expect(report.mission.reason).toBe('landed') | |
| 256 | expect(report.finalTruthPositionM[2]).toBe(0) | |
| 257 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 258 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) | |
| 259 | }) | |
| 260 | } | |
| 261 | ||
| 262 | ||
| 263 | test('a VTOL pause refuses a holding circle outside the declared 400 m envelope', () => { | |
| 264 | const profile = createVtolTransitionProfile() | |
| 265 | for (const task of profile.mission.tasks) if ((task.id === 'cruise-leg' || task.id === 'land') && 'positionM' in task) task.positionM[0] = 350 | |
| 266 | const report = runVehicleSimulation(profile, { seconds: 160, missionActions: [{ atMs: 40000, action: 'pause' }] }) | |
| 267 | console.log('VTOL_PAUSE_FENCE', JSON.stringify({ status: report.status, fault: report.fault, lastAtMs: report.path.at(-1)!.atMs, finalPositionM: report.finalTruthPositionM })) | |
| 268 | expect(report.path.filter(row => row.atMs < 40000).at(-1)!.mission.taskId).toBe('cruise-leg') | |
| 269 | expect(report.path.some(row => row.atMs === 40000 && row.mission.state === 'paused')).toBe(true) | |
| 270 | expect(report.status).toBe('blocked') | |
| 271 | expect(report.fault).toBe('vtol-pause-loiter-outside-envelope') | |
| 272 | expect(report.path.every(row => Math.hypot(row.truthPositionM[0], row.truthPositionM[1]) <= 400)).toBe(true) | |
| 273 | expect(report.finalTruthPositionM[2]).toBeLessThan(-1) | |
| 274 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 275 | expect(report.board.expiresAtMs).toBeNull() | |
| 276 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) | |
| 277 | }) | |
| @@ -0,0 +1,335 @@ | ||
| 1 | import { expect, test } from 'bun:test' | |
| 2 | import { createVtolTransitionProfile } from '../../resources/functions/drone/vehicles/vtol/transition-profile' | |
| 3 | import { runVehicleSimulation } from '../../resources/functions/drone/vehicles/runtime' | |
| 4 | import { createVtolFlightController } from '../../resources/functions/drone/vehicles/vtol/flight-controller' | |
| 5 | import { multiplyQuaternion, quaternionEuler } from '../../resources/functions/drone/estimation/frames' | |
| 6 | import { replayVehicleMission, validateVehicleMission } from '../../resources/functions/drone/missions/vehicle' | |
| 7 | import type { VehicleControllerContext, VehiclePlantState } from '../../resources/functions/drone/vehicles/types' | |
| 8 | ||
| 9 | // Agreed shared-runtime seam: actual front-transition dynamics and capture-bound observation; | |
| 10 | // independent model probes are assertions only, never controller inputs or physical evidence. | |
| 11 | test('an owned VTOL front transition transfers lift to the wing from actual relative-airflow observations', () => { | |
| 12 | const profile = createVtolTransitionProfile(), createSession = profile.createSession | |
| 13 | const independent: { atMs: number, state: VehiclePlantState }[] = [], inputs: VehicleControllerContext[] = [] | |
| 14 | profile.createSession = () => { | |
| 15 | const session = createSession(), controller = session.createController() | |
| 16 | return { ...session, createController: () => ({ step(c) { inputs.push(structuredClone(c)); return controller.step(c) } }), | |
| 17 | observeIndependent(state, atMs) { independent.push({ atMs, state: structuredClone(state) }) } } | |
| 18 | } | |
| 19 | const report = runVehicleSimulation(profile, { seconds: 35 }) | |
| 20 | const front = report.trace.rows.filter(row => row.result.taskId === 'front') | |
| 21 | expect(front.length).toBeGreaterThan(10) | |
| 22 | const confirmed = report.trace.rows.filter(row => row.observation.transition?.mode === 'cruise' && row.observation.transition.outcome === 'confirmed') | |
| 23 | expect(confirmed).toHaveLength(1) | |
| 24 | const row = confirmed[0]!, request = front.find(row => row.result.transition)?.result.transition! | |
| 25 | expect(row.observation.transition!.phase).toBe('cruise') | |
| 26 | expect(row.observation.transition!.measurementCapturedAtMs).toBeGreaterThanOrEqual(request.issuedAtMs + 500) | |
| 27 | expect(row.result.taskId).toBe('cruise-leg') | |
| 28 | const before = independent.filter(p => p.atMs >= request.issuedAtMs && p.atMs <= row.observation.nowMs) | |
| 29 | expect(before.some(p => (p.state.details.airspeedMps as number) > 12)).toBe(true) | |
| 30 | const wingBorne = before.filter(p => p.atMs >= row.observation.nowMs - 500) | |
| 31 | expect(wingBorne.length).toBeGreaterThan(100) | |
| 32 | expect(wingBorne.every(p => (p.state.details.rotorThrustN as number) < 1)).toBe(true) | |
| 33 | expect(wingBorne.every(p => (p.state.details.airspeedMps as number) >= 12 && (p.state.details.airspeedMps as number) <= 14)).toBe(true) | |
| 34 | expect(before.every(p => Math.abs(p.state.positionM[2] + 15) < 2)).toBe(true) | |
| 35 | expect(before.every(p => quaternionEuler(p.state.quaternionWxyz).slice(0, 2).every(n => Math.abs(n) < 0.45))).toBe(true) | |
| 36 | expect(report.path.some(p => p.actuators[4]! > 0)).toBe(true) | |
| 37 | expect(inputs.every(c => !Object.hasOwn(c, 'truth') && c.estimate.evidence === 'fixture' && c.controlRate?.evidence === 'modeled-qualified-imu')).toBe(true) | |
| 38 | expect(report.physicalFlightEnabled).toBe(false) | |
| 39 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) | |
| 40 | }) | |
| 41 | ||
| 42 | ||
| 43 | test('the owned VTOL route returns from wing-borne cruise to captured hover and a vertical landing', () => { | |
| 44 | const report = runVehicleSimulation(createVtolTransitionProfile(), { seconds: 160 }) | |
| 45 | expect(report.status).toBe('completed') | |
| 46 | const phases = report.trace.rows.flatMap(row => row.observation.transition?.outcome === 'confirmed' ? [row.observation.transition.phase] : []) | |
| 47 | expect(phases).toEqual(['hover', 'cruise', 'hover']) | |
| 48 | expect(report.path.some(p => p.mission.taskId === 'back' && p.transitionObservation?.phase === 'back-transition')).toBe(true) | |
| 49 | const back = report.path.filter(p => p.mission.taskId === 'back') | |
| 50 | expect(back.length).toBeGreaterThan(50) | |
| 51 | expect(back.some(p => p.actuators.slice(0, 4).reduce((a, n) => a + n, 0) > 15)).toBe(true) | |
| 52 | expect(back.filter(p => p.mission.transition !== null).every(p => p.actuators[4] === 0)).toBe(true) | |
| 53 | expect(report.path.at(-1)!.measurement!.values.onSurface).toBe(true) | |
| 54 | expect(report.finalTruthPositionM[2]).toBe(0) | |
| 55 | expect(Math.hypot(report.finalTruthPositionM[0] - 180, report.finalTruthPositionM[1])).toBeLessThan(0.3) | |
| 56 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 57 | expect(report.board.expiresAtMs).toBeNull() | |
| 58 | expect(report.mission.transition).toBeNull() | |
| 59 | expect(report.mission.targetM).toBeNull() | |
| 60 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) | |
| 61 | }) | |
| 62 | ||
| 63 | ||
| 64 | test('the full modeled back-capture allowance remains finite and is charged to the complete mission reserve', () => { | |
| 65 | const plan = createVtolTransitionProfile().mission, back = plan.tasks.find(t => t.id === 'back')! | |
| 66 | if (back.kind !== 'vtol-mode') throw new Error('Expected the back mode task') | |
| 67 | back.timeoutMs = 45000 | |
| 68 | expect(validateVehicleMission(plan).worstCaseMs).toBe(131520) | |
| 69 | expect(validateVehicleMission(plan).requiredWh).toBeCloseTo(43.84, 8) | |
| 70 | back.timeoutMs = 45001 | |
| 71 | expect(() => validateVehicleMission(plan)).toThrow('unsupported profile') | |
| 72 | back.timeoutMs = 45000 | |
| 73 | const front = plan.tasks.find(t => t.id === 'front')! | |
| 74 | if (front.kind !== 'vtol-mode') throw new Error('Expected the front mode task') | |
| 75 | front.timeoutMs = 15001 | |
| 76 | expect(() => validateVehicleMission(plan)).toThrow('unsupported profile') | |
| 77 | front.timeoutMs = 15000 | |
| 78 | plan.power.availableWh = 50 | |
| 79 | expect(() => validateVehicleMission(plan)).toThrow('preserve energy reserve') | |
| 80 | }) | |
| 81 | ||
| 82 | ||
| 83 | test('the original short back-capture budget fails without retry or a fabricated landing', () => { | |
| 84 | const profile = createVtolTransitionProfile(), back = profile.mission.tasks.find(t => t.id === 'back')! | |
| 85 | if (back.kind !== 'vtol-mode') throw new Error('Expected the back mode task') | |
| 86 | back.timeoutMs = 15000 | |
| 87 | const report = runVehicleSimulation(profile, { seconds: 60 }) | |
| 88 | expect(report.status).toBe('blocked') | |
| 89 | expect(report.mission.reason).toBe('vtol-transition-outcome-uncertain') | |
| 90 | expect(report.trace.rows.filter(r => r.observation.transition?.mode === 'hover' && r.observation.transition.outcome === 'confirmed')).toHaveLength(1) | |
| 91 | expect(report.path.some(p => p.mission.taskId === 'land')).toBe(false) | |
| 92 | expect(report.path.at(-1)!.measurement!.values.onSurface).toBe(false) | |
| 93 | expect(report.board.expiresAtMs).toBeNull() | |
| 94 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 95 | }) | |
| 96 | ||
| 97 | ||
| 98 | test('a physically failed modeled pusher cannot certify wing support or trigger an automatic retry', () => { | |
| 99 | const profile = createVtolTransitionProfile(), createSession = profile.createSession | |
| 100 | profile.createSession = () => { | |
| 101 | const session = createSession(), plant = session.createPlant() | |
| 102 | return { ...session, createPlant: () => ({ snapshot: () => plant.snapshot(), step(actuators, dtS, environment) { | |
| 103 | const applied = [...actuators] | |
| 104 | applied[4] = 0 | |
| 105 | return plant.step(applied, dtS, environment) | |
| 106 | } }) } | |
| 107 | } | |
| 108 | const report = runVehicleSimulation(profile, { seconds: 60 }) | |
| 109 | expect(report.status).toBe('blocked') | |
| 110 | expect(report.mission.reason).toBe('vtol-transition-outcome-uncertain') | |
| 111 | expect(report.path.some(p => p.actuators[4]! > 0.3)).toBe(true) | |
| 112 | expect(report.path.every(p => p.measurement!.values.pusherThrustN === 0)).toBe(true) | |
| 113 | expect(report.trace.rows.some(r => r.observation.transition?.mode === 'cruise' && r.observation.transition.outcome === 'confirmed')).toBe(false) | |
| 114 | expect(report.transitions.filter(t => t.taskId === 'front' && t.state === 'running')).toHaveLength(1) | |
| 115 | expect(report.path.some(p => p.mission.taskId === 'cruise-leg')).toBe(false) | |
| 116 | expect(report.mission.transition).toBeNull() | |
| 117 | expect(report.board.expiresAtMs).toBeNull() | |
| 118 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 119 | }) | |
| 120 | ||
| 121 | ||
| 122 | test('high ground speed in a co-moving-air fault fixture cannot substitute for relative wing airspeed', () => { | |
| 123 | // This artificial world fault follows the independently observed body velocity. It is not a | |
| 124 | // meteorological model, a fabricated air packet, or information supplied to the controller. | |
| 125 | const profile = createVtolTransitionProfile(), createSession = profile.createSession | |
| 126 | let active = false, wind: [number, number, number] = [0, 0, 0] | |
| 127 | profile.createSession = () => { | |
| 128 | const session = createSession(), controller = session.createController() | |
| 129 | return { ...session, createController: () => ({ step(c) { if (c.mission.taskId === 'front') active = true; return controller.step(c) } }), | |
| 130 | observeIndependent(state) { wind = [...state.velocityMps] } } | |
| 131 | } | |
| 132 | profile.faults = () => active ? { windMps: [...wind] } : {} | |
| 133 | const report = runVehicleSimulation(profile, { seconds: 60 }) | |
| 134 | expect(report.status).toBe('blocked') | |
| 135 | expect(report.mission.reason).toBe('vtol-transition-outcome-uncertain') | |
| 136 | const fast = report.trace.rows.filter(r => r.result.taskId === 'front' && Math.hypot(...r.observation.velocityMps) > 12) | |
| 137 | expect(fast.length).toBeGreaterThan(20) | |
| 138 | const fastStart = fast[0]!.observation.nowMs | |
| 139 | expect(report.path.filter(p => p.atMs >= fastStart).every(p => (p.measurement!.values.airspeedMps as number) < 0.1)).toBe(true) | |
| 140 | expect(report.trace.rows.some(r => r.observation.transition?.mode === 'cruise' && r.observation.transition.outcome === 'confirmed')).toBe(false) | |
| 141 | expect(report.path.some(p => p.mission.taskId === 'cruise-leg')).toBe(false) | |
| 142 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 143 | expect(report.board.expiresAtMs).toBeNull() | |
| 144 | }) | |
| 145 | ||
| 146 | ||
| 147 | function actualTransitionInputs() { | |
| 148 | const profile = createVtolTransitionProfile(), createSession = profile.createSession, inputs: VehicleControllerContext[] = [] | |
| 149 | profile.createSession = () => { | |
| 150 | const session = createSession(), controller = session.createController() | |
| 151 | return { ...session, createController: () => ({ step(c) { inputs.push(structuredClone(c)); return controller.step(c) } }) } | |
| 152 | } | |
| 153 | expect(runVehicleSimulation(profile, { seconds: 160 }).status).toBe('completed') | |
| 154 | return inputs | |
| 155 | } | |
| 156 | ||
| 157 | test('front, cruise, back and post-back hover retain latching source, reference and clock guards', () => { | |
| 158 | const inputs = actualTransitionInputs() | |
| 159 | for (const [task, atMs] of [['front', 13000], ['cruise-leg', 20000], ['back', 30000], ['land', 68000]] as const) { | |
| 160 | const index = inputs.findIndex(c => c.nowMs === atMs) | |
| 161 | expect(index).toBeGreaterThan(0) | |
| 162 | expect(inputs[index]!.mission.taskId).toBe(task) | |
| 163 | for (const change of [ | |
| 164 | (c: VehicleControllerContext) => { c.controlRate!.capturedAtMs -= 1000 }, | |
| 165 | (c: VehicleControllerContext) => { c.controlRate!.reference = 'foreign-reference' }, | |
| 166 | (c: VehicleControllerContext) => { c.measurement!.capturedAtMs -= 1000 }, | |
| 167 | (c: VehicleControllerContext) => { c.measurement!.sequence -= 2 }, | |
| 168 | (c: VehicleControllerContext) => { c.measurementPose!.reference = 'foreign-reference' }, | |
| 169 | (c: VehicleControllerContext) => { c.measurementPose!.capturedAtMs -= 20 }, | |
| 170 | (c: VehicleControllerContext) => { c.measurementPose!.gyroCapturedAtMs = NaN }, | |
| 171 | (c: VehicleControllerContext) => { c.estimate.localPosition!.origin!.ellipsoidHeightM += 1 }, | |
| 172 | (c: VehicleControllerContext) => { c.estimate.attitude!.quaternionWxyz = [2, 0, 0, 0] }, | |
| 173 | (c: VehicleControllerContext) => { c.nowMs -= 20 }, | |
| 174 | ]) { | |
| 175 | const controller = createVtolFlightController() | |
| 176 | let warmed = true | |
| 177 | for (const c of inputs.slice(0, index)) if (controller.step(c).inhibited) warmed = false | |
| 178 | expect(warmed).toBe(true) | |
| 179 | const invalid = structuredClone(inputs[index]!) | |
| 180 | change(invalid) | |
| 181 | const refused = controller.step(invalid) | |
| 182 | expect(refused.inhibited).toBe(true) | |
| 183 | expect(refused.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 184 | expect(refused.transition).toBeUndefined() | |
| 185 | const next = controller.step(inputs[index + 1]!) | |
| 186 | expect(next.inhibited).toBe(true) | |
| 187 | expect(next.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 188 | } | |
| 189 | } | |
| 190 | }) | |
| 191 | ||
| 192 | ||
| 193 | for (const [task, atMs] of [['front', 13000], ['cruise-leg', 20000], ['back', 30000], ['land', 68000]] as const) { | |
| 194 | for (const failure of ['dropImu', 'dropPosition', 'dropVehicleSensor', 'rcOverride', 'kill', 'hostLost'] as const) { | |
| 195 | test(`VTOL ${task} withdraws independent output authority for ${failure}`, () => { | |
| 196 | const profile = createVtolTransitionProfile() | |
| 197 | profile.faults = (_scenario, nowMs) => nowMs >= atMs ? { [failure]: true } : {} | |
| 198 | const report = runVehicleSimulation(profile, { seconds: 90 }) | |
| 199 | expect(report.status).toBe(failure === 'hostLost' ? 'host-lost' : 'blocked') | |
| 200 | expect(report.path.filter(p => p.atMs < atMs).at(-1)!.mission.taskId).toBe(task) | |
| 201 | expect(report.finalTruthPositionM[2]).toBeLessThan(0) | |
| 202 | expect(report.mission.state).not.toBe('completed') | |
| 203 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 204 | expect(report.board.expiresAtMs).toBeNull() | |
| 205 | if (failure === 'hostLost') { | |
| 206 | expect(report.board.fault).toBe('output-lease-expired') | |
| 207 | expect(report.authority.lastCheckedAtMs).toBeLessThan(atMs) | |
| 208 | } | |
| 209 | else { | |
| 210 | expect(report.mission.state).toBe('blocked') | |
| 211 | expect(report.mission.targetM).toBeNull() | |
| 212 | expect(report.mission.transition).toBeNull() | |
| 213 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) | |
| 214 | } | |
| 215 | if (failure === 'rcOverride') expect(report.authority.events.some(event => event.to === 'manual')).toBe(true) | |
| 216 | if (failure === 'kill') expect(report.authority.events.some(event => event.to === 'killed')).toBe(true) | |
| 217 | }) | |
| 218 | } | |
| 219 | } | |
| 220 | ||
| 221 | ||
| 222 | test('a turning VTOL cruise leg and back capture retain the same NED position and heading contract', () => { | |
| 223 | const profile = createVtolTransitionProfile() | |
| 224 | for (const task of profile.mission.tasks) { | |
| 225 | if (task.id === 'cruise-leg' || task.id === 'land') { | |
| 226 | if (!('positionM' in task)) throw new Error('Expected a position task') | |
| 227 | task.positionM[0] = 150; task.positionM[1] = 100 | |
| 228 | } | |
| 229 | if (task.id === 'back' && task.kind === 'vtol-mode') task.headingRad = 1 | |
| 230 | } | |
| 231 | const report = runVehicleSimulation(profile, { seconds: 160 }) | |
| 232 | expect(report.status).toBe('completed') | |
| 233 | expect(report.path.some(p => p.truthPositionM[1] > 90)).toBe(true) | |
| 234 | expect(Math.hypot(report.finalTruthPositionM[0] - 150, report.finalTruthPositionM[1] - 100)).toBeLessThan(0.3) | |
| 235 | expect(report.finalTruthPositionM[2]).toBe(0) | |
| 236 | expect(report.trace.rows.filter(r => r.observation.transition?.outcome === 'confirmed').map(r => r.observation.transition!.phase)).toEqual(['hover', 'cruise', 'hover']) | |
| 237 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 238 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) | |
| 239 | }) | |
| 240 | ||
| 241 | ||
| 242 | test('airflow capture attitude without its observed source epochs cannot drive the full controller', () => { | |
| 243 | const inputs = actualTransitionInputs(), index = inputs.findIndex(c => c.nowMs === 13000), controller = createVtolFlightController() | |
| 244 | for (const c of inputs.slice(0, index)) controller.step(c) | |
| 245 | const invalid = structuredClone(inputs[index]!) | |
| 246 | Reflect.deleteProperty(invalid.measurementPose!, 'attitudeCapturedAtMs') | |
| 247 | Reflect.deleteProperty(invalid.measurementPose!, 'gyroCapturedAtMs') | |
| 248 | const refused = controller.step(invalid) | |
| 249 | expect(refused.inhibited).toBe(true) | |
| 250 | expect(refused.transition).toBeUndefined() | |
| 251 | expect(refused.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 252 | expect(controller.step(inputs[index + 1]!).inhibited).toBe(true) | |
| 253 | }) | |
| 254 | ||
| 255 | ||
| 256 | test('a first full-flight mode request cannot invent a negative issuance or an oversized control budget', () => { | |
| 257 | const inputs = actualTransitionInputs(), index = inputs.findIndex(c => c.mission.transition?.mode === 'cruise') | |
| 258 | expect(index).toBeGreaterThan(0) | |
| 259 | for (const change of [ | |
| 260 | (c: VehicleControllerContext) => { c.mission.transition!.issuedAtMs = -1 }, | |
| 261 | (c: VehicleControllerContext) => { c.mission.transition!.expiresAtMs = c.mission.transition!.issuedAtMs + 15001 }, | |
| 262 | ]) { | |
| 263 | const controller = createVtolFlightController() | |
| 264 | for (const c of inputs.slice(0, index)) controller.step(c) | |
| 265 | const invalid = structuredClone(inputs[index]!) | |
| 266 | change(invalid) | |
| 267 | const refused = controller.step(invalid) | |
| 268 | expect(refused.inhibited).toBe(true) | |
| 269 | expect(refused.transition).toBeUndefined() | |
| 270 | expect(refused.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 271 | expect(controller.step(inputs[index + 1]!).inhibited).toBe(true) | |
| 272 | } | |
| 273 | }) | |
| 274 | ||
| 275 | ||
| 276 | test('full-flight acknowledgements retain the oldest capture attitude and gyro actually consumed', () => { | |
| 277 | const inputs = actualTransitionInputs(), controller = createVtolFlightController() | |
| 278 | let fullFlight = false | |
| 279 | const overclaims: number[] = [], preRequest: number[] = [] | |
| 280 | for (const c of inputs) { | |
| 281 | fullFlight ||= c.mission.transition?.mode === 'cruise' | |
| 282 | const response = controller.step(c), observation = response.transition | |
| 283 | if (!fullFlight || !observation) continue | |
| 284 | if (observation.measurementCapturedAtMs > c.measurementPose!.attitudeCapturedAtMs || observation.measurementCapturedAtMs > c.measurementPose!.gyroCapturedAtMs) overclaims.push(c.nowMs) | |
| 285 | if (observation.measurementCapturedAtMs < c.mission.transition!.issuedAtMs) preRequest.push(c.nowMs) | |
| 286 | } | |
| 287 | expect(fullFlight).toBe(true) | |
| 288 | expect(overclaims).toEqual([]) | |
| 289 | expect(preRequest).toEqual([]) | |
| 290 | }) | |
| 291 | ||
| 292 | ||
| 293 | test('back hover cannot confirm while a near-stationary body remains away from the requested heading', () => { | |
| 294 | const inputs = actualTransitionInputs(), controller = createVtolFlightController() | |
| 295 | const back = inputs.filter(c => c.mission.taskId === 'back' && c.mission.transition?.mode === 'hover') | |
| 296 | expect(back.length).toBeGreaterThan(50) | |
| 297 | const startMs = back.at(-1)!.nowMs - 800 | |
| 298 | const wrongYaw: [number, number, number, number] = [Math.cos(0.3 / 2), 0, 0, Math.sin(0.3 / 2)] | |
| 299 | let qualifiedSamples = 0 | |
| 300 | const confirmations: number[] = [] | |
| 301 | for (const c of inputs) { | |
| 302 | if (c.nowMs < startMs) { controller.step(c); continue } | |
| 303 | if (c.mission.taskId !== 'back' || !c.mission.transition) break | |
| 304 | // A measured yaw offset is a public input fixture, not a fabricated mode outcome. | |
| 305 | // Preserve actual position, velocity, gyro, airflow and every capture epoch. | |
| 306 | const offset = structuredClone(c) | |
| 307 | offset.estimate.attitude!.quaternionWxyz = multiplyQuaternion(wrongYaw, offset.estimate.attitude!.quaternionWxyz) | |
| 308 | offset.measurementPose!.quaternionWxyz = multiplyQuaternion(wrongYaw, offset.measurementPose!.quaternionWxyz) | |
| 309 | const response = controller.step(offset) | |
| 310 | expect(response.inhibited).toBe(false) | |
| 311 | const yaw = quaternionEuler(offset.estimate.attitude!.quaternionWxyz)[2] | |
| 312 | expect(Math.abs(Math.atan2(Math.sin(yaw - offset.mission.transition!.headingRad), Math.cos(yaw - offset.mission.transition!.headingRad)))).toBeGreaterThan(0.2) | |
| 313 | if (Math.hypot(...offset.estimate.localPosition!.velocityMps) <= 0.2 && Math.hypot(...offset.controlRate!.angularRateRadS) < 0.1) qualifiedSamples++ | |
| 314 | if (response.transition?.outcome === 'confirmed') confirmations.push(c.nowMs) | |
| 315 | } | |
| 316 | expect(qualifiedSamples).toBeGreaterThan(25) | |
| 317 | expect(confirmations).toEqual([]) | |
| 318 | // Continue the same controller with fresh observations of the recorded settled heading. | |
| 319 | // The prior wrong-heading window cannot count toward the required 500 ms dwell. | |
| 320 | const settled = back.at(-1)!, qualifiedAt: number[] = [] | |
| 321 | for (let tick = 1; tick <= 30; tick++) { | |
| 322 | const c = structuredClone(settled), elapsedMs = tick * 20 | |
| 323 | c.nowMs += elapsedMs; c.estimate.nowMs += elapsedMs | |
| 324 | c.estimate.attitude!.capturedAtMs! += elapsedMs | |
| 325 | c.estimate.localPosition!.capturedAtMs! += elapsedMs | |
| 326 | c.estimate.localPosition!.predictedToMs += elapsedMs | |
| 327 | c.controlRate!.capturedAtMs += elapsedMs | |
| 328 | c.measurement!.capturedAtMs += elapsedMs; c.measurement!.receivedAtMs += elapsedMs; c.measurement!.sequence += tick | |
| 329 | for (const key of ['capturedAtMs', 'positionCapturedAtMs', 'positionPredictedToMs', 'attitudeCapturedAtMs', 'attitudePredictedToMs', 'gyroCapturedAtMs'] as const) c.measurementPose![key] += elapsedMs | |
| 330 | const response = controller.step(c) | |
| 331 | expect(response.inhibited).toBe(false) | |
| 332 | if (response.transition?.outcome === 'confirmed') qualifiedAt.push(c.nowMs) | |
| 333 | } | |
| 334 | expect(qualifiedAt).toEqual([settled.nowMs + 520]) | |
| 335 | }) | |