ReviewOS

also looking at this

stacks/drone

fix: stabilize VTOL lateral flow and hover control

#91
Merged chris wants to merge codex/vtol-spanwise-continuity into main
17 files +556 -39

Review threads live on the whole diff, not on one commit, so none are shown here - a thread's line means something in the branch's final form, and painting it into an intermediate step would put it on code it is not about.

docs/vehicles-vtol.mdmodified+36-15
Changes to docs/vehicles-vtol.md
@@ -36,20 +36,40 @@ and a 120 N/s slew limit. An independent 14 N pusher and the owned trainer's aer
3636share the rigid body. Rotor allocation preserves collective thrust first, then roll/pitch demand,
3737then yaw. No coefficient is measured from a purchased airframe.
3838
39The VTOL wing uses the fictional `folded-incidence-v2` extension. Its forward pre-stall curve
40matches the trainer. In reversed chordwise flow, incidence is folded and the coefficient changes
41sign, with a cosine fade toward broadside. This removes the force/moment jump at the raw angle's
42positive/negative pi boundary and restores the local vertical response in the tested head/tail flow.
43Raw airflow angles remain available to the capture and envelope guards. The fixed-wing profile keeps
44its original trainer model. Neither curve represents measured reverse-flow or post-stall data.
39The VTOL wing uses the fictional `projected-incidence-v3` extension. At zero sideslip its forward
40pre-stall response exactly matches the trainer. Reversed chordwise incidence is folded, changes
41coefficient sign and fades toward broadside. Longitudinal lift, drag and static pitch use the fraction
42of dynamic pressure in the chord/vertical airflow plane. This removes the force/moment singularity
43where that plane's velocity goes to zero while spanwise flow stays finite. Total pressure and raw
44airflow angles remain available to capture and envelope guards. The separately declared lateral,
45surface and rate responses retain total pressure. The default fixed-wing trainer is unchanged.
46These are fictional coefficients, without measured post-stall, spanwise or reverse-flight data.
47Full-route CLI reports identify this curve with `aerodynamicsModel: 'projected-incidence-v3'`.
48
49The prior `folded-incidence-v2` removed the reverse-flow angle-wrap jump at zero spanwise velocity,
50but retained a 0.9555 N force jump between `[+1e-8, 2, 0]` and `[-1e-8, 2, 0]` m/s. Approaching the
51same lateral-flow axis from positive/negative vertical velocity produced a 4.0768 N vertical-force
52jump. The v3 checks cover these approach directions, finite zero-flow response and non-positive
53unpowered airflow work. They establish bounded mathematical properties, not a calibrated airframe.
54A predeclared 16-case runtime campaign introduces 2 m/s head, tail, east and west wind at observed
55front, cruise, back and landing phase entries. All complete their declared heading plans through
56source-qualified hover, contact and replay within the original budgets. Wind onset is applied on
57the next actual integration step; the CLI/workspace wind selectors still use the fixed 1000 ms onset.
58
59Hover control aligns the thrust direction separately from its heading correction. A large, saturated
60yaw error therefore cannot rotate the requested position correction into another axis. Opposite
61thrust directions have no unique shortest tilt axis; that unsupported condition inhibits and latches
62neutral output. Existing body-rate bounds, PID gains, source guards and allocation priorities remain.
4563
4664Back-transition braking compares the linear pre-stall estimate with 41 pitch candidates in the
4765existing -0.08 to 0.3 rad range. It evaluates the declared nonlinear wing/rotor force balance against
4866qualified captured airflow and estimated ground motion. Candidates must admit 0 to 48 N of lift-rotor
4967thrust while supporting the requested vertical force; high-speed candidate incidence remains within
50680.3 rad. The finite transition deadline, source guards, capture dwell and independent authority still
51apply. Replacing a correlated back request preserves rotary capture once reached, so recovery cannot
52restart forward-flight braking during a reverse position return.
69apply. Before low-speed rotary capture, the nose and candidate force search follow qualified measured
70airflow. The requested final hover heading remains the capture condition after braking. Replacing a
71correlated back request preserves rotary capture once reached, so recovery cannot restart forward-flight
72braking during a reverse position return.
5373
5474### Bounded wind cases
5575
@@ -92,8 +112,9 @@ profile.faults = (_scenario, atMs) => ({ windMps: atMs >= 1000 ? [2, 0, 0] : [0,
92112const result = runVehicleSimulation(profile, { seconds: 160 })
93113```
94114
95The original fixed north-facing east/west crosswind plans still reach the back-transition
96envelope guard without confirmed hover or landing. During the east case's reverse return at 38,000 ms,
115The fixed north-facing east/west crosswind plans still reach the back-transition envelope guard
116without confirmed hover or landing. The retained v2 east-case diagnosis, before the v3 flow projection,
117records a reverse return at 38,000 ms where
97118the modeled aerodynamic yaw moment is approximately -0.729 Nm while the fully allocated opposing
98119rotor yaw moment is only +0.416 Nm, with the rudder at its +0.4 rad bound. The qualified airflow capture
99120is 60 ms old, attitude 20 ms and position 100 ms, within the unchanged source guards. This is an
@@ -105,15 +126,15 @@ can oppose at most +0.470 Nm. This worked example does not establish a full stea
105126envelope, but changing gains cannot supply the missing torque in that condition. Neutral outputs
106127at the original airborne failure do not establish a safe landing.
107128
108The two wind-facing plans change only the declared back-hover heading and plan identity. They use
109the existing `vtol-mode.headingRad` mission contract and unchanged controller, model, reserve and
110finite budgets. The front heading remains north. These plans complete the source-qualified modeled hover
129The two wind-facing plans differ from their fixed-heading counterparts only in the declared
130back-hover heading and plan identity. They use the existing `vtol-mode.headingRad` mission contract
131and the same shared controller, model, reserve and finite budgets. The front heading remains north. These plans complete the source-qualified modeled hover
111132confirmation and contact route in the declared wind. Their extra `backHeadingRad` declaration and
112133`backHeadingMatched` check bind qualification to the actual recorded plan; a completed route under
113a different heading remains unmatched. No automatic heading substitution or retry is performed.
134a different heading remains unmatched. The plan retains its declared final heading, and there are no automatic retries.
114135The original `*-limit-2mps` cases keep their prior plans and blocked outcomes.
115136
116Wider wind directions, onset phases, a general feasibility planner and full-angle model qualification
137Wider wind directions, onset phases, a general feasibility planner and measured full-angle model qualification
117138remain in [#87](https://github.com/stacksjs/drone/issues/87). No arbitrary-wind, measured-airframe or
118139physical flight acceptance is claimed.
119140
resources/functions/drone/controls/lift-controller.tsmodified+14-4
Changes to resources/functions/drone/controls/lift-controller.ts
@@ -1,7 +1,8 @@
11import type { EstimatorSnapshot } from '../estimation/estimator'
22import type { SensorVector } from '../sensors/types'
33import type { Motors, Wrench } from './model'
4import { multiplyQuaternion, quaternionFromTilt } from '../estimation/frames'
4import { multiplyQuaternion, normalizeQuaternion, quaternionFromTilt } from '../estimation/frames'
5import { rotateBodyToNed } from './physics'
56import { createBoundedPid } from './pid'
67import { clampControl, validVector } from './model'
78export interface SimulatedControlInput {
@@ -66,9 +67,18 @@ export function createSimulatedLiftController(options: { headingAlignment: 'init
6667 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]
6768 const vertical = model.gravityMps2 - acceleration[2]
6869 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)))
69 const q = attitude.quaternionWxyz
70 const error = multiplyQuaternion([q[0], -q[1], -q[2], -q[3]], desired)
71 const rates = error.slice(1).map((n, i) => clampControl(8 * n * (error[0] < 0 ? -1 : 1), i === 2 ? 1.5 : 3)) as SensorVector
70 const q = normalizeQuaternion(attitude.quaternionWxyz)
71 // First align the thrust direction without twisting about it. An
72 // unattainable yaw goal must not rotate the position correction.
73 const from = rotateBodyToNed(q, [0, 0, 1]), to = rotateBodyToNed(desired, [0, 0, 1])
74 const dot = Math.max(-1, Math.min(1, from.reduce((sum, n, i) => sum + n * to[i]!, 0)))
75 if (1 + dot <= 1e-8) return inhibit('antipodal-thrust-direction')
76 const alignment = normalizeQuaternion([1 + dot, from[1] * to[2] - from[2] * to[1], from[2] * to[0] - from[0] * to[2], from[0] * to[1] - from[1] * to[0]])
77 const reduced = normalizeQuaternion(multiplyQuaternion(alignment, q))
78 const tiltError = multiplyQuaternion([q[0], -q[1], -q[2], -q[3]], reduced)
79 const yawError = multiplyQuaternion([reduced[0], -reduced[1], -reduced[2], -reduced[3]], desired)
80 const correction = [tiltError[1] * (tiltError[0] < 0 ? -1 : 1), tiltError[2] * (tiltError[0] < 0 ? -1 : 1), yawError[3] * (yawError[0] < 0 ? -1 : 1)]
81 const rates = correction.map((n, i) => clampControl(8 * n, i === 2 ? 1.5 : 3)) as SensorVector
7282 const torque = rates.map((n, i) => ratePid[i]!.step(n - input.angularRateRadS[i]!, input.angularRateRadS[i]!, dt)) as SensorVector
7383 const desiredWrench = { thrustN: model.massKg * Math.hypot(...[acceleration[0], acceleration[1], vertical]), torqueNm: torque }
7484 const allocation = allocate(desiredWrench, previous, dt)
resources/functions/drone/vehicles/air/plant.tsmodified+7-2
Changes to resources/functions/drone/vehicles/air/plant.ts
@@ -19,6 +19,11 @@ export function airAerodynamics(relativeBodyMps: SensorVector, rateRadS: SensorV
1919 const [u, v, w] = relativeBodyMps
2020 const speedMps = Math.hypot(u, v, w), alphaRad = Math.atan2(w, u), betaRad = Math.atan2(v, Math.hypot(u, w))
2121 const dynamicPressurePa = 0.5 * airModel.densityKgM3 * speedMps ** 2
22 // The VTOL extension applies longitudinal lift, drag and static pitch only
23 // to the incident chord/vertical flow plane. Total pressure remains measured
24 // and supplies the separately declared lateral, surface and rate response.
25 const planeFraction = foldReverseFlow && v !== 0 ? Math.hypot(u, w) / speedMps : 1
26 const longitudinalPressurePa = dynamicPressurePa * planeFraction ** 2
2227 // Explicit VTOL extension: fold the incidence and project stalled flow along
2328 // the chord. Preserve the forward pre-stall curve; fade to zero at broadside
2429 // and reverse the coefficient sign for flow over the trailing edge.
@@ -29,13 +34,13 @@ export function airAerodynamics(relativeBodyMps: SensorVector, rateRadS: SensorV
2934 // Bounded post-stall loss, retaining drag. No claim of dynamic stall or spin modeling.
3035 const liftCoefficient = chordDirection * clamp(linearLift, -1.1, 1.4) * Math.max(0.15, 1 - Math.max(0, Math.abs(coefficientAlphaRad) - airModel.stallAlphaRad) * 2.5)
3136 const dragCoefficient = 0.04 + 0.06 * liftCoefficient ** 2 + 0.8 * Math.sin(alphaRad) ** 2 + 0.2 * Math.sin(betaRad) ** 2
32 const lift = dynamicPressurePa * airModel.areaM2 * liftCoefficient, drag = dynamicPressurePa * airModel.areaM2 * dragCoefficient
37 const lift = longitudinalPressurePa * airModel.areaM2 * liftCoefficient, drag = longitudinalPressurePa * airModel.areaM2 * dragCoefficient
3338 const ca = Math.cos(alphaRad), sa = Math.sin(alphaRad)
3439 const forceN: SensorVector = [actuators[0]! * airModel.maxThrustN - drag * ca + lift * sa, -dynamicPressurePa * airModel.areaM2 * 0.6 * betaRad, -drag * sa - lift * ca]
3540 const rateScale = Math.max(1, speedMps)
3641 const torqueNm: SensorVector = [
3742 dynamicPressurePa * airModel.areaM2 * airModel.spanM * (0.12 * actuators[1]! - 0.3 * rateRadS[0] * airModel.spanM / (2 * rateScale)),
38 dynamicPressurePa * airModel.areaM2 * airModel.chordM * (0.8 * actuators[2]! - 0.15 * coefficientAlphaRad * chordDirection - 2 * rateRadS[1] * airModel.chordM / (2 * rateScale)),
43 dynamicPressurePa * airModel.areaM2 * airModel.chordM * (0.8 * actuators[2]! - 0.15 * coefficientAlphaRad * chordDirection * planeFraction ** 2 - 2 * rateRadS[1] * airModel.chordM / (2 * rateScale)),
3944 dynamicPressurePa * airModel.areaM2 * airModel.spanM * (0.08 * actuators[3]! + 0.12 * betaRad - 0.25 * rateRadS[2] * airModel.spanM / (2 * rateScale)),
4045 ]
4146 return { forceN, torqueNm, speedMps, alphaRad, betaRad, liftCoefficient, dragCoefficient, dynamicPressurePa, stalled: Math.abs(alphaRad) > airModel.stallAlphaRad }
resources/functions/drone/vehicles/vtol/flight-controller.tsmodified+3-0
Changes to resources/functions/drone/vehicles/vtol/flight-controller.ts
@@ -138,6 +138,9 @@ export function createVtolFlightController(): VehicleController {
138138 if (rotary) {
139139 backSlow ||= speed < 3
140140 if (!backSlow && phase === 'back-transition') {
141 // A requested hover heading is the final capture condition. While
142 // wing braking, keep the nose and force search in measured airflow.
143 heading = Math.atan2(airNed[1], airNed[0])
141144 // The linear pre-stall estimate seeds a bounded search of the actual fictional
142145 // wing/rotor force balance, including stalled drag. Keep the original pitch limits.
143146 const optimalAlpha = Math.max(0, (requiredUpN / Math.max(1, pressureArea) - 0.25) / (2 * 4.8))
resources/functions/drone/vehicles/vtol/plant.tsmodified+2-2
Changes to resources/functions/drone/vehicles/vtol/plant.ts
@@ -9,9 +9,9 @@ import { numericPacket } from './packets'
99import { vtolElectricalLoad, vtolElectricalMaximumW, vtolElectricalModel, vtolElectricalSupply } from './power'
1010
1111/** Fictional lift-and-cruise model. No measured hardware coefficients or flight qualification. */
12export const vtolModel = Object.freeze({ aerodynamicsModel: 'folded-incidence-v2' as const, massKg: 2.5, surfaceFrictionCoefficient: 0.5, gravityMps2: 9.80665, maxRotorThrustN: 12, rotorTimeConstantS: 0.06, rotorSlewNPerS: 120, rotorArmProjectionM: 0.4, reactionTorqueM: 0.02, inertiaKgM2: Object.freeze([0.3, 0.4, 0.6] as const), maxPusherThrustN: airModel.maxThrustN, pusherTimeConstantS: 0.08, pusherSlewNPerS: 60, wing: airModel })
12export const vtolModel = Object.freeze({ aerodynamicsModel: 'projected-incidence-v3' as const, massKg: 2.5, surfaceFrictionCoefficient: 0.5, gravityMps2: 9.80665, maxRotorThrustN: 12, rotorTimeConstantS: 0.06, rotorSlewNPerS: 120, rotorArmProjectionM: 0.4, reactionTorqueM: 0.02, inertiaKgM2: Object.freeze([0.3, 0.4, 0.6] as const), maxPusherThrustN: airModel.maxThrustN, pusherTimeConstantS: 0.08, pusherSlewNPerS: 60, wing: airModel })
1313
14/** Fictional continuous reverse-flow extension, not measured post-stall or reverse-flight data. */
14/** Fictional projected-flow extension, not measured post-stall, spanwise or reverse-flight data. */
1515export function vtolAerodynamics(relativeBodyMps: SensorVector, rateRadS: SensorVector, actuators: number[]) {
1616 return airAerodynamics(relativeBodyMps, rateRadS, actuators, true)
1717}
resources/functions/drone/vehicles/vtol/simulation.tsmodified+2-1
Changes to resources/functions/drone/vehicles/vtol/simulation.ts
@@ -1,4 +1,5 @@
11import { createVtolTransitionProfile } from './transition-profile'
2import { vtolModel } from './plant'
23import { runVehicleSimulation } from '../runtime'
34import type { VehicleMissionAction } from '../types'
45import type { SensorVector } from '../../sensors/types'
@@ -170,7 +171,7 @@ export function createVtolSimulationCase(scenario = 'nominal', options: VtolSimu
170171 const backHeadingMatched = backHeadingRad === null || backTask?.kind === 'vtol-mode' && backTask.mode === 'hover' && backTask.headingRad === backHeadingRad
171172 const windOutcomeMatched = !!wind && (wind.expectedOutcome === 'completed-route' ? completedRoute : result.status === 'blocked' && result.fault === 'vtol-transition-envelope' && result.mission.taskId === 'back' && result.mission.state === 'blocked' && result.mission.targetM === null && result.mission.transition === null && confirmed(['initial-hover', 'front']) && observedFlight && !landingContact && boardNeutral)
172173 const demonstrationMatched = wind ? worldMatched && windOutcomeMatched && backHeadingMatched : scenario === 'nominal' ? completedRoute : actions ? operatorMatched : faultObserved && observedFlight && !landingContact && result.mission.state !== 'completed' && boardNeutral
173 const summary = { ...report, scope: 'development-lift-and-cruise' as const, transitionEvidence, completeTransitions, observedFlight, landingContact, boardNeutral, demonstrationMatched, recordedMissionRows: trace.rows.length, evaluatedPathRows: path.length,
174 const summary = { ...report, scope: 'development-lift-and-cruise' as const, aerodynamicsModel: vtolModel.aerodynamicsModel, transitionEvidence, completeTransitions, observedFlight, landingContact, boardNeutral, demonstrationMatched, recordedMissionRows: trace.rows.length, evaluatedPathRows: path.length,
174175 ...(phase ? { scenarioEvidence: { phase: phase.phase, injectedAtMs: phase.atMs, taskId: before?.mission.taskId ?? null, faultObserved, sources: capturedSources } } : {}),
175176 ...(wind ? { windEvidence: { ...structuredClone(wind), ...exposure, worldMatched, outcomeMatched: windOutcomeMatched, ...(backHeadingRad === null ? {} : { backHeadingMatched }) } } : {}),
176177 ...(actions ? { operatorEvidence: { actions: actualActions, tasksBeforeActions, phaseMatched: operatorPhaseMatched, observedPaused, matched: operatorMatched, ...(operator?.recovery ? { recoveryCorrelated } : {}), ...(operator?.openCircuit ? { pusherOpenCircuitObserved } : {}), ...(operator?.outcome === 'hold-expiry' ? { globalEnduranceExpired, landingStarted } : {}) } } : {}) }
tests/unit/vtol-control.test.tsmodified+28-0
Changes to tests/unit/vtol-control.test.ts
@@ -64,3 +64,31 @@ test('the VTOL body follows a hover position step using raw IMU and GNSS rather
6464 expect(estimate.health.localPosition).toBe(true)
6565 expect(estimate.health.absoluteHeading).toBe(false)
6666})
67
68// The public lift output exposes desired body rates. A yaw goal must not rotate
69// the independent thrust-direction correction when yaw authority is exhausted.
70test('hover tilt correction stays independent of a large yaw heading error', () => {
71 const outputs = [0, Math.PI / 2].map(yawRad => {
72 const input = hoverInput()
73 input.estimate.attitude!.quaternionWxyz = [Math.SQRT1_2, 0, 0, Math.SQRT1_2]
74 input.estimate.attitude!.eulerRad = [0, 0, Math.PI / 2]
75 input.target = { ...input.target, positionM: [2, 1, -10], yawRad }
76 return createVtolHoverController({ headingAlignment: 'initial-simulation-north' }).step(input)
77 })
78 expect(outputs.every(output => !output.inhibited)).toBe(true)
79 expect(outputs[0]!.desiredRatesRadS![0]).toBeCloseTo(outputs[1]!.desiredRatesRadS![0]!, 10)
80 expect(outputs[0]!.desiredRatesRadS![1]).toBeCloseTo(outputs[1]!.desiredRatesRadS![1]!, 10)
81 expect(outputs[0]!.desiredRatesRadS![2]).not.toBe(outputs[1]!.desiredRatesRadS![2])
82})
83
84test('an antipodal thrust direction is inhibited and latched rather than using an undefined tilt axis', () => {
85 const controller = createVtolHoverController({ headingAlignment: 'initial-simulation-north' })
86 const input = hoverInput()
87 input.estimate.attitude!.quaternionWxyz = [0, 1, 0, 0]
88 input.estimate.attitude!.eulerRad = [Math.PI, 0, 0]
89 const result = controller.step(input)
90 expect(result.inhibited).toBe(true)
91 expect(result.reason).toBe('antipodal-thrust-direction')
92 expect(result.motorsN).toEqual([0, 0, 0, 0])
93 expect(controller.step(hoverInput(1020)).inhibited).toBe(true)
94})
tests/unit/vtol-lateral-flow.test.tsadded+30-0
Changes to tests/unit/vtol-lateral-flow.test.ts
@@ -0,0 +1,30 @@
1import { expect, test } from 'bun:test'
2import { vtolAerodynamics } from '../../resources/functions/drone/vehicles/vtol/plant'
3
4// Public fictional-aerodynamics seam: force/moment continuity at finite lateral
5// flow. This does not validate measured coefficients or a physical airframe.
6test('VTOL forces are continuous across zero chordwise flow with finite spanwise airspeed', () => {
7 const above = vtolAerodynamics([1e-8, 2, 0], [0, 0, 0], [0, 0, 0, 0, 0])
8 const below = vtolAerodynamics([-1e-8, 2, 0], [0, 0, 0], [0, 0, 0, 0, 0])
9 expect(Math.hypot(...above.forceN.map((value, i) => value - below.forceN[i]!))).toBeLessThan(1e-5)
10})
11
12test('force and static pitch converge to the lateral-flow limit from every longitudinal direction', () => {
13 for (const v of [-2, 2]) {
14 const center = vtolAerodynamics([0, v, 0], [0, 0, 0], [0, 0, 0, 0, 0])
15 for (const [u, w] of [[1e-8, 0], [-1e-8, 0], [0, 1e-8], [0, -1e-8], [1e-8, 1e-8], [-1e-8, 1e-8], [1e-8, -1e-8], [-1e-8, -1e-8]]) {
16 const next = vtolAerodynamics([u!, v, w!], [0, 0, 0], [0, 0, 0, 0, 0])
17 expect(Math.hypot(...next.forceN.map((value, i) => value - center.forceN[i]!))).toBeLessThan(1e-5)
18 expect(Math.hypot(...next.torqueNm.map((value, i) => value - center.torqueNm[i]!))).toBeLessThan(1e-5)
19 }
20 }
21})
22
23test('the projected unpowered model dissipates airflow energy through zero, lateral and reverse flow', () => {
24 for (const u of [-13, -2, -1e-8, 0, 1e-8, 2, 13]) for (const v of [-2, 0, 2]) for (const w of [-2, 0, 2]) {
25 const response = vtolAerodynamics([u, v, w], [0, 0, 0], [0, 0, 0, 0, 0])
26 expect([...response.forceN, ...response.torqueNm].every(Number.isFinite)).toBe(true)
27 expect(response.forceN[0] * u + response.forceN[1] * v + response.forceN[2] * w).toBeLessThanOrEqual(1e-8)
28 }
29 expect(vtolAerodynamics([0, 0, 0], [0, 0, 0], [0, 0, 0, 0, 0]).forceN).toEqual([0, -0, -0])
30})
tests/unit/vtol-transition-cli.test.tsmodified+1-0
Changes to tests/unit/vtol-transition-cli.test.ts
@@ -28,6 +28,7 @@ test('native VTOL full-route command reports qualified initial hover, cruise, ba
2828 const report = JSON.parse(result.stdout)
2929 expect(report.status).toBe('completed')
3030 expect(report.scope).toBe('development-lift-and-cruise')
31 expect(report.aerodynamicsModel).toBe('projected-incidence-v3')
3132 expect(report.demonstrationMatched).toBe(true)
3233 expect(report.transitionEvidence.map((row: { taskId: string, phase: string }) => [row.taskId, row.phase])).toEqual([
3334 ['initial-hover', 'hover'], ['front', 'cruise'], ['back', 'hover'],
tests/unit/vtol-wind-onset.test.tsadded+59-0
Changes to tests/unit/vtol-wind-onset.test.ts
@@ -0,0 +1,59 @@
1import { expect, test } from 'bun:test'
2import { createVtolTransitionProfile } from '../../resources/functions/drone/vehicles/vtol/transition-profile'
3import { runVehicleSimulation } from '../../resources/functions/drone/vehicles/runtime'
4import { replayVehicleMission } from '../../resources/functions/drone/missions/vehicle'
5import type { SensorVector } from '../../resources/functions/drone/sensors/types'
6
7const winds = [
8 { name: 'head', vector: [-2, 0, 0], heading: 0 },
9 { name: 'tail', vector: [2, 0, 0], heading: 0 },
10 { name: 'east', vector: [0, 2, 0], heading: -Math.PI / 2 },
11 { name: 'west', vector: [0, -2, 0], heading: Math.PI / 2 },
12] as const
13const phases = [{ name: 'front', task: 'front' }, { name: 'cruise', task: 'cruise-leg' }, { name: 'back', task: 'back' }, { name: 'landing', task: 'land' }] as const
14
15// Actual-runtime seam. The external weather step follows an observed public
16// mission phase, never a guessed timestamp or a controller wind input.
17for (const wind of winds) for (const phase of phases) test(`${wind.name} wind begins at the actual ${phase.name} phase and completes the declared heading plan`, () => {
18 const profile = createVtolTransitionProfile({ electrical: true }), factory = profile.createSession
19 const vector: SensorVector = [...wind.vector]
20 const back = profile.mission.tasks.find(task => task.id === 'back')
21 if (!back || back.kind !== 'vtol-mode') throw new Error('Missing back heading')
22 back.headingRad = wind.heading
23 let onset: number | null = null, enteredAt: number | null = null, firstApplied: number | null = null, applied = false
24 profile.faults = (_scenario, atMs) => ({ windMps: onset !== null && atMs >= onset ? vector : [0, 0, 0] })
25 profile.createSession = () => {
26 const session = factory()
27 return { ...session, createController() {
28 const controller = session.createController()
29 return { step(context) {
30 if (onset === null && context.enabled && context.mission.state === 'running' && context.mission.taskId === phase.task) { enteredAt = context.nowMs; onset = context.nowMs + 4 }
31 return controller.step(context)
32 } }
33 }, createPlant() {
34 const plant = session.createPlant()
35 return { snapshot: () => plant.snapshot(), step(actuators, dtS, world) {
36 const state = plant.step(actuators, dtS, world)
37 applied = world.windMps.every((value, i) => value === vector[i])
38 return state
39 } }
40 }, observeIndependent(state, atMs, actuators, hostAvailable) {
41 session.observeIndependent?.(state, atMs, actuators, hostAvailable)
42 if (applied && firstApplied === null) firstApplied = atMs
43 } }
44 }
45 const result = runVehicleSimulation(profile, { seconds: 160 })
46 expect(enteredAt).not.toBeNull()
47 expect(onset).toBe(enteredAt! + 4)
48 expect(firstApplied).toBe(onset)
49 expect(result.status).toBe('completed')
50 expect(result.fault).toBeNull()
51 expect(result.budget).toEqual({ worstCaseMs: 131520, requiredWh: 43.84 })
52 const confirmations = result.trace.rows.flatMap(row => row.observation.transition?.outcome === 'confirmed' ? [row.observation.transition] : [])
53 expect(confirmations.map(row => [row.taskId, row.phase])).toEqual([['initial-hover', 'hover'], ['front', 'cruise'], ['back', 'hover']])
54 expect(result.path.at(-1)!.measurement!.values.onSurface).toBe(true)
55 expect(result.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0])
56 expect(result.board.expiresAtMs).toBeNull()
57 expect(result.board.physicalActuationEnabled).toBe(false)
58 expect(replayVehicleMission(result.trace).snapshot()).toEqual(result.mission)
59})