also looking at this
feat: add owned VTOL hover development
#76
30 files
+780
-111
| @@ -142,6 +142,10 @@ jobs: | ||
| 142 | 142 | working-directory: ${{ runner.temp }} |
| 143 | 143 | - run: $GITHUB_WORKSPACE/storage/runtime-build/cli/drone controls:simulate --scenario hover --seconds 3 |
| 144 | 144 | working-directory: ${{ runner.temp }} |
| 145 | - run: $GITHUB_WORKSPACE/storage/runtime-build/cli/drone vtol:hover | |
| 146 | working-directory: ${{ runner.temp }} | |
| 147 | - run: $GITHUB_WORKSPACE/storage/runtime-build/cli/drone vtol:hover --scenario host-loss | |
| 148 | working-directory: ${{ runner.temp }} | |
| 145 | 149 | - run: $GITHUB_WORKSPACE/storage/runtime-build/cli/drone authority:simulate --scenario pi-loss --seconds 4 |
| 146 | 150 | working-directory: ${{ runner.temp }} |
| 147 | 151 | - run: $GITHUB_WORKSPACE/storage/runtime-build/cli/drone drivers:bench --driver ardupilot --drop-first-ack |
| @@ -76,6 +76,10 @@ 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. | |
| 82 | ||
| 79 | 83 | ## Simulated carried load |
| 80 | 84 | |
| 81 | 85 | Characterize a centered or forward point mass, a prescribed separation and native link loss through |
| @@ -0,0 +1,27 @@ | ||
| 1 | import { defineCommand } from '@stacksjs/cli' | |
| 2 | import { print, printError } from '@stacksjs/cli/output' | |
| 3 | import { createVtolHoverProfile, vtolHoverScenarios } from '../../resources/functions/drone/vehicles/vtol' | |
| 4 | import { runVehicleSimulation } from '../../resources/functions/drone/vehicles/runtime' | |
| 5 | ||
| 6 | export default defineCommand({ | |
| 7 | name: 'vtol:hover', | |
| 8 | description: 'Exercise the fictional lift-and-cruise airframe in hover only; no wing transition or physical control', | |
| 9 | options: { | |
| 10 | '--scenario <name>': { description: vtolHoverScenarios.join(', '), default: 'hover' }, | |
| 11 | '--seconds <n>': { description: 'Maximum virtual duration, 2..180 seconds', default: '60' }, | |
| 12 | }, | |
| 13 | handle(options) { | |
| 14 | try { | |
| 15 | const result = runVehicleSimulation(createVtolHoverProfile(options.scenario), { scenario: options.scenario, seconds: Number(options.seconds) }) | |
| 16 | const { trace, path, ...report } = result | |
| 17 | const expectedStatus = options.scenario === 'hover' ? 'completed' : options.scenario === 'host-loss' ? 'host-lost' : 'blocked' | |
| 18 | const observedFlight = path.some(row => row.truthPositionM[2] < -1 && row.actuators.slice(0, 4).some(n => n > 0)) | |
| 19 | const boardNeutral = result.board.actuators.every(n => n === 0) && result.board.expiresAtMs === null | |
| 20 | const hoverOnly = path.every(row => row.actuators.slice(4).every(n => n === 0)) | |
| 21 | const demonstrationMatched = result.status === expectedStatus && observedFlight && boardNeutral && hoverOnly | |
| 22 | print(JSON.stringify({ ...report, scope: 'development-hover-only', transitionsImplemented: false, expectedStatus, demonstrationMatched, observedFlight, boardNeutral, hoverOnly, recordedMissionRows: trace.rows.length, evaluatedPathRows: path.length }, (_key, value) => typeof value === 'bigint' ? String(value) : value, 2)) | |
| 23 | if (!demonstrationMatched) process.exitCode = 1 | |
| 24 | } | |
| 25 | catch (error) { printError(error instanceof Error ? error.message : String(error)); process.exitCode = 1 } | |
| 26 | }, | |
| 27 | }) | |
| @@ -50,6 +50,7 @@ import PatrolReplay from '../app/Commands/PatrolReplay' | ||
| 50 | 50 | |
| 51 | 51 | import AntennaBench from '../app/Commands/AntennaBench' |
| 52 | 52 | import AirSimulate from '../app/Commands/AirSimulate' |
| 53 | import VtolHover from '../app/Commands/VtolHover' | |
| 53 | 54 | import GroundSimulate from '../app/Commands/GroundSimulate' |
| 54 | 55 | import SurfaceSimulate from '../app/Commands/SurfaceSimulate' |
| 55 | 56 | import SubseaSimulate from '../app/Commands/SubseaSimulate' |
| @@ -126,6 +127,7 @@ PatrolSimulate(app) | ||
| 126 | 127 | PatrolReplay(app) |
| 127 | 128 | AntennaBench(app) |
| 128 | 129 | AirSimulate(app) |
| 130 | VtolHover(app) | |
| 129 | 131 | GroundSimulate(app) |
| 130 | 132 | SurfaceSimulate(app) |
| 131 | 133 | SubseaSimulate(app) |
| @@ -76,6 +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 | 80 | { text: 'Owned ground vehicle profile', link: '/vehicles-ground' }, |
| 80 | 81 | { text: 'Owned surface vessel profile', link: '/vehicles-surface' }, |
| 81 | 82 | { text: 'Owned subsea inspection profile', link: '/vehicles-subsea' }, |
| @@ -18,7 +18,9 @@ a scripted peer does not establish that a pinned autopilot firmware implements e | ||
| 18 | 18 | Do not add a database transaction queue, HTTP arm endpoint or dependency on an external autopilot |
| 19 | 19 | to our own runtime or mission engine. |
| 20 | 20 | |
| 21 | Keep serial/TCP, command-specific behavior, complete mission/parameter variants, key lifecycle, | |
| 22 | FTP/log/event/time/RTCM services and payload/ROS/CAN gateways as explicit remaining sub-capabilities. | |
| 21 | The initial slice deferred serial and TCP. Explicit loopback TCP was subsequently implemented with | |
| 22 | scripted peers and shared protocol transactions; see the [TCP qualification record](../validation.md#tcp-transport-software-slice). | |
| 23 | Serial, actual firmware transport campaigns, command-specific behavior, complete mission/parameter | |
| 24 | variants, key lifecycle, FTP/log/event/time/RTCM services and payload/ROS/CAN gateways remain open. | |
| 23 | 25 | See [the driver contract and limits](../driver-protocols.md) and |
| 24 | 26 | [retained conformance evidence](https://github.com/stacksjs/drone/blob/main/docs/driver-protocol-evidence.json). |
| @@ -56,6 +56,9 @@ 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. | |
| 61 | ||
| 59 | 62 | ## Explore a workflow |
| 60 | 63 | |
| 61 | 64 | | Workflow | Current development surface | Guide | |
| @@ -10,13 +10,13 @@ means there is no native implementation yet. A protocol-compatible transport is | ||
| 10 | 10 | | Calibration, sensor timing and quality | Native register acquisition, SI conversion, thermal/mount calibration profiles, independent age/stuck/dropout checks and replay; physical capture/calibration/timing pending | Local position/battery telemetry subset | [#3](https://github.com/stacksjs/drone/issues/3) | |
| 11 | 11 | | Attitude, navigation estimation, EKF, VIO, RTK and redundancy | Complementary tilt/relative-yaw, GNSS local-position Kalman blocks and pressure-relative height; heading, inertial EKF and hardware validation pending | Estimator output subset; no health-fusion parity | [#4](https://github.com/stacksjs/drone/issues/4) | |
| 12 | 12 | | Rate, attitude, velocity, position, allocation and tuning | Owned simulated cascaded loops/quad-X allocation with estimator feedback and rigid-body scenarios; board port, tuning and physical validation pending | Velocity-only quadcopter SITL | [#5](https://github.com/stacksjs/drone/issues/5) | |
| 13 | | Arming, RC override, kill, fences, battery and recovery | Versioned simulation authority, explicit arming/override/fault latches and modeled independent output expiry; board port, HIL and recovery controllers pending | No arm/mode/kill commands; physical receive-only | [#6](https://github.com/stacksjs/drone/issues/6) | | |
| 14 | | MAVLink routing, ACK, mission/parameter protocol, signing, time and transport | Bounded MAVLink 2 command ACK/retry, named parameters, waypoint transfer/resume and optional signing; full firmware SITL, serial/TCP and deeper services pending | ArduPilot/PX4 telemetry, SITL velocity and shared acknowledged protocol subset; scripted UDP evidence only | [#7](https://github.com/stacksjs/drone/issues/7) | | |
| 13 | | Arming, RC override, kill, fences, battery and recovery | Versioned simulation authority, explicit arming/override/fault latches and modeled independent output expiry; board port, HIL and recovery controllers pending | No dedicated arm/mode/kill workflow; generic SITL commands require caller-selected IDs and parameters; physical receive-only | [#6](https://github.com/stacksjs/drone/issues/6) | | |
| 14 | | MAVLink routing, ACK, mission/parameter protocol, signing, time and transport | Bounded MAVLink 2 command ACK/retry, named parameters, waypoint transfer/resume and optional signing over UDP or explicit loopback TCP; full firmware SITL, serial and deeper services pending | ArduPilot/PX4 telemetry, SITL velocity and the [shared acknowledged protocol subset](../driver-protocols.md); scripted UDP/TCP peer evidence, without firmware execution qualification | [#7](https://github.com/stacksjs/drone/issues/7) | | |
| 15 | 15 | | Payloads, buses, camera triggers, ESCs, power, gimbals and winches | Owned still-capture ACKs, bounded clock sync and estimator-pose association, Pi IMX500 adapter, replay and bus contracts; physical validation and other payload drivers pending | No payload actuator exposure | [#8](https://github.com/stacksjs/drone/issues/8) | |
| 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 | No log/FTP/parameter management | [#9](https://github.com/stacksjs/drone/issues/9) | | |
| 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 | No mission upload or mission-mode control | [#10](https://github.com/stacksjs/drone/issues/10) | | |
| 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 | | 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 aerodynamic trainer with runway takeoff/survey/landing and wind/airspeed-loss tests; VTOL, helicopter, soaring, blimp and experimental families remain planned | 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 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) | | |
| 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) | |
| @@ -32,7 +32,7 @@ means there is no native implementation yet. A protocol-compatible transport is | ||
| 32 | 32 | | Cargo, long-range/BVLOS cases and redundant links | Partial: owned carried-point-mass/CG physics, prescribed split and native link-loss characterization; delivery mission, release ACK, custody, energy margin and redundant links remain open | Native `cargo:load-bench` [simulation evidence and limits](../cargo-load.md); no physical or BVLOS qualification | [#25](https://github.com/stacksjs/drone/issues/25) | |
| 33 | 33 | | Gimbal/camera control, smooth paths and broadcast timecode | Partial: owned hover POI angular reframe, lagged/quantized gimbal, exposure-bound modeled stills and latched target/camera/authority faults; translational paths, physical integration, lens/video and handoff remain open | Native `cinema:simulate` [simulation semantics and limits](../cinema.md); no physical/broadcast timing qualification | [#26](https://github.com/stacksjs/drone/issues/26) | |
| 34 | 34 | | Environment, wildlife, anti-poaching observation and atmospheric samples | Partial: owned two-height synthetic cell sampling, calibration/geotags, conditional uncertainty, collection-policy faults and unsigned five-file provenance/export; physical samples, wildlife/authority and actual geofence qualification remain open | Native `environment:simulate` [semantics and limits](../environment.md); human review required | [#27](https://github.com/stacksjs/drone/issues/27) | |
| 35 | | Deep feature audit, refresh and conformance gates | Pinned directory accounting and sampled behavior audit: 79 cases, including ten gimbal/mount and ten camera-feedback/capture gaps; deeper review and implementation remain open | [Audit integrity and source provenance](../parity.md); 26 bounded claims, no general parity claim | [#28](https://github.com/stacksjs/drone/issues/28) | | |
| 35 | | Deep feature audit, refresh and conformance gates | Pinned directory accounting and sampled behavior audit: 89 cases, including ten transport and ten camera-feedback/capture gaps; deeper review and implementation remain open | [Audit integrity and source provenance](../parity.md); 26 bounded claims, no general parity claim | [#28](https://github.com/stacksjs/drone/issues/28) | | |
| 36 | 36 | |
| 37 | 37 | ## Auditable source baseline |
| 38 | 38 | |
| @@ -61,9 +61,23 @@ not explain an earlier failure whose Chrome evidence was removed. | ||
| 61 | 61 | The generated server function registry also loads successfully without duplicate runtime exports. |
| 62 | 62 | - This is scripted protocol-peer conformance, not actual autopilot firmware SITL, flight or HIL. |
| 63 | 63 | [Issue #7](https://github.com/stacksjs/drone/issues/7) remains open for pinned-firmware campaigns, |
| 64 | serial/TCP, complete command/mission/parameter variants, persistent signing/key lifecycle and the | |
| 64 | serial, complete command/mission/parameter variants, persistent signing/key lifecycle and the | |
| 65 | 65 | explicit FTP/log/event/time/RTCM/payload/ROS/CAN sub-capabilities. Physical writes remain disabled. |
| 66 | 66 | |
| 67 | ## TCP transport software slice | |
| 68 | ||
| 69 | The optional ArduPilot and PX4 factories also expose explicit loopback TCP. Scripted peer tests | |
| 70 | cover incremental framing, bounded pending writes, link loss, cancellation, signed input and fresh | |
| 71 | session construction after failure. The native `drivers:bench --transport tcp` command uses the same | |
| 72 | protocol implementation as UDP; copied standalone CLI campaigns exercise both firmware identities | |
| 73 | and transports outside the checkout. | |
| 74 | ||
| 75 | This transport slice was published in [b18e1d6](https://github.com/stacksjs/drone/commit/b18e1d63d344452cf45719cf3b4072a752b71bce), | |
| 76 | with its [exact-head qualification run](https://github.com/stacksjs/drone/actions/runs/39870834185). | |
| 77 | These are scripted peer and software-runtime results. They do not qualify actual firmware execution, | |
| 78 | physical writes, serial transport, transparent operation replay or persistent signing state. | |
| 79 | Read the [transport deadlines, queue limits and reconnection contract](driver-protocols.md#tcp-framing-loss-and-reconnection). | |
| 80 | ||
| 67 | 81 | ## Simulated authority and failsafe slice |
| 68 | 82 | |
| 69 | 83 | - 146 Bun tests pass with 16,781 assertions, including 15 authority tests. The two native CTest |
| @@ -0,0 +1,64 @@ | ||
| 1 | # VTOL hover development | |
| 2 | ||
| 3 | The owned lift-and-cruise model can execute a **hover-only** mission through the shared raw sensor, | |
| 4 | estimator, mission, authority and independent output-lease runtime. It takes off vertically, reaches | |
| 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 | |
| 7 | profiles in the `/missions` workspace. | |
| 8 | ||
| 9 | ```sh | |
| 10 | ./buddy vtol:hover | |
| 11 | ./buddy vtol:hover --scenario host-loss | |
| 12 | ./buddy vtol:hover --scenario imu-loss | |
| 13 | ``` | |
| 14 | ||
| 15 | The native Stacks command defaults to 60 virtual seconds. It accepts `hover`, `imu-loss`, | |
| 16 | `position-loss`, `vehicle-sensor-loss`, `host-loss`, `rc-override` and `kill`. Faults begin at 3000 ms, | |
| 17 | after takeoff. A report retains the actual `completed`, `blocked`, `host-lost` or `duration-exhausted` | |
| 18 | status. Exit 0 means the selected demonstration matched its expected outcome, observed flight, | |
| 19 | retained neutral pusher/surfaces and ended with neutral board outputs. It does not turn a fault | |
| 20 | into a completed mission. An unmatched outcome, invalid option or insufficient duration exits 1. | |
| 21 | ||
| 22 | The copied standalone CLI registers the same command definition: | |
| 23 | ||
| 24 | ```sh | |
| 25 | bun run runtime:cli | |
| 26 | ./storage/runtime-build/cli/drone vtol:hover | |
| 27 | ``` | |
| 28 | ||
| 29 | ## Model and measurements | |
| 30 | ||
| 31 | The fictional 2.5 kg body has four vertical rotors, each limited to 12 N, with 60 ms motor response | |
| 32 | and a 120 N/s slew limit. An independent 14 N pusher and the owned trainer's aerodynamic surfaces | |
| 33 | share the rigid body. Rotor allocation preserves collective thrust first, then roll/pitch demand, | |
| 34 | then yaw. No coefficient is measured from a purchased airframe. | |
| 35 | ||
| 36 | The hover demonstration commands the pusher and three surfaces to zero. Calibrated modeled raw | |
| 37 | IMU/GNSS samples feed the owned estimator. The controller receives estimated pose, a current qualified | |
| 38 | raw gyro capture and delayed contact measurements. Plant truth is used for sensor synthesis and | |
| 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 | |
| 41 | contact; mission completion still requires estimated arrival, low speed, settling and fresh contact. | |
| 42 | ||
| 43 | Contact and paused-ground neutral decisions occur after estimator/rate validation. A stale gyro | |
| 44 | cannot become acceptable because contact is present. Control faults latch for the controller | |
| 45 | lifetime. Each session owns its plant, controller, sensor sequence and returned actuator arrays. | |
| 46 | RC takeover, kill and sensor loss revoke shared authority. Host death leaves its final host snapshot | |
| 47 | stale while the independently ticking modeled board expires the output lease. | |
| 48 | ||
| 49 | The mission's 50 Wh capacity, 1200 W ceiling and 30% reserve are declared budget inputs for this | |
| 50 | fictional hover experiment. They are not identified electrical power, measured endurance or a | |
| 51 | complete hybrid energy model. Neutral outputs during an airborne fault do not establish a safe | |
| 52 | emergency landing. Physical flight commands remain disabled. | |
| 53 | ||
| 54 | ## Remaining implementation | |
| 55 | ||
| 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 | |
| 58 | qualification, bounded transition abort/recovery, complete hybrid power/reserve accounting and | |
| 59 | the common mission workspace. Full TECS/soaring, tiltrotor/tailsitter mechanisms, helicopters and | |
| 60 | other airframe families remain separate work. Native board control, measured airframe identification, | |
| 61 | HIL and physical flight require their own evidence. | |
| 62 | ||
| 63 | Read the [shared vehicle contract](vehicles.md), [fixed-wing model](vehicles-air.md) and | |
| 64 | [authority contract](authority.md). | |
| @@ -61,7 +61,9 @@ recompute mission replay, reject altered traces and exercise payload uncertainty | ||
| 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 | 63 | vehicle acceptance remain separate gates. VTOL, helicopter, soaring, blimp and other vehicle |
| 64 | families remain explicit roadmap work. | |
| 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. | |
| 65 | 67 | |
| 66 | 68 | Read [air](vehicles-air.md), [ground](vehicles-ground.md), [surface](vehicles-surface.md), [subsea](vehicles-subsea.md), |
| 67 | 69 | [mission semantics](missions.md) and [the replacement matrix](roadmap/feature-coverage.md). |
| @@ -1,78 +1,10 @@ | ||
| 1 | import type { EstimatorSnapshot } from '../estimation/estimator' | |
| 2 | import type { SensorVector } from '../sensors/types' | |
| 3 | import type { Motors } from './model' | |
| 4 | import { multiplyQuaternion, quaternionFromTilt } from '../estimation/frames' | |
| 1 | import { createSimulatedLiftController } from './lift-controller' | |
| 5 | 2 | import { allocateQuadX } from './allocation' |
| 6 | import { createBoundedPid } from './pid' | |
| 7 | import { clampControl, multirotorModel as model, validVector } from './model' | |
| 8 | export interface SimulatedControlInput { | |
| 9 | kind: 'simulation' | |
| 10 | enabled: boolean | |
| 11 | nowMs: number | |
| 12 | estimate: EstimatorSnapshot | |
| 13 | angularRateRadS: SensorVector | |
| 14 | rateCapturedAtMs: number | |
| 15 | target: { positionM: SensorVector, yawRad: number, issuedAtMs: number } | |
| 16 | } | |
| 17 | /** No vehicle adapter or IO. The initial heading relationship is a declared simulation assumption. */ | |
| 3 | import { multirotorModel as model } from './model' | |
| 4 | ||
| 5 | export type { SimulatedControlInput, SimulatedControlOutput } from './lift-controller' | |
| 6 | ||
| 7 | /** Original quad-X model and tuning, on the shared simulation-only lift cascade. */ | |
| 18 | 8 | export function createSimulatedMultirotorController(options: { headingAlignment: 'initial-simulation-north' }) { |
| 19 | if (options.headingAlignment !== 'initial-simulation-north') throw new TypeError('An explicit simulation heading alignment is required') | |
| 20 | 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 })) | |
| 21 | const ratePid = [0, 1, 2].map(i => createBoundedPid({ kp: i === 2 ? 0.08 : 0.12, ki: i === 2 ? 0.04 : 0.08, kd: i === 2 ? 0.001 : 0.002, integralLimit: 0.12, outputLimit: i === 2 ? 0.25 : 0.6, derivativeTauS: 0.03, trackingGain: 4 })) | |
| 22 | let previous: Motors = [0, 0, 0, 0], lastNow: number | null = null, reference: string | null = null, fault: string | null = null | |
| 23 | function clear() { for (const pid of [...velocityPid, ...ratePid]) pid.reset(); previous = [0, 0, 0, 0] } | |
| 24 | function inhibit(reason: string, latch = reference !== null) { | |
| 25 | if (latch) fault = reason | |
| 26 | clear() | |
| 27 | return { kind: 'simulated-control' as const, atMs: lastNow, inhibited: true, reason, physicalFlightEnabled: false as const, boardOutputsInhibited: true as const, motorsN: [...previous] as Motors, limited: false, desiredVelocityMps: null, desiredAccelerationMps2: null, desiredRatesRadS: null, desiredWrench: null, achievedWrench: null, integrals: { velocity: velocityPid.map(p => p.state().integral), rate: ratePid.map(p => p.state().integral) } } | |
| 28 | } | |
| 29 | return { | |
| 30 | reset() { clear(); lastNow = null; reference = null; fault = null }, | |
| 31 | step(input: SimulatedControlInput) { | |
| 32 | if (input.kind !== 'simulation') return inhibit('simulation-only', true) | |
| 33 | const now = input.nowMs | |
| 34 | if (!Number.isFinite(now) || (lastNow !== null && now <= lastNow)) return inhibit('control-clock-discontinuity', true) | |
| 35 | const dt = lastNow === null ? 0.02 : (now - lastNow) / 1000 | |
| 36 | lastNow = now | |
| 37 | if (!input.enabled) { reference = null; fault = null; return inhibit('disabled', false) } | |
| 38 | if (fault) return inhibit(fault, true) | |
| 39 | if (dt < 0.005 || dt > 0.04) return inhibit('control-schedule-gap', true) | |
| 40 | const e = input.estimate, attitude = e?.attitude, position = e?.localPosition, target = input.target | |
| 41 | if (!e || typeof e.bootId !== 'bigint' || e.bootId <= 0n || !Object.values(e.resets).every(n => Number.isInteger(n) && n >= 0) || e.evidence !== 'fixture' || e.physicalFlightEnabled !== false || !e.health.tilt || !e.health.localPosition || !attitude || !position) return inhibit('estimator-unhealthy') | |
| 42 | if (!Number.isFinite(e.nowMs) || Math.abs(e.nowMs - now) > 1e-6 || attitude.capturedAtMs === null || !Number.isFinite(attitude.capturedAtMs) || attitude.capturedAtMs > now || now - attitude.capturedAtMs > 60 | |
| 43 | || position.capturedAtMs === null || !Number.isFinite(position.capturedAtMs) || !Number.isFinite(position.predictedToMs) || position.predictedToMs > now || now - position.predictedToMs > 40 || position.capturedAtMs > now || now - position.capturedAtMs > 200 || !Number.isFinite(input.rateCapturedAtMs) || input.rateCapturedAtMs > now || now - input.rateCapturedAtMs > 60) return inhibit('stale-control-observation') | |
| 44 | if (attitude.reference !== 'GRAVITY_ALIGNED_RELATIVE_YAW' || position.frame !== 'LOCAL_NED' || position.altitudeDatum !== 'WGS84_ELLIPSOID' || !validVector(position.positionM) || !validVector(position.velocityMps) || !validVector(input.angularRateRadS) | |
| 45 | || !position.origin || ![position.origin.latitudeDeg, position.origin.longitudeDeg, position.origin.ellipsoidHeightM].every(Number.isFinite) || Math.hypot(...input.angularRateRadS) > 35 || attitude.quaternionWxyz.length !== 4 || !attitude.quaternionWxyz.every(Number.isFinite) || Math.abs(Math.hypot(...attitude.quaternionWxyz) - 1) > 1e-6) return inhibit('invalid-control-frame-or-values') | |
| 46 | if (Math.abs(input.rateCapturedAtMs - attitude.capturedAtMs) > 1e-6) return inhibit('unmatched-rate-epoch') | |
| 47 | if (!target || !validVector(target.positionM) || Math.hypot(...target.positionM) > 100 || !Number.isFinite(target.yawRad) || Math.abs(target.yawRad) > Math.PI || !Number.isFinite(target.issuedAtMs) || target.issuedAtMs > now || now - target.issuedAtMs > 200) return inhibit('invalid-or-stale-target') | |
| 48 | const identity = JSON.stringify([String(e.bootId), e.resets, position.origin]) | |
| 49 | if (reference !== null && identity !== reference) return inhibit('estimator-reference-reset', true) | |
| 50 | reference = identity | |
| 51 | const velocity = target.positionM.map((n, i) => n - position.positionM[i]!) as SensorVector | |
| 52 | const horizontal = Math.hypot(velocity[0], velocity[1]) | |
| 53 | if (horizontal > 3) { velocity[0] *= 3 / horizontal; velocity[1] *= 3 / horizontal } | |
| 54 | velocity[2] = clampControl(velocity[2], 2) | |
| 55 | const acceleration = velocity.map((n, i) => velocityPid[i]!.step(n - position.velocityMps[i]!, position.velocityMps[i]!, dt)) as SensorVector | |
| 56 | const beforeLimit = [...acceleration] | |
| 57 | const horizontalAcceleration = Math.hypot(acceleration[0], acceleration[1]), tiltBound = Math.tan(25 * Math.PI / 180) * (model.gravityMps2 - acceleration[2]) | |
| 58 | if (horizontalAcceleration > Math.min(3, tiltBound)) { | |
| 59 | const scale = Math.min(3, tiltBound) / horizontalAcceleration | |
| 60 | acceleration[0] *= scale; acceleration[1] *= scale | |
| 61 | } | |
| 62 | for (let i = 0; i < 3; i++) velocityPid[i]!.track(acceleration[i]! - beforeLimit[i]!, dt) | |
| 63 | const yaw = target.yawRad, ax = Math.cos(yaw) * acceleration[0] + Math.sin(yaw) * acceleration[1], ay = -Math.sin(yaw) * acceleration[0] + Math.cos(yaw) * acceleration[1] | |
| 64 | const vertical = model.gravityMps2 - acceleration[2] | |
| 65 | const desired = multiplyQuaternion([Math.cos(yaw / 2), 0, 0, Math.sin(yaw / 2)], quaternionFromTilt(Math.atan2(ay, Math.hypot(ax, vertical)), Math.atan2(-ax, vertical))) | |
| 66 | const q = attitude.quaternionWxyz | |
| 67 | const error = multiplyQuaternion([q[0], -q[1], -q[2], -q[3]], desired) | |
| 68 | const rates = error.slice(1).map((n, i) => clampControl(8 * n * (error[0] < 0 ? -1 : 1), i === 2 ? 1.5 : 3)) as SensorVector | |
| 69 | const torque = rates.map((n, i) => ratePid[i]!.step(n - input.angularRateRadS[i]!, input.angularRateRadS[i]!, dt)) as SensorVector | |
| 70 | const desiredWrench = { thrustN: model.massKg * Math.hypot(...[acceleration[0], acceleration[1], vertical]), torqueNm: torque } | |
| 71 | const allocation = allocateQuadX(desiredWrench, previous, dt) | |
| 72 | for (let i = 0; i < 3; i++) ratePid[i]!.track(allocation.achieved.torqueNm[i]! - torque[i]!, dt) | |
| 73 | previous = allocation.motorsN | |
| 74 | return { kind: 'simulated-control' as const, atMs: lastNow, inhibited: false, reason: 'simulation-feedback', physicalFlightEnabled: false as const, boardOutputsInhibited: true as const, motorsN: [...previous] as Motors, limited: allocation.limited, desiredVelocityMps: velocity, desiredAccelerationMps2: acceleration, desiredRatesRadS: rates, desiredWrench, achievedWrench: allocation.achieved, integrals: { velocity: velocityPid.map(p => p.state().integral), rate: ratePid.map(p => p.state().integral) } } | |
| 75 | }, | |
| 76 | } | |
| 9 | return createSimulatedLiftController(options, { massKg: model.massKg, gravityMps2: model.gravityMps2, rateScale: [1, 1, 1], allocate: allocateQuadX }) | |
| 77 | 10 | } |
| 78 | export type SimulatedControlOutput = ReturnType<ReturnType<typeof createSimulatedMultirotorController>['step']> | |
| @@ -1,10 +1,9 @@ | ||
| 1 | 1 | import type { EstimatorSnapshot } from '../estimation/estimator' |
| 2 | 2 | import type { SensorVector } from '../sensors/types' |
| 3 | import type { Motors } from './model' | |
| 3 | import type { Motors, Wrench } from './model' | |
| 4 | 4 | import { multiplyQuaternion, quaternionFromTilt } from '../estimation/frames' |
| 5 | import { allocateQuadX } from './allocation' | |
| 6 | 5 | import { createBoundedPid } from './pid' |
| 7 | import { clampControl, multirotorModel as model, validVector } from './model' | |
| 6 | import { clampControl, validVector } from './model' | |
| 8 | 7 | export interface SimulatedControlInput { |
| 9 | 8 | kind: 'simulation' |
| 10 | 9 | enabled: boolean |
| @@ -14,11 +13,15 @@ export interface SimulatedControlInput { | ||
| 14 | 13 | rateCapturedAtMs: number |
| 15 | 14 | target: { positionM: SensorVector, yawRad: number, issuedAtMs: number } |
| 16 | 15 | } |
| 17 | /** No vehicle adapter or IO. The initial heading relationship is a declared simulation assumption. */ | |
| 18 | export function createSimulatedMultirotorController(options: { headingAlignment: 'initial-simulation-north' }) { | |
| 16 | interface LiftProfile { massKg: number, gravityMps2: number, rateScale: SensorVector, allocate(wrench: Wrench, previous: Motors, dtS: number): { motorsN: Motors, achieved: Wrench, limited: boolean } } | |
| 17 | /** Shared simulated lift cascade. Vehicle wrappers own mass, tuning and allocation; no vehicle IO. */ | |
| 18 | export function createSimulatedLiftController(options: { headingAlignment: 'initial-simulation-north' }, profile: LiftProfile) { | |
| 19 | 19 | if (options.headingAlignment !== 'initial-simulation-north') throw new TypeError('An explicit simulation heading alignment is required') |
| 20 | const model = { massKg: profile.massKg, gravityMps2: profile.gravityMps2 }, allocate = profile.allocate | |
| 21 | const rateScale = Array.isArray(profile.rateScale) ? [...profile.rateScale] : [] | |
| 22 | if (!Number.isFinite(model.massKg) || model.massKg <= 0 || !Number.isFinite(model.gravityMps2) || model.gravityMps2 <= 0 || !validVector(rateScale) || rateScale.some(n => n <= 0) || typeof allocate !== 'function') throw new RangeError('Invalid simulated lift profile') | |
| 20 | 23 | 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 })) |
| 21 | const ratePid = [0, 1, 2].map(i => createBoundedPid({ kp: i === 2 ? 0.08 : 0.12, ki: i === 2 ? 0.04 : 0.08, kd: i === 2 ? 0.001 : 0.002, integralLimit: 0.12, outputLimit: i === 2 ? 0.25 : 0.6, derivativeTauS: 0.03, trackingGain: 4 })) | |
| 24 | const ratePid = [0, 1, 2].map(i => createBoundedPid({ kp: (i === 2 ? 0.08 : 0.12) * rateScale[i]!, ki: (i === 2 ? 0.04 : 0.08) * rateScale[i]!, kd: (i === 2 ? 0.001 : 0.002) * rateScale[i]!, integralLimit: 0.12 * rateScale[i]!, outputLimit: (i === 2 ? 0.25 : 0.6) * rateScale[i]!, derivativeTauS: 0.03, trackingGain: 4 })) | |
| 22 | 25 | let previous: Motors = [0, 0, 0, 0], lastNow: number | null = null, reference: string | null = null, fault: string | null = null |
| 23 | 26 | function clear() { for (const pid of [...velocityPid, ...ratePid]) pid.reset(); previous = [0, 0, 0, 0] } |
| 24 | 27 | function inhibit(reason: string, latch = reference !== null) { |
| @@ -68,11 +71,11 @@ export function createSimulatedMultirotorController(options: { headingAlignment: | ||
| 68 | 71 | const rates = error.slice(1).map((n, i) => clampControl(8 * n * (error[0] < 0 ? -1 : 1), i === 2 ? 1.5 : 3)) as SensorVector |
| 69 | 72 | const torque = rates.map((n, i) => ratePid[i]!.step(n - input.angularRateRadS[i]!, input.angularRateRadS[i]!, dt)) as SensorVector |
| 70 | 73 | const desiredWrench = { thrustN: model.massKg * Math.hypot(...[acceleration[0], acceleration[1], vertical]), torqueNm: torque } |
| 71 | const allocation = allocateQuadX(desiredWrench, previous, dt) | |
| 74 | const allocation = allocate(desiredWrench, previous, dt) | |
| 72 | 75 | for (let i = 0; i < 3; i++) ratePid[i]!.track(allocation.achieved.torqueNm[i]! - torque[i]!, dt) |
| 73 | 76 | previous = allocation.motorsN |
| 74 | 77 | return { kind: 'simulated-control' as const, atMs: lastNow, inhibited: false, reason: 'simulation-feedback', physicalFlightEnabled: false as const, boardOutputsInhibited: true as const, motorsN: [...previous] as Motors, limited: allocation.limited, desiredVelocityMps: velocity, desiredAccelerationMps2: acceleration, desiredRatesRadS: rates, desiredWrench, achievedWrench: allocation.achieved, integrals: { velocity: velocityPid.map(p => p.state().integral), rate: ratePid.map(p => p.state().integral) } } |
| 75 | 78 | }, |
| 76 | 79 | } |
| 77 | 80 | } |
| 78 | export type SimulatedControlOutput = ReturnType<ReturnType<typeof createSimulatedMultirotorController>['step']> | |
| 81 | export type SimulatedControlOutput = ReturnType<ReturnType<typeof createSimulatedLiftController>['step']> | |
| @@ -15,7 +15,7 @@ export function vehicleMissionContains(plan: VehicleMissionPlan, p: unknown) { | ||
| 15 | 15 | export function vehicleTaskTarget(plan: VehicleMissionPlan, task: VehicleMissionTask) { return task.kind === 'return' ? [...plan.rallyM] : 'positionM' in task ? [...task.positionM] : null } |
| 16 | 16 | export function validateVehicleMission(input: unknown): { plan: VehicleMissionPlan, worstCaseMs: number, requiredWh: number } { |
| 17 | 17 | const invalid = (): never => { throw new TypeError('Invalid vehicle mission or unsupported profile') } |
| 18 | if (!object(input) || !keys(input, ['version', 'vehicle', 'id', 'frame', 'altitude', 'terrain', 'origin', 'fence', 'rallyM', 'power', 'start', 'tasks']) || input.version !== 2 || !['fixed-wing', 'ground', 'surface', 'subsea'].includes(input.vehicle as string) || !id(input.id) || input.frame !== 'LOCAL_NED' || input.altitude !== 'ORIGIN_RELATIVE_DOWN' || input.terrain !== 'DECLARED_MODEL_SURFACE' || !id(input.start)) invalid() | |
| 18 | if (!object(input) || !keys(input, ['version', 'vehicle', 'id', 'frame', 'altitude', 'terrain', 'origin', 'fence', 'rallyM', 'power', 'start', 'tasks']) || input.version !== 2 || !['fixed-wing', 'vtol', 'ground', 'surface', 'subsea'].includes(input.vehicle as string) || !id(input.id) || input.frame !== 'LOCAL_NED' || input.altitude !== 'ORIGIN_RELATIVE_DOWN' || input.terrain !== 'DECLARED_MODEL_SURFACE' || !id(input.start)) invalid() | |
| 19 | 19 | const p = input as unknown as VehicleMissionPlan |
| 20 | 20 | if (!object(p.origin) || !keys(p.origin, ['latitudeDeg', 'longitudeDeg', 'ellipsoidHeightM']) || !number(p.origin.latitudeDeg, -90, 90) || !number(p.origin.longitudeDeg, -180, 180) || !number(p.origin.ellipsoidHeightM, -500, 10000) |
| 21 | 21 | || !object(p.fence) || !keys(p.fence, ['radiusM', 'minDownM', 'maxDownM']) || !number(p.fence.radiusM, 1, 5000) || !number(p.fence.minDownM, -1000, 0) || !number(p.fence.maxDownM, 0, 10) || p.fence.minDownM >= p.fence.maxDownM || !vehicleMissionContains(p, p.rallyM) |
| @@ -33,7 +33,7 @@ export function validateVehicleMission(input: unknown): { plan: VehicleMissionPl | ||
| 33 | 33 | } |
| 34 | 34 | else if (t.kind === 'payload') { |
| 35 | 35 | fields.push('operation', 'containerId', 'timeoutMs', 'next') |
| 36 | const supported = p.vehicle === 'fixed-wing' ? ['camera-still'] : p.vehicle === 'ground' ? ['camera-still', 'material-drop', 'mower-enable', 'mower-disable'] : p.vehicle === 'surface' ? ['camera-still', 'water-sample'] : ['camera-still', 'inspection-capture'] | |
| 36 | const supported = p.vehicle === 'fixed-wing' || p.vehicle === 'vtol' ? ['camera-still'] : p.vehicle === 'ground' ? ['camera-still', 'material-drop', 'mower-enable', 'mower-disable'] : p.vehicle === 'surface' ? ['camera-still', 'water-sample'] : ['camera-still', 'inspection-capture'] | |
| 37 | 37 | if (!supported.includes(t.operation) || (t.containerId !== null && !id(t.containerId)) || (t.operation === 'water-sample' && !id(t.containerId)) || !number(t.timeoutMs, 100, 10000) || !id(t.next)) invalid() |
| 38 | 38 | durationMs = t.timeoutMs; edges = [t.next] |
| 39 | 39 | } |
| @@ -41,8 +41,8 @@ export function validateVehicleMission(input: unknown): { plan: VehicleMissionPl | ||
| 41 | 41 | fields = [...fields, 'acceptanceM', 'maxArrivalSpeedMps', 'settleMs', 'timeoutMs', 'next'] |
| 42 | 42 | if (t.kind !== 'return') { fields.push('positionM'); if (!vehicleMissionContains(p, (t as { positionM?: unknown }).positionM)) invalid() } |
| 43 | 43 | if (!('acceptanceM' in t) || !number(t.acceptanceM, 0.05, 150) || !number(t.maxArrivalSpeedMps, 0.05, 50) || !number(t.settleMs, 0, 10000) || !number(t.timeoutMs, 1000, 60000) || t.settleMs >= t.timeoutMs) invalid() |
| 44 | if ((t.kind === 'takeoff' || t.kind === 'land') && p.vehicle !== 'fixed-wing') invalid() | |
| 45 | if (t.kind === 'stop' && (p.vehicle === 'fixed-wing' || t.maxArrivalSpeedMps > 0.5)) invalid() | |
| 44 | if ((t.kind === 'takeoff' || t.kind === 'land') && p.vehicle !== 'fixed-wing' && p.vehicle !== 'vtol') invalid() | |
| 45 | if (t.kind === 'stop' && (p.vehicle === 'fixed-wing' || p.vehicle === 'vtol' || t.maxArrivalSpeedMps > 0.5)) invalid() | |
| 46 | 46 | if (t.kind === 'land' && t.maxArrivalSpeedMps > 2) invalid() |
| 47 | 47 | const terminal = t.kind === 'land' || t.kind === 'stop' |
| 48 | 48 | if (!terminal && !id(t.next)) invalid() |
| @@ -1,5 +1,5 @@ | ||
| 1 | 1 | import type { EstimatorSnapshot } from '../estimation/estimator' |
| 2 | import type { SensorVector } from '../sensors/types' | |
| 2 | import type { ImuMeasurement, SensorVector } from '../sensors/types' | |
| 3 | 3 | import type { VehicleControlOutput, VehicleControllerContext, VehicleMeasurement, VehicleMeasurementPose, VehicleProfile } from './types' |
| 4 | 4 | import { createPhysicsSensorFeed } from '../controls/sensors' |
| 5 | 5 | import { createEstimator } from '../estimation/estimator' |
| @@ -56,7 +56,8 @@ export function runVehicleSimulation(profileInput: VehicleProfile, options: { sc | ||
| 56 | 56 | estimate = navigation ? navigation.update(baseEstimate, measurement, nowMs) : baseEstimate |
| 57 | 57 | if (nowMs % 20 !== 0) continue |
| 58 | 58 | const reference = vehicleReference(estimate) |
| 59 | const capturePose = predictVehicleMeasurementPose(estimate, sensors.sensors.find(s => s.kind === 'imu')?.observation, nowMs) | |
| 59 | const imuObservation = sensors.sensors.find(s => s.kind === 'imu')?.observation | |
| 60 | const capturePose = predictVehicleMeasurementPose(estimate, imuObservation, nowMs) | |
| 60 | 61 | if ((capturePose || profileInput.vehicle === 'subsea') && !faults.dropVehicleSensor) { |
| 61 | 62 | if (queue.length >= 8) throw new RangeError('Vehicle sensor queue exhausted') |
| 62 | 63 | const packet = structuredClone(session.renderMeasurement(truth, nowMs, reference)) |
| @@ -83,7 +84,9 @@ export function runVehicleSimulation(profileInput: VehicleProfile, options: { sc | ||
| 83 | 84 | } |
| 84 | 85 | supervisor.update(observation) |
| 85 | 86 | if (nowMs === 600) { send('arm'); send('engage', [0, 0, 0]) } |
| 86 | const context: VehicleControllerContext = { nowMs, enabled: supervisor.snapshot().state === 'autonomous', estimate, measurement, measurementPose, mission } | |
| 87 | const context: VehicleControllerContext = { nowMs, enabled: supervisor.snapshot().state === 'autonomous', estimate, measurement, measurementPose, mission, | |
| 88 | ...(profileInput.vehicle === 'vtol' ? { controlRate: capturePose ? { evidence: 'modeled-qualified-imu' as const, reference: capturePose.reference, capturedAtMs: capturePose.gyroCapturedAtMs, angularRateRadS: [...(imuObservation!.value as ImuMeasurement).angularRateRadS] as SensorVector } : null } : {}), | |
| 89 | } | |
| 87 | 90 | payload?.observe?.(structuredClone(context)) |
| 88 | 91 | const ack = payload?.poll(nowMs) ?? null |
| 89 | 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 } |
| @@ -3,7 +3,7 @@ import type { GeodeticOrigin, Quaternion } from '../estimation/frames' | ||
| 3 | 3 | import type { SensorVector } from '../sensors/types' |
| 4 | 4 | import type { MissionAction, MissionObservation, MissionState } from '../missions/engine' |
| 5 | 5 | |
| 6 | export type VehicleKind = 'fixed-wing' | 'ground' | 'surface' | 'subsea' | |
| 6 | export type VehicleKind = 'fixed-wing' | 'vtol' | 'ground' | 'surface' | 'subsea' | |
| 7 | 7 | export interface VehicleActuator { name: string, unit: 'normalized' | 'newton' | 'radian' | 'metres-per-second', min: number, max: number, neutral: number } |
| 8 | 8 | export interface VehiclePlantState { |
| 9 | 9 | positionM: SensorVector |
| @@ -80,7 +80,9 @@ export interface VehicleControlOutput { | ||
| 80 | 80 | boardOutputsInhibited: true |
| 81 | 81 | } |
| 82 | 82 | export interface VehicleMeasurementPose { capturedAtMs: number, reference: string, positionM: SensorVector, quaternionWxyz: Quaternion, positionCapturedAtMs: number, positionPredictedToMs: number, attitudeCapturedAtMs: number, attitudePredictedToMs: number, gyroCapturedAtMs: number, attitudePrediction: 'qualified-gyro-hold' } |
| 83 | export interface VehicleControllerContext { nowMs: number, enabled: boolean, estimate: EstimatorSnapshot, measurement: VehicleMeasurement | null, measurementPose?: VehicleMeasurementPose | null, mission: VehicleMissionSnapshot } | |
| 83 | /** Current calibrated gyro from the same qualified raw IMU capture as estimator attitude. */ | |
| 84 | export interface VehicleControlRate { evidence: 'modeled-qualified-imu', reference: string, capturedAtMs: number, angularRateRadS: SensorVector } | |
| 85 | export interface VehicleControllerContext { nowMs: number, enabled: boolean, estimate: EstimatorSnapshot, measurement: VehicleMeasurement | null, measurementPose?: VehicleMeasurementPose | null, controlRate?: VehicleControlRate | null, mission: VehicleMissionSnapshot } | |
| 84 | 86 | export interface VehicleController { step(context: VehicleControllerContext): VehicleControlOutput } |
| 85 | 87 | export interface VehicleFaults { |
| 86 | 88 | windMps?: SensorVector |
| @@ -0,0 +1,28 @@ | ||
| 1 | import type { Motors, Wrench } from '../../controls/model' | |
| 2 | import { validVector } from '../../controls/model' | |
| 3 | import { vtolModel as model } from './plant' | |
| 4 | import { numericPacket } from './packets' | |
| 5 | ||
| 6 | /** Front-left, front-right, rear-right, rear-left; BODY_FRD moments. */ | |
| 7 | export function allocateVtolLift(wrench: Wrench) { | |
| 8 | if (!wrench || !Array.isArray(wrench.torqueNm)) throw new RangeError('Invalid VTOL lift wrench') | |
| 9 | const thrustN = wrench.thrustN, torqueNm = numericPacket(wrench.torqueNm, 3) | |
| 10 | if (!Number.isFinite(thrustN) || !validVector(torqueNm)) throw new RangeError('Invalid VTOL lift wrench') | |
| 11 | const t = Math.max(0, Math.min(model.maxRotorThrustN, thrustN / 4)), r = torqueNm[0] / (4 * model.rotorArmProjectionM), p = torqueNm[1] / (4 * model.rotorArmProjectionM), y = torqueNm[2] / (4 * model.reactionTorqueM) | |
| 12 | function addWithinBounds(base: number[], differential: number[]) { | |
| 13 | let scale = 1 | |
| 14 | for (let i = 0; i < 4; i++) { | |
| 15 | const change = differential[i]! | |
| 16 | if (change > 0) scale = Math.min(scale, (model.maxRotorThrustN - base[i]!) / change) | |
| 17 | else if (change < 0) scale = Math.min(scale, -base[i]! / change) | |
| 18 | } | |
| 19 | return base.map((n, i) => Math.max(0, Math.min(model.maxRotorThrustN, n + scale * differential[i]!))) | |
| 20 | } | |
| 21 | // Preserve collective first, then the roll/pitch vector, then yaw. Report the achieved wrench for anti-windup. | |
| 22 | const rollPitch = addWithinBounds([t, t, t, t], [r + p, -r + p, -r - p, r - p]) | |
| 23 | const rotorsN = addWithinBounds(rollPitch, [y, -y, y, -y]) as Motors | |
| 24 | const [a, b, c, d] = rotorsN | |
| 25 | const achieved: Wrench = { thrustN: a + b + c + d, torqueNm: [model.rotorArmProjectionM * (a - b - c + d), model.rotorArmProjectionM * (a + b - c - d), model.reactionTorqueM * (a - b + c - d)] } | |
| 26 | const limited = Math.abs(achieved.thrustN - thrustN) > 1e-9 || achieved.torqueNm.some((n, i) => Math.abs(n - torqueNm[i]!) > 1e-9) | |
| 27 | return { rotorsN, achieved, limited } | |
| 28 | } | |
| @@ -0,0 +1,33 @@ | ||
| 1 | import type { VehicleController, VehicleControllerContext, VehicleControlOutput } from '../types' | |
| 2 | import { createVehicleMeasurementQualifier, vehicleReference } from '../measurements' | |
| 3 | import { createVtolHoverController } from './hover-controller' | |
| 4 | ||
| 5 | /** Hover-only adapter on the shared mission/authority runtime. No wing transition policy. */ | |
| 6 | export function createVtolVehicleHoverController(): VehicleController { | |
| 7 | const hover = createVtolHoverController({ headingAlignment: 'initial-simulation-north' }), qualifier = createVehicleMeasurementQualifier('vtol') | |
| 8 | let fault = '', hasFlown = false | |
| 9 | function output(c: VehicleControllerContext, actuators: number[], reason: string, inhibited: boolean): VehicleControlOutput { | |
| 10 | return { kind: 'simulated-vehicle-control', vehicle: 'vtol', atMs: c.nowMs, actuators, reason, inhibited, physicalFlightEnabled: false, boardOutputsInhibited: true } | |
| 11 | } | |
| 12 | return { | |
| 13 | step(c) { | |
| 14 | if (!c.enabled || fault) return output(c, [0, 0, 0, 0, 0, 0, 0, 0], fault || 'authority-disabled', true) | |
| 15 | let measurement | |
| 16 | try { measurement = qualifier.qualify(c.measurement, c.nowMs, vehicleReference(c.estimate)) } | |
| 17 | catch { fault = 'vtol-sensor-unqualified' } | |
| 18 | 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 | if (fault) return output(c, [0, 0, 0, 0, 0, 0, 0, 0], fault, true) | |
| 20 | if (measurement!.values.onSurface === false) hasFlown = true | |
| 21 | 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 | // A declared 0.1 m plane intercept permits finite contact rather than asymptotic height holding. | |
| 25 | 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 | |
| 28 | 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 | if (!result.inhibited && landing && hasFlown && measurement!.values.onSurface === true) return output(c, [0, 0, 0, 0, 0, 0, 0, 0], 'vertical-contact', false) | |
| 30 | return output(c, [...result.motorsN, 0, 0, 0, 0], result.reason, result.inhibited) | |
| 31 | }, | |
| 32 | } | |
| 33 | } | |
| @@ -0,0 +1,16 @@ | ||
| 1 | import { createSimulatedLiftController } from '../../controls/lift-controller' | |
| 2 | import { allocateVtolLift } from './allocation' | |
| 3 | import { vtolModel as model } from './plant' | |
| 4 | ||
| 5 | /** Fictional hover tuning scaled for the declared diagonal inertia. No transition or physical control. */ | |
| 6 | export function createVtolHoverController(options: { headingAlignment: 'initial-simulation-north' }) { | |
| 7 | return createSimulatedLiftController(options, { | |
| 8 | massKg: model.massKg, | |
| 9 | gravityMps2: model.gravityMps2, | |
| 10 | rateScale: [10, 40 / 3, 12], | |
| 11 | allocate(wrench) { | |
| 12 | const result = allocateVtolLift(wrench) | |
| 13 | return { motorsN: result.rotorsN, achieved: result.achieved, limited: result.limited } | |
| 14 | }, | |
| 15 | }) | |
| 16 | } | |
| @@ -0,0 +1,49 @@ | ||
| 1 | import type { VehicleProfile } from '../types' | |
| 2 | import { createVtolPlant, vtolModel } from './plant' | |
| 3 | import { createVtolVehicleHoverController } from './controller' | |
| 4 | export const vtolHoverScenarios = Object.freeze(['hover', 'imu-loss', 'position-loss', 'vehicle-sensor-loss', 'host-loss', 'rc-override', 'kill'] as const) | |
| 5 | ||
| 6 | /** Development hover mission only. Front/cruise/back transitions remain unimplemented. */ | |
| 7 | export function createVtolHoverProfile(scenario = 'hover'): VehicleProfile { | |
| 8 | if (!(vtolHoverScenarios as readonly string[]).includes(scenario)) throw new RangeError('Unknown VTOL hover scenario') | |
| 9 | return { | |
| 10 | scenario, vehicle: 'vtol', name: 'Owned VTOL hover development', evidence: 'modeled-vehicle-profile', physicalFlightEnabled: false, | |
| 11 | actuators: [ | |
| 12 | ...['front-left', 'front-right', 'rear-right', 'rear-left'].map(name => ({ name: `lift-${name}`, unit: 'newton' as const, min: 0, max: vtolModel.maxRotorThrustN, neutral: 0 })), | |
| 13 | { name: 'pusher', unit: 'normalized', min: 0, max: 1, neutral: 0 }, | |
| 14 | ...['aileron', 'elevator', 'rudder'].map(name => ({ name, unit: 'radian' as const, min: -0.4, max: 0.4, neutral: 0 })), | |
| 15 | ], | |
| 16 | authorityFence: { radiusM: 100, minDownM: -15, maxDownM: 1 }, | |
| 17 | mission: { | |
| 18 | version: 2, vehicle: 'vtol', id: 'vtol-hover-development', frame: 'LOCAL_NED', altitude: 'ORIGIN_RELATIVE_DOWN', terrain: 'DECLARED_MODEL_SURFACE', | |
| 19 | origin: { latitudeDeg: 0, longitudeDeg: 0, ellipsoidHeightM: 100 }, fence: { radiusM: 100, minDownM: -15, maxDownM: 1 }, rallyM: [0, 0, -5], | |
| 20 | // Declared budget ceiling for this fictional hover case, not identified propulsion power. | |
| 21 | power: { availableWh: 50, maxW: 1200, reservePercent: 30 }, start: 'start', | |
| 22 | tasks: [ | |
| 23 | { id: 'start', kind: 'start', next: 'depart' }, | |
| 24 | { id: 'depart', kind: 'takeoff', positionM: [0, 0, -5], acceptanceM: 0.2, maxArrivalSpeedMps: 0.2, settleMs: 200, timeoutMs: 20000, next: 'hover-waypoint' }, | |
| 25 | { id: 'hover-waypoint', kind: 'waypoint', positionM: [2, 1, -5], acceptanceM: 0.2, maxArrivalSpeedMps: 0.2, settleMs: 200, timeoutMs: 20000, next: 'land' }, | |
| 26 | { id: 'land', kind: 'land', positionM: [2, 1, 0], acceptanceM: 0.3, maxArrivalSpeedMps: 0.2, settleMs: 200, timeoutMs: 20000 }, | |
| 27 | ], | |
| 28 | }, | |
| 29 | scenarios: vtolHoverScenarios, | |
| 30 | createSession() { | |
| 31 | return { createPlant: createVtolPlant, createController: createVtolVehicleHoverController, | |
| 32 | renderMeasurement(state, atMs, reference) { | |
| 33 | return { version: 1, evidence: 'modeled-vehicle-sensors', vehicle: 'vtol', sequence: Math.floor(atMs / 20) + 1, reference, capturedAtMs: atMs, receivedAtMs: atMs + 60, | |
| 34 | values: { onSurface: state.onSurface, stopped: Math.hypot(...state.velocityMps) <= 0.2, airspeedMps: Math.round((state.details.airspeedMps as number) * 1000) / 1000 } } | |
| 35 | }, | |
| 36 | } | |
| 37 | }, | |
| 38 | faults(selected, atMs) { | |
| 39 | if (atMs < 3000) return {} | |
| 40 | if (selected === 'imu-loss') return { dropImu: true } | |
| 41 | if (selected === 'position-loss') return { dropPosition: true } | |
| 42 | if (selected === 'vehicle-sensor-loss') return { dropVehicleSensor: true } | |
| 43 | if (selected === 'host-loss') return { hostLost: true } | |
| 44 | if (selected === 'rc-override') return { rcOverride: true } | |
| 45 | if (selected === 'kill') return { kill: true } | |
| 46 | return {} | |
| 47 | }, | |
| 48 | } | |
| 49 | } | |
| @@ -0,0 +1,11 @@ | ||
| 1 | /** Capture own numeric data without invoking array iterators or index accessors. */ | |
| 2 | export function numericPacket(value: unknown, length: number): number[] { | |
| 3 | if (!Array.isArray(value) || value.length !== length) throw new RangeError('Invalid VTOL numeric packet') | |
| 4 | const packet: number[] = [] | |
| 5 | for (let index = 0; index < length; index++) { | |
| 6 | const field = Object.getOwnPropertyDescriptor(value, String(index)) | |
| 7 | if (!field?.enumerable || !Object.hasOwn(field, 'value') || typeof field.value !== 'number' || !Number.isFinite(field.value)) throw new RangeError('Invalid VTOL numeric packet') | |
| 8 | packet.push(field.value) | |
| 9 | } | |
| 10 | return packet | |
| 11 | } | |
| @@ -0,0 +1,74 @@ | ||
| 1 | import type { SensorVector } from '../../sensors/types' | |
| 2 | import type { Quaternion } from '../../estimation/frames' | |
| 3 | import type { VehiclePlant, VehiclePlantState } from '../types' | |
| 4 | import { validVector } from '../../controls/model' | |
| 5 | import { multiplyQuaternion, normalizeQuaternion } from '../../estimation/frames' | |
| 6 | import { rotateBodyToNed, rotateNedToBody } from '../../controls/physics' | |
| 7 | import { airAerodynamics, airModel } from '../air/plant' | |
| 8 | import { numericPacket } from './packets' | |
| 9 | ||
| 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 }) | |
| 12 | ||
| 13 | /** Independent lift rotors and the owned trainer wing/pusher share one rigid body. */ | |
| 14 | export function createVtolPlant(initial: { positionM?: SensorVector, velocityMps?: SensorVector, quaternionWxyz?: Quaternion, rotorsN?: [number, number, number, number], pusherThrustN?: number } = {}): VehiclePlant { | |
| 15 | let positionM = numericPacket(initial.positionM === undefined ? [0, 0, 0] : initial.positionM, 3) as SensorVector | |
| 16 | let rotorsN = numericPacket(initial.rotorsN === undefined ? [0, 0, 0, 0] : initial.rotorsN, 4) | |
| 17 | let pusherThrustN = initial.pusherThrustN === undefined ? 0 : initial.pusherThrustN | |
| 18 | let velocityMps = numericPacket(initial.velocityMps === undefined ? [0, 0, 0] : initial.velocityMps, 3) as SensorVector, accelerationMps2: SensorVector = [0, 0, 0] | |
| 19 | let windMps: SensorVector = [0, 0, 0] | |
| 20 | let quaternionWxyz = normalizeQuaternion(numericPacket(initial.quaternionWxyz === undefined ? [1, 0, 0, 0] : initial.quaternionWxyz, 4) as Quaternion), angularRateRadS: SensorVector = [0, 0, 0] | |
| 21 | if (!validVector(positionM) || !validVector(velocityMps) || positionM[2] > 0 || rotorsN.length !== 4 || rotorsN.some(n => !Number.isFinite(n) || n < 0 || n > vtolModel.maxRotorThrustN) || !Number.isFinite(pusherThrustN) || pusherThrustN < 0 || pusherThrustN > vtolModel.maxPusherThrustN) throw new RangeError('Invalid initial VTOL model') | |
| 22 | let onSurface = positionM[2] === 0 | |
| 23 | function snapshot(): VehiclePlantState { | |
| 24 | const relative = rotateNedToBody(quaternionWxyz, velocityMps.map((n, i) => n - windMps[i]!) as SensorVector) | |
| 25 | const airspeedMps = Math.hypot(...relative) | |
| 26 | return { positionM: [...positionM], velocityMps: [...velocityMps], accelerationMps2: [...accelerationMps2], quaternionWxyz: [...quaternionWxyz], angularRateRadS: [...angularRateRadS], onSurface, details: { airspeedMps, alphaRad: Math.atan2(relative[2], relative[0]), betaRad: Math.atan2(relative[1], Math.hypot(relative[0], relative[2])), dynamicPressurePa: 0.5 * airModel.densityKgM3 * airspeedMps ** 2, rotorThrustN: rotorsN.reduce((a, b) => a + b, 0), pusherThrustN } } | |
| 27 | } | |
| 28 | return { | |
| 29 | snapshot, | |
| 30 | step(inputActuators, dtS, environment) { | |
| 31 | const actuators = numericPacket(inputActuators, 8) | |
| 32 | const nextWindMps = numericPacket(environment.windMps, 3) as SensorVector, currentMps = numericPacket(environment.currentMps, 3) as SensorVector | |
| 33 | if (actuators.slice(0, 4).some(n => n < 0 || n > vtolModel.maxRotorThrustN) || actuators[4]! < 0 || actuators[4]! > 1 || actuators.slice(5).some(n => Math.abs(n) > 0.4)) throw new RangeError('Invalid VTOL actuator channels') | |
| 34 | if (!Number.isFinite(dtS) || dtS <= 0 || dtS > 0.01 || !validVector(nextWindMps) || !validVector(currentMps) || currentMps.some(n => n !== 0)) throw new RangeError('Invalid VTOL environment') | |
| 35 | const relative = rotateNedToBody(quaternionWxyz, velocityMps.map((n, i) => n - nextWindMps[i]!) as SensorVector) | |
| 36 | const response = 1 - Math.exp(-dtS / vtolModel.rotorTimeConstantS), maximumChangeN = vtolModel.rotorSlewNPerS * dtS | |
| 37 | const nextRotorsN = rotorsN.map((n, i) => n + Math.max(-maximumChangeN, Math.min(maximumChangeN, (actuators[i]! - n) * response))) | |
| 38 | const pusherChangeN = (actuators[4]! * vtolModel.maxPusherThrustN - pusherThrustN) * (1 - Math.exp(-dtS / vtolModel.pusherTimeConstantS)) | |
| 39 | const pusherMaximumChangeN = vtolModel.pusherSlewNPerS * dtS | |
| 40 | const nextPusherThrustN = pusherThrustN + Math.max(-pusherMaximumChangeN, Math.min(pusherMaximumChangeN, pusherChangeN)) | |
| 41 | const aero = airAerodynamics(relative, angularRateRadS, [nextPusherThrustN / vtolModel.maxPusherThrustN, actuators[5]!, actuators[6]!, actuators[7]!, 0]) | |
| 42 | // Front-left, front-right, rear-right, rear-left. Rotor reaction signs are +, -, +, - about BODY_FRD z. | |
| 43 | const torqueNm: SensorVector = [ | |
| 44 | aero.torqueNm[0] + vtolModel.rotorArmProjectionM * (nextRotorsN[0]! - nextRotorsN[1]! - nextRotorsN[2]! + nextRotorsN[3]!), | |
| 45 | aero.torqueNm[1] + vtolModel.rotorArmProjectionM * (nextRotorsN[0]! + nextRotorsN[1]! - nextRotorsN[2]! - nextRotorsN[3]!), | |
| 46 | aero.torqueNm[2] + vtolModel.reactionTorqueM * (nextRotorsN[0]! - nextRotorsN[1]! + nextRotorsN[2]! - nextRotorsN[3]!), | |
| 47 | ] | |
| 48 | const momentum = angularRateRadS.map((n, i) => n * vtolModel.inertiaKgM2[i]!) | |
| 49 | const cross: SensorVector = [angularRateRadS[1] * momentum[2]! - angularRateRadS[2] * momentum[1]!, angularRateRadS[2] * momentum[0]! - angularRateRadS[0] * momentum[2]!, angularRateRadS[0] * momentum[1]! - angularRateRadS[1] * momentum[0]!] | |
| 50 | const nextAngularRateRadS = angularRateRadS.map((n, i) => n + dtS * (torqueNm[i]! - cross[i]!) / vtolModel.inertiaKgM2[i]!) as SensorVector | |
| 51 | if (!nextAngularRateRadS.every(Number.isFinite) || Math.hypot(...nextAngularRateRadS) > 40) throw new RangeError('VTOL rotor model diverged') | |
| 52 | const angle = Math.hypot(...nextAngularRateRadS) * dtS, scale = angle < 1e-10 ? dtS / 2 : Math.sin(angle / 2) * dtS / angle | |
| 53 | const nextQuaternionWxyz = normalizeQuaternion(multiplyQuaternion(quaternionWxyz, [Math.cos(angle / 2), nextAngularRateRadS[0] * scale, nextAngularRateRadS[1] * scale, nextAngularRateRadS[2] * scale])) | |
| 54 | const forceN = rotateBodyToNed(nextQuaternionWxyz, [aero.forceN[0], aero.forceN[1], aero.forceN[2] - nextRotorsN.reduce((a, b) => a + b, 0)]) | |
| 55 | const nextAccelerationMps2 = forceN.map((n, i) => n / vtolModel.massKg + (i === 2 ? vtolModel.gravityMps2 : 0)) as SensorVector | |
| 56 | const nextVelocityMps = velocityMps.map((n, i) => n + nextAccelerationMps2[i]! * dtS) as SensorVector | |
| 57 | const nextPositionM = positionM.map((n, i) => n + nextVelocityMps[i]! * dtS) as SensorVector | |
| 58 | 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 | // Reject the entire integration before committing any retained state. | |
| 62 | windMps = nextWindMps | |
| 63 | rotorsN = nextRotorsN | |
| 64 | pusherThrustN = nextPusherThrustN | |
| 65 | angularRateRadS = nextAngularRateRadS | |
| 66 | quaternionWxyz = nextQuaternionWxyz | |
| 67 | accelerationMps2 = nextAccelerationMps2 | |
| 68 | velocityMps = nextVelocityMps | |
| 69 | positionM = nextPositionM | |
| 70 | onSurface = nextOnSurface | |
| 71 | return snapshot() | |
| 72 | }, | |
| 73 | } | |
| 74 | } | |
| @@ -193,9 +193,10 @@ try { | ||
| 193 | 193 | } |
| 194 | 194 | finally { restoreAudit() } |
| 195 | 195 | |
| 196 | // The native actual COMMIT has completed before its hook delays response release. | |
| 197 | // Refusal after this boundary cannot undo SQL or its content-free refresh hint. | |
| 198 | for (const kind of ['result', 'token', 'incident'] as const) { | |
| 196 | // Expiry can occur during native COMMIT or while its response is held. In both | |
| 197 | // cases the expired result must be refused without pretending SQL rolled back. | |
| 198 | let heldResponseCases = 0 | |
| 199 | for (const kind of ['result', 'token', 'incident', 'commit-crossing'] as const) { | |
| 199 | 200 | const committed = await create(`registration-commit-${kind}`) |
| 200 | 201 | let token = f.senderToken, deadline = 0 |
| 201 | 202 | if (kind === 'token') { |
| @@ -210,20 +211,42 @@ try { | ||
| 210 | 211 | writer.query('UPDATE public_safety_incidents SET config_json = ?, retain_until = ? WHERE incident_key = ?').run(publicSafetyRecordingData(committed.config), committed.config.retention.expiresAt, committed.config.id) |
| 211 | 212 | } |
| 212 | 213 | const before = await f.snapshot(committed.config.id), hintCount = hints.length |
| 213 | let actualCommits = 0 | |
| 214 | f.database.db.primary.setTransactionDefaults({ afterCommit: async () => { | |
| 214 | let actualCommits = 0, crossingCallbacks = 0, nativeReturnMarginMs: number | null = null, refusalObserved = false | |
| 215 | f.database.db.primary.setTransactionDefaults({ logger: event => { | |
| 216 | // QB's "commit" logger is inside the transaction callback, before native | |
| 217 | // begin() resolves. This real elapsed pause exercises expiry across COMMIT. | |
| 218 | if (kind !== 'commit-crossing' || event.type !== 'commit') return | |
| 219 | crossingCallbacks++ | |
| 220 | const waitMs = Math.max(1, deadline - Date.now() + 20) | |
| 221 | assert(waitMs <= 600, 'Controlled commit crossing retains its bounded 500 ms lifetime') | |
| 222 | Atomics.wait(new Int32Array(new SharedArrayBuffer(4)), 0, 0, waitMs) | |
| 223 | assert(Date.now() >= deadline, 'Controlled callback pause actually crosses expiry') | |
| 224 | }, afterCommit: async () => { | |
| 215 | 225 | actualCommits++ |
| 216 | assert(Date.now() < deadline, 'Actual SQL COMMIT precedes the scoped deadline') | |
| 226 | // A separate native SQLite connection proves this writer's change is | |
| 227 | // committed. Hook receipt time is an upper bound, not a SQL commit clock. | |
| 228 | const persisted = writer.query('SELECT version, (SELECT COUNT(*) FROM public_safety_sources WHERE incident_id = public_safety_incidents.id) AS source_count FROM public_safety_incidents WHERE incident_key = ?').get(committed.config.id) as { version: number, source_count: number } | |
| 229 | assert.deepEqual(persisted, { version: 2, source_count: 1 }) | |
| 230 | nativeReturnMarginMs = deadline - Date.now() | |
| 231 | if (nativeReturnMarginMs > 0) heldResponseCases++ | |
| 232 | if (kind === 'commit-crossing') assert(nativeReturnMarginMs <= 0, 'Explicit crossing arrives after expiry') | |
| 217 | 233 | await Bun.sleep(Math.max(1, deadline - Date.now() + 20)) |
| 234 | assert(Date.now() >= deadline, 'Response release actually follows the scoped expiry') | |
| 218 | 235 | } }) |
| 219 | 236 | try { |
| 220 | 237 | await assert.rejects(() => f.as(token, () => f.records.registerPublicSafetySourceChecked(committed.config.id, { expectedVersion: 1, recording: committed.run }, async () => { |
| 221 | if (kind === 'result') { deadline = Date.now() + 500; return deadline } | |
| 238 | if (kind === 'result' || kind === 'commit-crossing') { deadline = Date.now() + 500; return deadline } | |
| 222 | 239 | return Date.now() + 30000 |
| 223 | 240 | })), f.status(404)) |
| 241 | refusalObserved = true | |
| 242 | } | |
| 243 | finally { | |
| 244 | f.database.db.primary.setTransactionDefaults({ afterCommit: undefined, logger: undefined }) | |
| 245 | const phase = nativeReturnMarginMs === null ? 'native-return-unobserved' : nativeReturnMarginMs > 0 ? 'held-after-native-return' : 'expired-at-native-return' | |
| 246 | console.error(`REGISTRATION_COMMIT_BOUNDARY ${JSON.stringify({ kind, phase, actualCommits, crossingCallbacks, nativeReturnMarginMs, refusalObserved, refusalMarginMs: deadline - Date.now() })}`) | |
| 224 | 247 | } |
| 225 | finally { f.database.db.primary.setTransactionDefaults({ afterCommit: undefined }) } | |
| 226 | 248 | assert.equal(actualCommits, 1) |
| 249 | assert.equal(crossingCallbacks, kind === 'commit-crossing' ? 1 : 0) | |
| 227 | 250 | const after = await f.snapshot(committed.config.id) |
| 228 | 251 | assert.equal(after.incident!.version, 2) |
| 229 | 252 | assert.equal(after.audits.length, before.audits.length + 1) |
| @@ -233,6 +256,7 @@ try { | ||
| 233 | 256 | assert.equal((writer.query('SELECT COUNT(*) AS count FROM public_safety_sources WHERE incident_id = ?').get(after.incident!.id as number) as { count: number }).count, 1) |
| 234 | 257 | if (kind !== 'incident') assert.equal((await sources(committed.config.id)).entries.length, 1) |
| 235 | 258 | } |
| 259 | assert(heldResponseCases > 0, 'At least one actual native return precedes expiry and the held response crosses it') | |
| 236 | 260 | console.log('PASS native guarded source registration compatibility, authority, rollback and COMMIT deadlines') |
| 237 | 261 | } |
| 238 | 262 | finally { unhookHints(); setServer(null); writer.close(); await f.database.closeDatabaseConnection() } |
| @@ -72,6 +72,10 @@ | ||
| 72 | 72 | "./unit/vehicle-authority.test.ts", |
| 73 | 73 | "./unit/vehicle-runtime.test.ts", |
| 74 | 74 | "./unit/air.test.ts", |
| 75 | "./unit/vtol.test.ts", | |
| 76 | "./unit/vtol-control.test.ts", | |
| 77 | "./unit/vtol-runtime.test.ts", | |
| 78 | "./unit/vtol-cli.test.ts", | |
| 75 | 79 | "./unit/ground.test.ts", |
| 76 | 80 | "./unit/surface.test.ts", |
| 77 | 81 | "./unit/mission-workspace.test.ts", |
| @@ -14,7 +14,10 @@ test('native execution registration guard shares the sole mutation writer and de | ||
| 14 | 14 | const timeout = setTimeout(() => { if (running.exitCode === null) running.kill('SIGKILL') }, 90000) |
| 15 | 15 | try { |
| 16 | 16 | const [code, stdout, stderr] = await Promise.all([running.exited, new Response(running.stdout).text(), new Response(running.stderr).text()]) |
| 17 | const boundaries = stderr.split('\n').filter(line => line.startsWith('REGISTRATION_COMMIT_BOUNDARY ')) | |
| 18 | for (const boundary of boundaries) console.log(boundary) | |
| 17 | 19 | expect(code, `${stdout}\n${stderr}`).toBe(0) |
| 20 | expect(boundaries.length).toBe(4) | |
| 18 | 21 | expect(stdout).toContain('PASS native guarded source registration compatibility, authority, rollback and COMMIT deadlines') |
| 19 | 22 | } |
| 20 | 23 | finally { clearTimeout(timeout) } |
| @@ -0,0 +1,31 @@ | ||
| 1 | import { expect, test } from 'bun:test' | |
| 2 | import { command } from '@stacksjs/testing' | |
| 3 | ||
| 4 | test('native VTOL hover command preserves nominal and lost-host outcomes with explicit development scope', async () => { | |
| 5 | for (const [scenario, status] of [['hover', 'completed'], ['host-loss', 'host-lost']]) { | |
| 6 | const result = await command(['vtol:hover', '--scenario', scenario!]).withTimeout(60000) | |
| 7 | expect(result.exitCode).toBe(0) | |
| 8 | const report = JSON.parse(result.stdout) | |
| 9 | expect(report.status).toBe(status) | |
| 10 | expect(report.scope).toBe('development-hover-only') | |
| 11 | expect(report.transitionsImplemented).toBe(false) | |
| 12 | expect(report.physicalFlightEnabled).toBe(false) | |
| 13 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 14 | expect(report.recordedMissionRows).toBeGreaterThan(100) | |
| 15 | if (scenario === 'host-loss') expect(report.board.fault).toBe('output-lease-expired') | |
| 16 | else expect(report.mission.reason).toBe('landed') | |
| 17 | } | |
| 18 | }, 180000) | |
| 19 | ||
| 20 | test('native VTOL command rejects unknown scenarios and preserves insufficient-duration failure', async () => { | |
| 21 | const invalid = await command(['vtol:hover', '--scenario', 'cruise']).withTimeout(60000) | |
| 22 | expect(invalid.exitCode).toBe(1) | |
| 23 | expect(invalid.stdout).toBe('') | |
| 24 | const short = await command(['vtol:hover', '--seconds', '2']).withTimeout(60000) | |
| 25 | expect(short.exitCode).toBe(1) | |
| 26 | const report = JSON.parse(short.stdout) | |
| 27 | expect(report.status).toBe('duration-exhausted') | |
| 28 | expect(report.demonstrationMatched).toBe(false) | |
| 29 | expect(report.mission.state).toBe('running') | |
| 30 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 31 | }, 180000) | |
| @@ -0,0 +1,66 @@ | ||
| 1 | import { expect, test } from 'bun:test' | |
| 2 | import type { SimulatedControlInput } from '../../resources/functions/drone/controls/controller' | |
| 3 | import { createEstimatorReplay } from '../../resources/functions/drone/estimation/replay' | |
| 4 | import { createVtolHoverController } from '../../resources/functions/drone/vehicles/vtol/hover-controller' | |
| 5 | import type { ImuMeasurement, SensorVector } from '../../resources/functions/drone/sensors/types' | |
| 6 | import { createVtolPlant } from '../../resources/functions/drone/vehicles/vtol/plant' | |
| 7 | import { createPhysicsSensorFeed } from '../../resources/functions/drone/controls/sensors' | |
| 8 | import { createEstimator } from '../../resources/functions/drone/estimation/estimator' | |
| 9 | import motion from '../fixtures/estimation/motion-v1.json' | |
| 10 | ||
| 11 | function hoverInput(nowMs = 1000): SimulatedControlInput { | |
| 12 | const estimate = createEstimatorReplay(motion).advanceTo(1040).estimate | |
| 13 | estimate.nowMs = nowMs | |
| 14 | estimate.attitude!.quaternionWxyz = [1, 0, 0, 0] | |
| 15 | estimate.attitude!.capturedAtMs = nowMs | |
| 16 | estimate.localPosition!.positionM = [0, 0, -10] | |
| 17 | estimate.localPosition!.velocityMps = [0, 0, 0] | |
| 18 | estimate.localPosition!.capturedAtMs = nowMs | |
| 19 | estimate.localPosition!.predictedToMs = nowMs | |
| 20 | return { kind: 'simulation', enabled: true, nowMs, estimate, angularRateRadS: [0, 0, 0], rateCapturedAtMs: nowMs, target: { positionM: [0, 0, -10], yawRad: 0, issuedAtMs: nowMs } } | |
| 21 | } | |
| 22 | ||
| 23 | // Public estimator-to-control seam catches model mass and allocation, not closed-loop flight or transition qualification. | |
| 24 | test('owned VTOL hover feedback requests the 2.5 kg weight using its own rotor order and limits', () => { | |
| 25 | const output = createVtolHoverController({ headingAlignment: 'initial-simulation-north' }).step(hoverInput()) | |
| 26 | expect(output.inhibited).toBe(false) | |
| 27 | expect(output.desiredWrench!.thrustN).toBeCloseTo(24.516625, 10) | |
| 28 | expect(output.desiredWrench!.torqueNm).toEqual([0, 0, 0]) | |
| 29 | for (const thrustN of output.motorsN) expect(thrustN).toBeCloseTo(6.12915625, 10) | |
| 30 | expect(output.physicalFlightEnabled).toBe(false) | |
| 31 | expect(output.boardOutputsInhibited).toBe(true) | |
| 32 | }) | |
| 33 | ||
| 34 | // Public owned sensor/estimator/control/plant seams catch closed-loop response, not mission, authority or transition qualification. | |
| 35 | test('the VTOL body follows a hover position step using raw IMU and GNSS rather than plant pose feedback', () => { | |
| 36 | const plant = createVtolPlant(), feed = createPhysicsSensorFeed(0) | |
| 37 | const estimator = createEstimator({ origin: { latitudeDeg: 0, longitudeDeg: 0, ellipsoidHeightM: 100 }, gravityCorrection: 'initialization-only' }) | |
| 38 | const controller = createVtolHoverController({ headingAlignment: 'initial-simulation-north' }) | |
| 39 | let state = plant.snapshot(), command = [0, 0, 0, 0], activeTicks = 0, inhibitedTicks = 0, invalidCommands = 0, maxTiltRad = 0 | |
| 40 | let estimate = estimator.snapshot(0) | |
| 41 | const targetAt = (nowMs: number): SensorVector => nowMs >= 2000 ? [2, 1, -1.5] : [0, 0, -1] | |
| 42 | for (let nowMs = 0; nowMs <= 20000; nowMs += 4) { | |
| 43 | if (nowMs > 600) state = plant.step([...command, 0, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] }) | |
| 44 | feed.capture(state, nowMs) | |
| 45 | const sensors = feed.advanceTo(nowMs) | |
| 46 | estimate = estimator.update(sensors) | |
| 47 | if (nowMs >= 600 && nowMs % 20 === 0) { | |
| 48 | const observation = sensors.sensors.find(s => s.kind === 'imu')?.observation | |
| 49 | const imu = observation?.value as ImuMeasurement | undefined | |
| 50 | const output = controller.step({ kind: 'simulation', enabled: true, nowMs, estimate, angularRateRadS: imu?.angularRateRadS ?? [0, 0, 0], rateCapturedAtMs: observation?.estimatedCaptureMs ?? -Infinity, target: { positionM: targetAt(nowMs), yawRad: 0, issuedAtMs: nowMs } }) | |
| 51 | command = [...output.motorsN] | |
| 52 | if (output.inhibited) inhibitedTicks++ | |
| 53 | else activeTicks++ | |
| 54 | if (command.some(n => !Number.isFinite(n) || n < 0 || n > 12)) invalidCommands++ | |
| 55 | } | |
| 56 | maxTiltRad = Math.max(maxTiltRad, Math.acos(Math.max(-1, Math.min(1, 1 - 2 * (state.quaternionWxyz[1] ** 2 + state.quaternionWxyz[2] ** 2))))) | |
| 57 | } | |
| 58 | expect(activeTicks).toBe(971) | |
| 59 | expect(inhibitedTicks).toBe(0) | |
| 60 | expect(invalidCommands).toBe(0) | |
| 61 | expect(Math.hypot(...state.positionM.map((n, i) => n - targetAt(20000)[i]!))).toBeLessThan(0.2) | |
| 62 | expect(maxTiltRad).toBeLessThan(0.44) | |
| 63 | expect(state.onSurface).toBe(false) | |
| 64 | expect(estimate.health.localPosition).toBe(true) | |
| 65 | expect(estimate.health.absoluteHeading).toBe(false) | |
| 66 | }) | |
| @@ -0,0 +1,94 @@ | ||
| 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 type { VehicleControllerContext } from '../../resources/functions/drone/vehicles/types' | |
| 6 | import { createVtolVehicleHoverController } from '../../resources/functions/drone/vehicles/vtol/controller' | |
| 7 | ||
| 8 | // Shared runtime seam includes raw sensors, estimator, mission, authority and independent leases; no transition or hardware qualification. | |
| 9 | test('the owned VTOL hover mission takes off vertically and lands under shared mission authority', () => { | |
| 10 | const report = runVehicleSimulation(createVtolHoverProfile(), { seconds: 60 }) | |
| 11 | expect(report.status).toBe('completed') | |
| 12 | expect(report.mission.reason).toBe('landed') | |
| 13 | expect(report.navigationEvidence.observedSensors.imu).toBeGreaterThan(100) | |
| 14 | expect(report.navigationEvidence.observedSensors.gnss).toBeGreaterThan(20) | |
| 15 | expect(report.path.some(row => row.truthPositionM[2] < -4)).toBe(true) | |
| 16 | expect(report.path.some(row => row.actuators.slice(0, 4).some(n => n > 0))).toBe(true) | |
| 17 | expect(report.path.every(row => row.actuators.slice(4).every(n => n === 0))).toBe(true) | |
| 18 | expect(report.path.at(-1)!.measurement!.values.onSurface).toBe(true) | |
| 19 | expect(report.finalTruthPositionM[2]).toBe(0) | |
| 20 | expect(Math.hypot(report.finalTruthPositionM[0] - 2, report.finalTruthPositionM[1] - 1)).toBeLessThan(0.3) | |
| 21 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 22 | expect(report.physicalFlightEnabled).toBe(false) | |
| 23 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) | |
| 24 | }) | |
| 25 | ||
| 26 | function observedControllerInputs() { | |
| 27 | const profile = createVtolHoverProfile(), createSession = profile.createSession, inputs: VehicleControllerContext[] = [] | |
| 28 | profile.createSession = () => { | |
| 29 | const session = createSession(), createController = session.createController | |
| 30 | return { ...session, createController() { | |
| 31 | const controller = createController() | |
| 32 | return { step(context) { inputs.push(structuredClone(context)); return controller.step(context) } } | |
| 33 | } } | |
| 34 | } | |
| 35 | expect(runVehicleSimulation(profile, { seconds: 60 }).status).toBe('completed') | |
| 36 | return inputs | |
| 37 | } | |
| 38 | ||
| 39 | test('fresh landing contact cannot bypass a stale raw rate observation or clear its inhibition', () => { | |
| 40 | const inputs = observedControllerInputs() | |
| 41 | const contactIndex = inputs.findIndex(c => c.enabled && c.mission.taskKind === 'land' && c.measurement?.values.onSurface === true) | |
| 42 | expect(contactIndex).toBeGreaterThan(0) | |
| 43 | const controller = createVtolVehicleHoverController() | |
| 44 | expect(controller.step(inputs[contactIndex - 1]!).inhibited).toBe(false) | |
| 45 | const stale = structuredClone(inputs[contactIndex]!) | |
| 46 | stale.controlRate!.capturedAtMs -= 1000 | |
| 47 | const rejected = controller.step(stale) | |
| 48 | expect(rejected.inhibited).toBe(true) | |
| 49 | expect(rejected.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 50 | expect(controller.step(inputs[contactIndex + 1]!).inhibited).toBe(true) | |
| 51 | }) | |
| 52 | ||
| 53 | test('pausing a qualified VTOL on the ground keeps propulsion neutral until explicit mission resume', () => { | |
| 54 | const inputs = observedControllerInputs(), controller = createVtolVehicleHoverController() | |
| 55 | const paused = structuredClone(inputs[0]!) | |
| 56 | paused.mission.state = 'paused' | |
| 57 | paused.mission.targetM = [0, 0, 0] | |
| 58 | expect(paused.measurement!.values.onSurface).toBe(true) | |
| 59 | const held = controller.step(paused) | |
| 60 | expect(held.inhibited).toBe(false) | |
| 61 | expect(held.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 62 | const resumed = controller.step(inputs[1]!) | |
| 63 | expect(resumed.inhibited).toBe(false) | |
| 64 | expect(resumed.actuators.slice(0, 4).some(n => n > 0)).toBe(true) | |
| 65 | }) | |
| 66 | ||
| 67 | test('VTOL sensor loss, manual takeover and host death revoke distinct owned output leases', () => { | |
| 68 | // Neutral lease withdrawal does not establish a safe emergency landing. | |
| 69 | for (const scenario of ['imu-loss', 'position-loss', 'vehicle-sensor-loss', 'host-loss', 'rc-override', 'kill']) { | |
| 70 | const report = runVehicleSimulation(createVtolHoverProfile(scenario), { scenario, seconds: 30 }) | |
| 71 | expect(report.status).toBe(scenario === 'host-loss' ? 'host-lost' : 'blocked') | |
| 72 | expect(report.path.some(row => row.truthPositionM[2] < -1)).toBe(true) | |
| 73 | expect(report.path.some(row => row.actuators.slice(0, 4).some(n => n > 0))).toBe(true) | |
| 74 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 75 | expect(report.board.expiresAtMs).toBeNull() | |
| 76 | if (scenario === 'host-loss') expect(report.board.fault).toBe('output-lease-expired') | |
| 77 | else expect(report.mission.state).toBe('blocked') | |
| 78 | } | |
| 79 | }) | |
| 80 | ||
| 81 | test('separate VTOL sessions retain independent controller faults and returned actuator arrays', () => { | |
| 82 | const inputs = observedControllerInputs(), profile = createVtolHoverProfile() | |
| 83 | const first = profile.createSession().createController(), second = profile.createSession().createController() | |
| 84 | const firstOutput = first.step(inputs[0]!) | |
| 85 | expect(second.step(inputs[0]!)).toEqual(firstOutput) | |
| 86 | firstOutput.actuators[0] = 99 | |
| 87 | const invalid = structuredClone(inputs[1]!) | |
| 88 | invalid.controlRate!.reference = 'another-reference' | |
| 89 | expect(first.step(invalid).inhibited).toBe(true) | |
| 90 | expect(first.step(inputs[2]!).inhibited).toBe(true) | |
| 91 | const unaffected = second.step(inputs[1]!) | |
| 92 | expect(unaffected.inhibited).toBe(false) | |
| 93 | expect(unaffected.actuators.slice(0, 4).every(n => n >= 0 && n <= 12)).toBe(true) | |
| 94 | }) | |
| @@ -0,0 +1,173 @@ | ||
| 1 | import { expect, test } from 'bun:test' | |
| 2 | import { createVtolPlant } from '../../resources/functions/drone/vehicles/vtol/plant' | |
| 3 | import { allocateVtolLift } from '../../resources/functions/drone/vehicles/vtol/allocation' | |
| 4 | ||
| 5 | // Public plant seam: checks vertical force integration without forward airspeed; it does not qualify a controller or physical aircraft. | |
| 6 | test('lift-and-cruise rotors support the declared airframe without forward airspeed or an imposed position', () => { | |
| 7 | // Fictional 2.5 kg model: each of four rotors supplies 6.12915625 N in a level hover. | |
| 8 | const rotorsN: [number, number, number, number] = [6.12915625, 6.12915625, 6.12915625, 6.12915625] | |
| 9 | const supported = createVtolPlant({ positionM: [0, 0, -10], rotorsN }) | |
| 10 | const environment = { windMps: [0, 0, 0] as [number, number, number], currentMps: [0, 0, 0] as [number, number, number] } | |
| 11 | let state = supported.snapshot() | |
| 12 | // Channels are four rotor thrusts in N, normalized pusher throttle, then three surfaces in radians. | |
| 13 | for (let step = 0; step < 250; step++) state = supported.step([...rotorsN, 0, 0, 0, 0], 0.004, environment) | |
| 14 | expect(state.positionM[2]).toBeCloseTo(-10, 8) | |
| 15 | expect(state.velocityMps[2]).toBeCloseTo(0, 8) | |
| 16 | expect(state.accelerationMps2[2]).toBeCloseTo(0, 8) | |
| 17 | expect(state.details.airspeedMps).toBeCloseTo(0, 8) | |
| 18 | expect(state.onSurface).toBe(false) | |
| 19 | ||
| 20 | const unsupported = createVtolPlant({ positionM: [0, 0, -10] }) | |
| 21 | const falling = unsupported.step([0, 0, 0, 0, 0, 0, 0, 0], 0.004, environment) | |
| 22 | expect(falling.accelerationMps2[2]).toBeCloseTo(9.80665, 8) | |
| 23 | expect(falling.velocityMps[2]).toBeGreaterThan(0) | |
| 24 | expect(falling.positionM[2]).toBeGreaterThan(-10) | |
| 25 | }) | |
| 26 | ||
| 27 | test('rotor thrust spools up and decays after command release instead of changing instantaneously', () => { | |
| 28 | const plant = createVtolPlant({ positionM: [0, 0, -10] }) | |
| 29 | const spooling = plant.step([12, 12, 12, 12, 0, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] }) | |
| 30 | expect(spooling.accelerationMps2[2]).toBeGreaterThan(8) | |
| 31 | expect(spooling.accelerationMps2[2]).toBeLessThan(9.80665) | |
| 32 | const released = plant.step([0, 0, 0, 0, 0, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] }) | |
| 33 | expect(released.accelerationMps2[2]).toBeGreaterThan(8) | |
| 34 | expect(released.accelerationMps2[2]).toBeLessThan(9.80665) | |
| 35 | expect(released.details.rotorThrustN).toBeGreaterThan(0) | |
| 36 | expect(released.details.rotorThrustN).toBeLessThan(spooling.details.rotorThrustN as number) | |
| 37 | }) | |
| 38 | ||
| 39 | test('independent lift rotors generate body moments without forward airspeed', () => { | |
| 40 | // Rotor order: front-left, front-right, rear-right, rear-left in BODY_FRD. | |
| 41 | const oneRotor = createVtolPlant({ positionM: [0, 0, -10] }) | |
| 42 | const turning = oneRotor.step([12, 0, 0, 0, 0, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] }) | |
| 43 | expect(turning.angularRateRadS.every(n => n > 0)).toBe(true) | |
| 44 | expect(turning.quaternionWxyz.slice(1).every(n => n > 0)).toBe(true) | |
| 45 | const frontPair = createVtolPlant({ positionM: [0, 0, -10] }) | |
| 46 | const pitching = frontPair.step([12, 12, 0, 0, 0, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] }) | |
| 47 | expect(pitching.angularRateRadS[0]).toBeCloseTo(0, 10) | |
| 48 | expect(pitching.angularRateRadS[1]).toBeGreaterThan(0) | |
| 49 | expect(pitching.angularRateRadS[2]).toBeCloseTo(0, 10) | |
| 50 | }) | |
| 51 | ||
| 52 | test('wing lift and the independent pusher act on the same body using relative airflow', () => { | |
| 53 | const still = createVtolPlant({ positionM: [0, 0, -20], velocityMps: [10, 0, 0] }) | |
| 54 | const glide = still.step([0, 0, 0, 0, 0, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] }) | |
| 55 | // At 10 m/s the declared trainer wing supplies 12.25 N of lift and 2.14375 N of drag. | |
| 56 | expect(glide.accelerationMps2[2]).toBeCloseTo(4.90665, 8) | |
| 57 | expect(glide.accelerationMps2[0]).toBeCloseTo(-0.8575, 8) | |
| 58 | const powered = createVtolPlant({ positionM: [0, 0, -20], velocityMps: [10, 0, 0], pusherThrustN: 7 }) | |
| 59 | const thrust = powered.step([0, 0, 0, 0, 0.5, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] }) | |
| 60 | expect(thrust.accelerationMps2[0]).toBeCloseTo(1.9425, 8) | |
| 61 | const windy = createVtolPlant({ positionM: [0, 0, -20], velocityMps: [10, 0, 0] }) | |
| 62 | const headwind = windy.step([0, 0, 0, 0, 0, 0, 0, 0], 0.004, { windMps: [-5, 0, 0], currentMps: [0, 0, 0] }) | |
| 63 | expect(headwind.accelerationMps2[2]).toBeLessThan(glide.accelerationMps2[2]) | |
| 64 | expect(headwind.details.airspeedMps).toBeGreaterThan(14.9) | |
| 65 | expect(headwind.details.airspeedMps).toBeLessThan(15.1) | |
| 66 | }) | |
| 67 | ||
| 68 | test('aerodynamic surfaces gain moment authority with airflow while rotor controls remain independent', () => { | |
| 69 | const resting = createVtolPlant({ positionM: [0, 0, -20] }) | |
| 70 | const noAir = resting.step([0, 0, 0, 0, 0, 0.4, 0.4, 0.4], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] }) | |
| 71 | expect(noAir.angularRateRadS).toEqual([0, 0, 0]) | |
| 72 | const moving = createVtolPlant({ positionM: [0, 0, -20], velocityMps: [10, 0, 0] }) | |
| 73 | const controlled = moving.step([0, 0, 0, 0, 0, 0.4, 0.4, 0.4], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] }) | |
| 74 | // Independently worked first-step surface moments: 5.6448, 5.22144 and 3.7632 Nm. | |
| 75 | expect(controlled.angularRateRadS[0]).toBeCloseTo(0.075264, 8) | |
| 76 | expect(controlled.angularRateRadS[1]).toBeCloseTo(0.0522144, 8) | |
| 77 | expect(controlled.angularRateRadS[2]).toBeCloseTo(0.025088, 8) | |
| 78 | }) | |
| 79 | ||
| 80 | test('body-relative airflow and forces rotate consistently into the fixed NED frame', () => { | |
| 81 | const eastFacing = createVtolPlant({ positionM: [0, 0, -20], velocityMps: [0, 10, 0], quaternionWxyz: [Math.SQRT1_2, 0, 0, Math.SQRT1_2], pusherThrustN: 7 }) | |
| 82 | const east = eastFacing.step([0, 0, 0, 0, 0.5, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] }) | |
| 83 | expect(east.accelerationMps2[0]).toBeCloseTo(0, 8) | |
| 84 | expect(east.accelerationMps2[1]).toBeCloseTo(1.9425, 8) | |
| 85 | expect(east.accelerationMps2[2]).toBeCloseTo(4.90665, 8) | |
| 86 | expect(east.details.betaRad).toBeCloseTo(0, 8) | |
| 87 | }) | |
| 88 | ||
| 89 | test('malformed numeric packets are refused before changing the owned body or wind', () => { | |
| 90 | const plant = createVtolPlant({ positionM: [0, 0, -20] }) | |
| 91 | const before = plant.snapshot() | |
| 92 | const sparse: number[] = [] | |
| 93 | sparse.length = 8 | |
| 94 | sparse.fill(0, 1) | |
| 95 | expect(() => plant.step(sparse, 0.004, { windMps: [-2, 0, 0], currentMps: [0, 0, 0] })).toThrow() | |
| 96 | expect(plant.snapshot()).toEqual(before) | |
| 97 | expect(() => createVtolPlant({ quaternionWxyz: [1, 0, 0, 0, 7] as never })).toThrow() | |
| 98 | expect(() => plant.step([13, 0, 0, 0, 0, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] })).toThrow() | |
| 99 | expect(plant.snapshot()).toEqual(before) | |
| 100 | }) | |
| 101 | ||
| 102 | test('explicit null initial numeric fields cannot become a default vehicle state', () => { | |
| 103 | for (const field of ['positionM', 'velocityMps', 'quaternionWxyz', 'rotorsN', 'pusherThrustN']) { | |
| 104 | expect(() => createVtolPlant({ [field]: null } as never)).toThrow() | |
| 105 | } | |
| 106 | expect(createVtolPlant().snapshot().positionM).toEqual([0, 0, 0]) | |
| 107 | }) | |
| 108 | ||
| 109 | test('the independent pusher spools up and retains decaying thrust after release', () => { | |
| 110 | const plant = createVtolPlant({ positionM: [0, 0, -20] }) | |
| 111 | const spooling = plant.step([0, 0, 0, 0, 1, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] }) | |
| 112 | expect(spooling.accelerationMps2[0]).toBeGreaterThan(0) | |
| 113 | expect(spooling.accelerationMps2[0]).toBeLessThan(1) | |
| 114 | expect(spooling.details.pusherThrustN).toBeGreaterThan(0) | |
| 115 | expect(spooling.details.pusherThrustN).toBeLessThan(14) | |
| 116 | const released = plant.step([0, 0, 0, 0, 0, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] }) | |
| 117 | expect(released.details.pusherThrustN).toBeGreaterThan(0) | |
| 118 | expect(released.details.pusherThrustN).toBeLessThan(spooling.details.pusherThrustN as number) | |
| 119 | }) | |
| 120 | ||
| 121 | test('a divergent integration leaves the retained body intact and the next ordinary step usable', () => { | |
| 122 | // Public step/snapshot catches rejected integration poisoning; it does not qualify a flight controller or recovery policy. | |
| 123 | for (const windMps of [[1000000, 1000000, 0], [0, 0, 1000000]] as [number, number, number][]) { | |
| 124 | const initial = { positionM: [0, 0, -20] as [number, number, number], velocityMps: [10, 0, 0] as [number, number, number], rotorsN: [1, 2, 3, 4] as [number, number, number, number], pusherThrustN: 7 } | |
| 125 | const plant = createVtolPlant(initial), reference = createVtolPlant(initial) | |
| 126 | const before = plant.snapshot() | |
| 127 | const command = [12, 12, 12, 12, 1, 0, 0, 0] | |
| 128 | expect(() => plant.step(command, 0.004, { windMps, currentMps: [0, 0, 0] })).toThrow() | |
| 129 | expect(plant.snapshot()).toEqual(before) | |
| 130 | const environment = { windMps: [0, 0, 0] as [number, number, number], currentMps: [0, 0, 0] as [number, number, number] } | |
| 131 | expect(plant.step(command, 0.004, environment)).toEqual(reference.step(command, 0.004, environment)) | |
| 132 | } | |
| 133 | }) | |
| 134 | ||
| 135 | test('VTOL lift allocation produces the declared BODY_FRD wrench in front-left rotor order', () => { | |
| 136 | // Independent inverse: [10, 6, 4, 4] N produces 24 N and [1.6, 3.2, 0.08] Nm. | |
| 137 | const result = allocateVtolLift({ thrustN: 24, torqueNm: [1.6, 3.2, 0.08] }) | |
| 138 | expect(result.rotorsN).toEqual([10, 6, 4, 4]) | |
| 139 | expect(result.achieved.thrustN).toBe(24) | |
| 140 | expect(result.achieved.torqueNm[0]).toBeCloseTo(1.6, 10) | |
| 141 | expect(result.achieved.torqueNm[1]).toBeCloseTo(3.2, 10) | |
| 142 | expect(result.achieved.torqueNm[2]).toBeCloseTo(0.08, 10) | |
| 143 | expect(result.limited).toBe(false) | |
| 144 | }) | |
| 145 | ||
| 146 | test('saturated lift allocation preserves collective before roll and pitch and sacrifices yaw first', () => { | |
| 147 | const result = allocateVtolLift({ thrustN: 44, torqueNm: [3.2, 0, 0.08] }) | |
| 148 | expect(result.rotorsN).toEqual([12, 10, 10, 12]) | |
| 149 | expect(result.achieved.thrustN).toBe(44) | |
| 150 | expect(result.achieved.torqueNm).toEqual([1.6, 0, 0]) | |
| 151 | expect(result.limited).toBe(true) | |
| 152 | }) | |
| 153 | ||
| 154 | test('lift allocation refuses malformed and nonfinite wrench packets instead of returning invalid rotor commands', () => { | |
| 155 | const sparse: number[] = [] | |
| 156 | sparse.length = 3 | |
| 157 | sparse.fill(0, 1) | |
| 158 | for (const wrench of [{ thrustN: Number.NaN, torqueNm: [0, 0, 0] }, { thrustN: 24, torqueNm: sparse }, { thrustN: 24, torqueNm: [0, 0, Infinity] }, { thrustN: 24, torqueNm: [0, 0, 0, 1] }, { thrustN: 24, torqueNm: null }]) { | |
| 159 | expect(() => allocateVtolLift(wrench as never)).toThrow() | |
| 160 | } | |
| 161 | }) | |
| 162 | ||
| 163 | test('inherited slots and custom iterators cannot disguise malformed lift torque packets', () => { | |
| 164 | const sparse: number[] = [] | |
| 165 | sparse.length = 3 | |
| 166 | sparse.fill(0, 1) | |
| 167 | Object.setPrototypeOf(sparse, Object.assign(Object.create(Array.prototype), { 0: 0 })) | |
| 168 | let iteratorCalls = 0 | |
| 169 | const rewritten = [NaN, 0, 0] | |
| 170 | Object.defineProperty(rewritten, Symbol.iterator, { value: function* () { iteratorCalls++; yield 0; yield 0; yield 0 } }) | |
| 171 | for (const torqueNm of [sparse, rewritten]) expect(() => allocateVtolLift({ thrustN: 24, torqueNm } as never)).toThrow() | |
| 172 | expect(iteratorCalls).toBe(0) | |
| 173 | }) | |