ReviewOS
Reload

also looking at this

stacks/drone

feat: model owned VTOL flight transitions

#78
Merged chris wants to merge codex/owned-vtol-flight-controls into main
13 files +1087 -33

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

docs/vehicles-vtol.mdmodified+50-7
@@ -3,7 +3,8 @@
33The owned lift-and-cruise model can execute a **hover-only** mission through the shared raw sensor,
44estimator, mission, authority and independent output-lease runtime. It takes off vertically, reaches
55a nearby hover waypoint and lands against fresh modeled contact evidence. Wing transitions and
6cruise control are not implemented. This development profile is separate from the selectable
6cruise control are not implemented in this hover profile. A separate experimental library controller
7is described below. This development profile is separate from the selectable
78profiles in the `/missions` workspace.
89
910```sh
@@ -64,14 +65,56 @@ neutral until observation is qualified. Unobserved headings expire, and this hov
6465cruise requests as aborted. This is a library prerequisite, not front/back transition control or a
6566new CLI scenario. Existing hover-only recordings keep their prior shapes.
6667
68## Experimental lift-and-cruise library controller
69
70`createVtolTransitionProfile()` in `resources/functions/drone/vehicles/vtol/transition-profile.ts`
71is a separate development experiment on the same owned runtime. Its nominal mission observes initial
72hover, takes off vertically to 15 m, requests a front transition, flies a cruise leg, requests back
73transition, returns to the back request's capture point, observes settled hover, and lands vertically.
74A turning leg also has an actual completed model campaign. This profile is not registered as a CLI
75scenario or selectable workspace profile.
76
77The controller receives measured relative airspeed, differential pressure, airflow angles and modeled
78rotor/pusher response, together with the qualified raw gyro and estimated navigation. Delayed airflow
79angles are transformed through their matching capture-time attitude. The measured NED airflow vector
80is held to the decision and then expressed in the current gyro-predicted body attitude. This is a
81bounded synthetic observation approximation, not identified wind estimation or physical air-data
82calibration. Banked navigation follows the route while coordinated yaw follows the measured air
83course. Surface torque and residual rotor torque are allocated together; measured wing lift reduces
84collective rather than adding full rotor weight support on top of the wing.
85
86Front confirmation requires sustained measured 12 to 14 m/s relative airflow, bounded angles,
87height/rate and less than 1 N of measured and commanded lift-rotor support. Both decision time and
88oldest-source coverage must qualify for 500 ms, including the attitude and gyro epochs used to
89transform delayed airflow. Ground speed or a timer alone cannot confirm cruise.
90A failed modeled pusher and an artificial co-moving-air fixture retain failed transition outcomes.
91
92Back transition turns the pusher off and decelerates under the declared aerodynamic model before
93returning to the original capture point at a horizontal target speed bounded to 3 m/s. The nominal
94model needs about 38 seconds for deceleration, return and hover qualification. Observed hover also
95requires wrapped heading error within 0.1 rad for the complete 500 ms dwell. Hover/back requests
96therefore have a finite 45-second ceiling; front requests retain 15 seconds. The original 15-second
97back budget still fails without retry or a claimed landing. This is a virtual task allowance,
98not a wall-clock test timeout. The complete declared mission budget is 131,520 ms, or 43.84 Wh at its
991200 W ceiling, with 100 Wh capacity and 30% reserve. These remain declared planning values, without
100a complete hybrid electrical-power law or measured battery identification.
101
102The experiment keeps the 2 m transition-height deviation, 0.6 rad roll, 0.5 rad pitch and 22 m/s
103speed guards. Its own target range is bounded to 400 m. The quadcopter and hover development kernel's
104existing 3 m/s horizontal target and 100 m target-radius guards are unchanged. Source, capture-pose,
105reference and clock faults latch. Real modeled sensor loss, RC override, kill and host-loss campaigns
106cover front transition, cruise, back transition and post-back landing through the independent output
107lease. Neutral outputs during an airborne failure do not demonstrate a safe recovery or landing.
108
67109## Remaining implementation
68110
69[Issue #12](https://github.com/stacksjs/drone/issues/12) remains open for full correlated front/back
70transition control and observed outcomes, airspeed and attitude
71qualification, bounded transition abort/recovery, complete hybrid power/reserve accounting and
72the common mission workspace. Full TECS/soaring, tiltrotor/tailsitter mechanisms, helicopters and
73other airframe families remain separate work. Native board control, measured airframe identification,
74HIL and physical flight require their own evidence.
111[Issue #12](https://github.com/stacksjs/drone/issues/12) remains open. The experimental nominal
112controller does not establish complete VTOL support. Phase pause/recovery, explicit bounded
113transition abort and landing, the complete hybrid power/reserve model, native transition CLI and
114the common mission workspace still require implementation and qualification. Wind campaigns and
115broader mission/envelope coverage remain necessary. Full TECS/soaring, tiltrotor/tailsitter mechanisms,
116helicopters and other airframe families remain separate work. Native board control, measured airframe
117identification, HIL and physical flight require their own evidence.
75118
76119Read the [shared vehicle contract](vehicles.md), [fixed-wing model](vehicles-air.md) and
77120[authority contract](authority.md).
resources/functions/drone/missions/vehicle.tsmodified+1-1
@@ -28,7 +28,7 @@ export function validateVehicleMission(input: unknown): { plan: VehicleMissionPl
2828 else if (t.kind === 'abort') { /* Terminal release, never an emergency landing. */ }
2929 else if (t.kind === 'vtol-mode') {
3030 fields.push('mode', 'headingRad', 'timeoutMs', 'next')
31 if (p.vehicle !== 'vtol' || !['hover', 'cruise'].includes(t.mode) || !number(t.headingRad, -Math.PI, Math.PI) || !Number.isSafeInteger(t.timeoutMs) || !number(t.timeoutMs, 100, 15000) || !id(t.next)) invalid()
31 if (p.vehicle !== 'vtol' || !['hover', 'cruise'].includes(t.mode) || !number(t.headingRad, -Math.PI, Math.PI) || !Number.isSafeInteger(t.timeoutMs) || !number(t.timeoutMs, 100, t.mode === 'hover' ? 45000 : 15000) || !id(t.next)) invalid()
3232 durationMs = t.timeoutMs; edges = [t.next]
3333 }
3434 else if (t.kind === 'branch') {
resources/functions/drone/vehicles/vtol/flight-controller.tsadded+158-0
@@ -0,0 +1,158 @@
1import type { VehicleController, VehicleControllerContext, VehicleControlOutput, VehicleVtolTransitionCommand, VehicleVtolTransitionObservation } from '../types'
2import type { SensorVector } from '../../sensors/types'
3import type { Quaternion } from '../../estimation/frames'
4import { createVehicleMeasurementQualifier, vehicleReference } from '../measurements'
5import { quaternionEuler, quaternionFromTilt, multiplyQuaternion, normalizeQuaternion } from '../../estimation/frames'
6import { rotateBodyToNed, rotateNedToBody } from '../../controls/physics'
7import { createBoundedPid } from '../../controls/pid'
8import { airAerodynamics, airModel } from '../air/plant'
9import { createVtolVehicleHoverController } from './controller'
10import { numericPacket } from './packets'
11import { allocateVtolLift } from './allocation'
12import { vtolModel } from './plant'
13
14const clamp = (n: number, low: number, high: number) => Math.max(low, Math.min(high, n))
15const wrap = (n: number) => Math.atan2(Math.sin(n), Math.cos(n))
16
17/** Original fictional pressure/force-aware lift-and-cruise control, undergoing owned-model qualification. */
18export function createVtolFlightController(): VehicleController {
19 const qualifier = createVehicleMeasurementQualifier('vtol'), hover = createVtolVehicleHoverController()
20 const ratePid = [0, 1, 2].map(i => createBoundedPid({ kp: i === 2 ? 0.96 : 1.6, ki: i === 2 ? 0.48 : 0.8, kd: 0.02, integralLimit: 1, outputLimit: i === 2 ? 2 : 6, derivativeTauS: 0.03, trackingGain: 4 }))
21 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 }))
22 let hasWingControl = false, backSlow = false
23 let lastNow: number | null = null, identity: string | null = null, fault = ''
24 let phase: VehicleVtolTransitionObservation['phase'] = 'hover', request: VehicleVtolTransitionCommand | null = null, signature = '', confirmedEvent = ''
25 let stableSource: number | null = null, stableDecision: number | null = null
26 function output(c: VehicleControllerContext, actuators: number[], reason: string, inhibited = false): VehicleControlOutput { return { kind: 'simulated-vehicle-control', vehicle: 'vtol', atMs: c.nowMs, actuators: [...actuators], reason, inhibited, physicalFlightEnabled: false, boardOutputsInhibited: true } }
27 function neutral(c: VehicleControllerContext, reason: string) { fault ||= reason; return output(c, [0, 0, 0, 0, 0, 0, 0, 0], fault, true) }
28 return {
29 step(c) {
30 if (!Number.isSafeInteger(c.nowMs) || c.nowMs < 0 || lastNow !== null && (c.nowMs <= lastNow || c.nowMs - lastNow > 40)) return neutral(c, 'vtol-flight-clock-gap')
31 const dtS = lastNow === null ? 0.02 : (c.nowMs - lastNow) / 1000
32 lastNow = c.nowMs
33 if (!c.enabled || fault) return output(c, [0, 0, 0, 0, 0, 0, 0, 0], fault || 'authority-disabled', true)
34 let measurement, position: SensorVector, velocity: SensorVector, q: Quaternion, captureQ: Quaternion, rates: SensorVector, target: SensorVector
35 try {
36 measurement = qualifier.qualify(c.measurement, c.nowMs, vehicleReference(c.estimate))
37 const e = c.estimate, p = e.localPosition, a = e.attitude, rate = c.controlRate
38 if (!measurement || c.mission.vehicle !== 'vtol' || e.evidence !== 'fixture' || e.physicalFlightEnabled !== false || typeof e.bootId !== 'bigint' || e.bootId <= 0n || !Object.values(e.resets).every(n => Number.isSafeInteger(n) && n >= 0) || e.nowMs !== c.nowMs || !e.health.tilt || !e.health.localPosition || !p || !a || !rate || rate.evidence !== 'modeled-qualified-imu' || rate.reference !== measurement.reference || a.reference !== 'GRAVITY_ALIGNED_RELATIVE_YAW' || p.frame !== 'LOCAL_NED' || p.altitudeDatum !== 'WGS84_ELLIPSOID' || p.predictedToMs !== c.nowMs || p.capturedAtMs === null || a.capturedAtMs === null || !Number.isSafeInteger(p.capturedAtMs) || !Number.isSafeInteger(a.capturedAtMs) || p.capturedAtMs > c.nowMs || c.nowMs - p.capturedAtMs > 200 || a.capturedAtMs > c.nowMs || c.nowMs - a.capturedAtMs > 60 || rate.capturedAtMs !== a.capturedAtMs) throw new TypeError('Unqualified navigation')
39 position = numericPacket(p.positionM, 3) as SensorVector; velocity = numericPacket(p.velocityMps, 3) as SensorVector
40 q = numericPacket(a.quaternionWxyz, 4) as Quaternion; rates = numericPacket(rate.angularRateRadS, 3) as SensorVector
41 target = numericPacket(c.mission.targetM, 3) as SensorVector
42 const pose = c.measurementPose
43 if (!pose || pose.reference !== measurement.reference || pose.capturedAtMs !== measurement.capturedAtMs || pose.attitudePrediction !== 'qualified-gyro-hold' || pose.attitudePredictedToMs !== pose.capturedAtMs || pose.positionPredictedToMs !== pose.capturedAtMs || ![pose.attitudeCapturedAtMs, pose.gyroCapturedAtMs, pose.positionCapturedAtMs].every(n => Number.isSafeInteger(n) && n >= 0) || pose.attitudeCapturedAtMs !== pose.gyroCapturedAtMs || pose.gyroCapturedAtMs > pose.capturedAtMs || pose.capturedAtMs - pose.gyroCapturedAtMs > 60 || pose.positionCapturedAtMs > pose.capturedAtMs || pose.capturedAtMs - pose.positionCapturedAtMs > 200) throw new TypeError('Unmatched air capture pose')
44 captureQ = numericPacket(pose.quaternionWxyz, 4) as Quaternion
45 if (Math.abs(Math.hypot(...captureQ) - 1) > 1e-6) throw new TypeError('Invalid air capture attitude')
46 if (Math.abs(Math.hypot(...q) - 1) > 1e-6 || Math.hypot(...rates) > 35 || Math.hypot(...target) > 400 || Math.hypot(...position) > 500 || !p.origin || ![p.origin.latitudeDeg, p.origin.longitudeDeg, p.origin.ellipsoidHeightM].every(Number.isFinite)) throw new TypeError('Invalid navigation values')
47 const nextIdentity = JSON.stringify([String(e.bootId), e.resets, p.origin])
48 if (identity !== null && identity !== nextIdentity) throw new TypeError('Changed navigation reference')
49 identity = nextIdentity
50 const heldS = (c.nowMs - rate.capturedAtMs) / 1000, magnitude = Math.hypot(...rates), angle = magnitude * heldS
51 const scale = magnitude < 1e-10 ? heldS / 2 : Math.sin(angle / 2) / magnitude
52 q = normalizeQuaternion(multiplyQuaternion(q, [Math.cos(angle / 2), rates[0] * scale, rates[1] * scale, rates[2] * scale]))
53 const v = measurement.values
54 for (const key of ['airspeedMps', 'differentialPressurePa', 'alphaRad', 'betaRad', 'rotorThrustN', 'pusherThrustN']) if (typeof v[key] !== 'number' || !Number.isFinite(v[key])) throw new TypeError('Missing VTOL air measurement')
55 if ((v.airspeedMps as number) < 0 || (v.airspeedMps as number) > 60 || (v.differentialPressurePa as number) < 0 || Math.abs(Math.sqrt(2 * (v.differentialPressurePa as number) / airModel.densityKgM3) - (v.airspeedMps as number)) > 0.02 || Math.abs(v.alphaRad as number) > Math.PI || Math.abs(v.betaRad as number) > Math.PI || (v.rotorThrustN as number) < 0 || (v.rotorThrustN as number) > 48 || (v.pusherThrustN as number) < 0 || (v.pusherThrustN as number) > 14) throw new TypeError('Invalid VTOL air measurement')
56 }
57 catch { return neutral(c, 'vtol-flight-observation-unqualified') }
58 const incoming = c.mission.transition
59 if (incoming) {
60 let anchor: number[]
61 try { anchor = numericPacket(incoming.positionM, 3) }
62 catch { return neutral(c, 'vtol-flight-request-unqualified') }
63 if (c.mission.taskKind !== 'vtol-mode' || incoming.taskId !== c.mission.taskId || typeof incoming.eventId !== 'string' || !incoming.eventId || incoming.eventId.length > 256 || incoming.reference !== measurement!.reference || !['hover', 'cruise'].includes(incoming.mode) || !Number.isFinite(incoming.headingRad) || Math.abs(incoming.headingRad) > Math.PI || anchor.some((n, i) => n !== target[i]) || !Number.isSafeInteger(incoming.issuedAtMs) || incoming.issuedAtMs < 0 || !Number.isSafeInteger(incoming.expiresAtMs) || incoming.issuedAtMs > c.nowMs || incoming.expiresAtMs <= c.nowMs || incoming.expiresAtMs <= incoming.issuedAtMs || incoming.expiresAtMs - incoming.issuedAtMs > (incoming.mode === 'hover' ? 45000 : 15000)) return neutral(c, 'vtol-flight-request-unqualified')
64 const encoded = JSON.stringify(incoming)
65 if (request?.eventId === incoming.eventId && signature !== encoded) return neutral(c, 'vtol-flight-request-changed')
66 if (request?.eventId !== incoming.eventId && incoming.mode === 'cruise') {
67 if (phase !== 'hover' || measurement!.values.onSurface) return neutral(c, 'vtol-front-requires-airborne-hover')
68 request = structuredClone(incoming); signature = encoded; phase = 'front-transition'; hasWingControl = true; stableSource = null; stableDecision = null
69 }
70 if (request?.eventId !== incoming.eventId && incoming.mode === 'hover' && hasWingControl) {
71 if (phase !== 'cruise') return neutral(c, 'vtol-back-requires-observed-cruise')
72 request = structuredClone(incoming); signature = encoded; phase = 'back-transition'; backSlow = false; stableSource = null; stableDecision = null
73 for (const pid of velocityPid) pid.reset()
74 }
75 }
76 if (phase === 'hover' && !hasWingControl) return hover.step(c)
77 const [roll, pitch, yaw] = quaternionEuler(q!), values = measurement!.values
78 const speed = values.airspeedMps as number, alpha = values.alphaRad as number, beta = values.betaRad as number
79 const landing = phase === 'hover' && c.mission.state === 'running' && c.mission.taskKind === 'land'
80 if (measurement!.values.onSurface && !landing || speed > 22 || Math.abs(roll) > 0.6 || Math.abs(pitch) > 0.5 || speed > 8 && (Math.abs(alpha) > 0.32 || Math.abs(beta) > 0.4) || phase !== 'hover' && Math.abs(position![2] - request!.positionM[2]) > 2) return neutral(c, 'vtol-transition-envelope')
81 if (landing && measurement!.values.onSurface) return output(c, [0, 0, 0, 0, 0, 0, 0, 0], 'vertical-contact')
82 const rotary = phase === 'back-transition' || phase === 'hover'
83 const heading = phase === 'front-transition' || rotary ? request!.headingRad : Math.atan2(target![1] - position![1], target![0] - position![0])
84 const course = Math.hypot(velocity![0], velocity![1]) > 3 ? Math.atan2(velocity![1], velocity![0]) : yaw
85 let bankTarget = clamp(wrap(heading - course) * 0.8, -0.3, 0.3)
86 let downAcceleration = clamp((target![2] - position![2]) * 1.2 - velocity![2] * 1.5, -3, 3)
87 const capturedRelative: SensorVector = [speed * Math.cos(alpha) * Math.cos(beta), speed * Math.sin(beta), speed * Math.sin(alpha) * Math.cos(beta)]
88 // Delayed BODY_FRD airflow belongs to its qualified capture pose. Hold that measured NED
89 // airflow vector to this decision, then express it in the current gyro-predicted attitude.
90 const airNed = rotateBodyToNed(captureQ!, capturedRelative)
91 const relative = rotateNedToBody(q!, airNed)
92 const gamma = Math.atan2(-airNed[2], Math.hypot(airNed[0], airNed[1]))
93 const pressureArea = (values.differentialPressurePa as number) * airModel.areaM2
94 let requiredUpN = vtolModel.massKg * (vtolModel.gravityMps2 - downAcceleration)
95 const alphaTarget = clamp((requiredUpN / Math.max(1, pressureArea * Math.cos(bankTarget)) - 0.25) / 4.8, -0.08, 0.18)
96 let pitchTarget = clamp(gamma + alphaTarget, -0.2, 0.25) * clamp((speed - 10) / 2, 0, 1)
97 const zero = airAerodynamics(relative, rates!, [0, 0, 0, 0, 0])
98 if (rotary) {
99 backSlow ||= speed < 3
100 if (!backSlow && phase === 'back-transition') {
101 // With the pusher off, high-speed braking is bounded by wing drag and the lift surplus.
102 // The linear pre-stall lift slope gives the pitch that maximizes supported rotor braking.
103 const optimalAlpha = Math.max(0, (requiredUpN / Math.max(1, pressureArea) - 0.25) / (2 * 4.8))
104 pitchTarget = clamp(gamma + optimalAlpha, -0.08, 0.3)
105 bankTarget = 0
106 }
107 else {
108 const goal = [...target!] as SensorVector
109 if (landing) goal[2] += 0.1
110 const desiredVelocity = goal.map((n, i) => n - position![i]!) as SensorVector
111 const horizontal = Math.hypot(desiredVelocity[0], desiredVelocity[1])
112 if (horizontal > 3) { desiredVelocity[0] *= 3 / horizontal; desiredVelocity[1] *= 3 / horizontal }
113 desiredVelocity[2] = clamp(desiredVelocity[2], -2, 2)
114 const acceleration = desiredVelocity.map((n, i) => velocityPid[i]!.step(n - velocity![i]!, velocity![i]!, dtS)) as SensorVector
115 const aHorizontal = Math.hypot(acceleration[0], acceleration[1])
116 if (aHorizontal > 3) { acceleration[0] *= 3 / aHorizontal; acceleration[1] *= 3 / aHorizontal }
117 downAcceleration = acceleration[2]; requiredUpN = vtolModel.massKg * (vtolModel.gravityMps2 - downAcceleration)
118 const zeroNed = rotateBodyToNed(q!, zero.forceN)
119 const fx = vtolModel.massKg * acceleration[0] - zeroNed[0], fy = vtolModel.massKg * acceleration[1] - zeroNed[1]
120 const forward = Math.cos(heading) * fx + Math.sin(heading) * fy, right = -Math.sin(heading) * fx + Math.cos(heading) * fy
121 const up = Math.max(1, requiredUpN + zeroNed[2])
122 pitchTarget = clamp(Math.atan2(-forward, up), -0.4, 0.4)
123 bankTarget = clamp(Math.atan2(right, Math.hypot(forward, up)), -0.4, 0.4)
124 }
125 }
126 // Wing navigation banks toward the route while the nose follows the measured air course.
127 // An instantaneous waypoint bearing is not a physically coordinated yaw command.
128 const yawTarget = phase === 'cruise' ? Math.atan2(airNed[1], airNed[0]) : heading
129 const desired = multiplyQuaternion([Math.cos(yawTarget / 2), 0, 0, Math.sin(yawTarget / 2)], quaternionFromTilt(bankTarget, pitchTarget))
130 const error = multiplyQuaternion([q![0], -q![1], -q![2], -q![3]], desired)
131 const turnRate = phase === 'cruise' ? vtolModel.gravityMps2 * Math.tan(bankTarget) / Math.max(8, speed) : 0
132 const turnRates = rotateNedToBody(q!, [0, 0, turnRate])
133 const desiredRates = error.slice(1).map((n, i) => clamp(8 * n * (error[0] < 0 ? -1 : 1) + turnRates[i]!, i === 2 ? -1.5 : -3, i === 2 ? 1.5 : 3))
134 const torque = desiredRates.map((n, i) => ratePid[i]!.step(n - rates![i]!, rates![i]!, dtS)) as SensorVector
135 const pusher = rotary ? 0 : clamp((-zero.forceN[0] + vtolModel.massKg * clamp((13 - speed) * 1.2, -2.5, 2.5)) / vtolModel.maxPusherThrustN, 0, 1)
136 const coefficients = [pressureArea * airModel.spanM * 0.12, pressureArea * airModel.chordM * 0.8, pressureArea * airModel.spanM * 0.08]
137 const surfaces = torque.map((n, i) => coefficients[i]! > 1e-6 ? clamp((n - zero.torqueNm[i]!) / coefficients[i]!, -0.4, 0.4) : 0)
138 const aero = airAerodynamics(relative, rates!, [pusher, ...surfaces, 0])
139 const aeroNed = rotateBodyToNed(q!, aero.forceN)
140 const collective = clamp((requiredUpN + aeroNed[2]) / Math.max(0.7, Math.cos(roll) * Math.cos(pitch)), 0, 48)
141 const allocation = allocateVtolLift({ thrustN: collective, torqueNm: torque.map((n, i) => n - aero.torqueNm[i]!) as SensorVector })
142 for (let i = 0; i < 3; i++) ratePid[i]!.track(allocation.achieved.torqueNm[i]! + aero.torqueNm[i]! - torque[i]!, dtS)
143 const response = output(c, [...allocation.rotorsN, pusher, ...surfaces], phase === 'front-transition' ? 'force-managed-front-transition' : phase === 'cruise' ? 'wing-borne-cruise' : phase === 'back-transition' ? 'force-managed-back-transition' : 'force-managed-hover')
144 const source = Math.min(measurement!.capturedAtMs, c.measurementPose!.attitudeCapturedAtMs, c.measurementPose!.gyroCapturedAtMs, c.estimate.attitude!.capturedAtMs!, c.estimate.localPosition!.capturedAtMs!, c.controlRate!.capturedAtMs)
145 if (incoming && incoming.eventId !== confirmedEvent && source >= incoming.issuedAtMs) {
146 const stable = incoming.mode === 'cruise'
147 ? speed >= 12 && speed <= 14 && Math.abs(alpha) < 0.15 && Math.abs(beta) < 0.15 && Math.abs(roll) < 0.25 && Math.abs(pitch) < 0.25 && Math.abs(velocity![2]) < 0.4 && Math.abs(position![2] - incoming.positionM[2]) < 1 && (values.rotorThrustN as number) < 1 && collective < 1
148 : Math.hypot(...velocity!) <= 0.2 && Math.hypot(...rates!) < 0.1 && Math.abs(wrap(incoming.headingRad - yaw)) <= 0.1 && Math.abs(roll) < 0.1 && Math.abs(pitch) < 0.1 && Math.hypot(...position!.map((n, i) => n - incoming.positionM[i]!)) < 0.3 && (values.rotorThrustN as number) > 20 && (values.rotorThrustN as number) < 30
149 if (!stable) { stableSource = null; stableDecision = null }
150 else { stableSource ??= source; stableDecision ??= c.nowMs }
151 const confirmed = stable && source - stableSource! >= 500 && c.nowMs - stableDecision! >= 500
152 if (confirmed) { phase = incoming.mode; confirmedEvent = incoming.eventId }
153 response.transition = { evidence: 'modeled-vtol-controller-observation', eventId: incoming.eventId, taskId: incoming.taskId, mode: incoming.mode, reference: incoming.reference, capturedAtMs: c.nowMs, measurementCapturedAtMs: source, phase, outcome: confirmed ? 'confirmed' : 'progress', reason: confirmed ? incoming.mode === 'cruise' ? 'wing-support-qualified' : 'hover-support-qualified' : incoming.mode === 'cruise' ? 'wing-support-observing' : 'hover-support-observing' }
154 }
155 return response
156 },
157 }
158}
resources/functions/drone/vehicles/vtol/transition-profile.tsadded+38-0
@@ -0,0 +1,38 @@
1import type { VehicleProfile } from '../types'
2import { createVtolHoverProfile } from './index'
3import { createVtolPlant } from './plant'
4import { createVtolFlightController } from './flight-controller'
5
6/** Declared fictional transition experiment, undergoing controller qualification. No physical IO. */
7export function createVtolTransitionProfile(): VehicleProfile {
8 const base = createVtolHoverProfile()
9 return {
10 ...base, scenario: 'nominal', scenarios: ['nominal'], name: 'Owned VTOL lift-and-cruise development',
11 authorityFence: { radiusM: 400, minDownM: -40, maxDownM: 1 },
12 mission: { ...base.mission, id: 'vtol-transition-development', fence: { radiusM: 400, minDownM: -40, maxDownM: 1 }, rallyM: [0, 0, -15],
13 power: { availableWh: 100, maxW: 1200, reservePercent: 30 },
14 tasks: [
15 { id: 'start', kind: 'start', next: 'initial-hover' },
16 { id: 'initial-hover', kind: 'vtol-mode', mode: 'hover', headingRad: 0, timeoutMs: 1500, next: 'depart' },
17 { id: 'depart', kind: 'takeoff', positionM: [0, 0, -15], acceptanceM: 0.25, maxArrivalSpeedMps: 0.2, settleMs: 200, timeoutMs: 20000, next: 'front' },
18 { id: 'front', kind: 'vtol-mode', mode: 'cruise', headingRad: 0, timeoutMs: 15000, next: 'cruise-leg' },
19 { id: 'cruise-leg', kind: 'waypoint', positionM: [180, 0, -15], acceptanceM: 8, maxArrivalSpeedMps: 15, settleMs: 0, timeoutMs: 25000, next: 'back' },
20 { id: 'back', kind: 'vtol-mode', mode: 'hover', headingRad: 0, timeoutMs: 45000, next: 'land' },
21 { id: 'land', kind: 'land', positionM: [180, 0, 0], acceptanceM: 0.3, maxArrivalSpeedMps: 0.2, settleMs: 200, timeoutMs: 25000 },
22 ],
23 },
24 createSession() {
25 return { createPlant: createVtolPlant, createController: createVtolFlightController,
26 renderMeasurement(state, atMs, reference) {
27 const rounded = (n: number) => Math.round(n * 1000000) / 1000000
28 return { version: 1, evidence: 'modeled-vehicle-sensors', vehicle: 'vtol', sequence: Math.floor(atMs / 20) + 1, reference, capturedAtMs: atMs, receivedAtMs: atMs + 60,
29 values: { onSurface: state.onSurface, stopped: Math.hypot(...state.velocityMps) <= 0.2,
30 airspeedMps: rounded(state.details.airspeedMps as number), differentialPressurePa: rounded(state.details.dynamicPressurePa as number),
31 alphaRad: rounded(state.details.alphaRad as number), betaRad: rounded(state.details.betaRad as number),
32 rotorThrustN: rounded(state.details.rotorThrustN as number), pusherThrustN: rounded(state.details.pusherThrustN as number) } }
33 },
34 }
35 },
36 faults() { return {} },
37 }
38}
tests/tsconfig.runtime.jsonmodified+1-0
@@ -77,6 +77,7 @@
7777 "./unit/vtol-runtime.test.ts",
7878 "./unit/vtol-cli.test.ts",
7979 "./unit/vtol-mode-mission.test.ts",
80 "./unit/vtol-transition.test.ts",
8081 "./unit/ground.test.ts",
8182 "./unit/surface.test.ts",
8283 "./unit/mission-workspace.test.ts",
tests/unit/vtol-transition.test.tsadded+335-0
@@ -0,0 +1,335 @@
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 { createVtolFlightController } from '../../resources/functions/drone/vehicles/vtol/flight-controller'
5import { multiplyQuaternion, quaternionEuler } from '../../resources/functions/drone/estimation/frames'
6import { replayVehicleMission, validateVehicleMission } from '../../resources/functions/drone/missions/vehicle'
7import type { VehicleControllerContext, VehiclePlantState } from '../../resources/functions/drone/vehicles/types'
8
9// Agreed shared-runtime seam: actual front-transition dynamics and capture-bound observation;
10// independent model probes are assertions only, never controller inputs or physical evidence.
11test('an owned VTOL front transition transfers lift to the wing from actual relative-airflow observations', () => {
12 const profile = createVtolTransitionProfile(), createSession = profile.createSession
13 const independent: { atMs: number, state: VehiclePlantState }[] = [], inputs: VehicleControllerContext[] = []
14 profile.createSession = () => {
15 const session = createSession(), controller = session.createController()
16 return { ...session, createController: () => ({ step(c) { inputs.push(structuredClone(c)); return controller.step(c) } }),
17 observeIndependent(state, atMs) { independent.push({ atMs, state: structuredClone(state) }) } }
18 }
19 const report = runVehicleSimulation(profile, { seconds: 35 })
20 const front = report.trace.rows.filter(row => row.result.taskId === 'front')
21 expect(front.length).toBeGreaterThan(10)
22 const confirmed = report.trace.rows.filter(row => row.observation.transition?.mode === 'cruise' && row.observation.transition.outcome === 'confirmed')
23 expect(confirmed).toHaveLength(1)
24 const row = confirmed[0]!, request = front.find(row => row.result.transition)?.result.transition!
25 expect(row.observation.transition!.phase).toBe('cruise')
26 expect(row.observation.transition!.measurementCapturedAtMs).toBeGreaterThanOrEqual(request.issuedAtMs + 500)
27 expect(row.result.taskId).toBe('cruise-leg')
28 const before = independent.filter(p => p.atMs >= request.issuedAtMs && p.atMs <= row.observation.nowMs)
29 expect(before.some(p => (p.state.details.airspeedMps as number) > 12)).toBe(true)
30 const wingBorne = before.filter(p => p.atMs >= row.observation.nowMs - 500)
31 expect(wingBorne.length).toBeGreaterThan(100)
32 expect(wingBorne.every(p => (p.state.details.rotorThrustN as number) < 1)).toBe(true)
33 expect(wingBorne.every(p => (p.state.details.airspeedMps as number) >= 12 && (p.state.details.airspeedMps as number) <= 14)).toBe(true)
34 expect(before.every(p => Math.abs(p.state.positionM[2] + 15) < 2)).toBe(true)
35 expect(before.every(p => quaternionEuler(p.state.quaternionWxyz).slice(0, 2).every(n => Math.abs(n) < 0.45))).toBe(true)
36 expect(report.path.some(p => p.actuators[4]! > 0)).toBe(true)
37 expect(inputs.every(c => !Object.hasOwn(c, 'truth') && c.estimate.evidence === 'fixture' && c.controlRate?.evidence === 'modeled-qualified-imu')).toBe(true)
38 expect(report.physicalFlightEnabled).toBe(false)
39 expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission)
40})
41
42
43test('the owned VTOL route returns from wing-borne cruise to captured hover and a vertical landing', () => {
44 const report = runVehicleSimulation(createVtolTransitionProfile(), { seconds: 160 })
45 expect(report.status).toBe('completed')
46 const phases = report.trace.rows.flatMap(row => row.observation.transition?.outcome === 'confirmed' ? [row.observation.transition.phase] : [])
47 expect(phases).toEqual(['hover', 'cruise', 'hover'])
48 expect(report.path.some(p => p.mission.taskId === 'back' && p.transitionObservation?.phase === 'back-transition')).toBe(true)
49 const back = report.path.filter(p => p.mission.taskId === 'back')
50 expect(back.length).toBeGreaterThan(50)
51 expect(back.some(p => p.actuators.slice(0, 4).reduce((a, n) => a + n, 0) > 15)).toBe(true)
52 expect(back.filter(p => p.mission.transition !== null).every(p => p.actuators[4] === 0)).toBe(true)
53 expect(report.path.at(-1)!.measurement!.values.onSurface).toBe(true)
54 expect(report.finalTruthPositionM[2]).toBe(0)
55 expect(Math.hypot(report.finalTruthPositionM[0] - 180, report.finalTruthPositionM[1])).toBeLessThan(0.3)
56 expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0])
57 expect(report.board.expiresAtMs).toBeNull()
58 expect(report.mission.transition).toBeNull()
59 expect(report.mission.targetM).toBeNull()
60 expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission)
61})
62
63
64test('the full modeled back-capture allowance remains finite and is charged to the complete mission reserve', () => {
65 const plan = createVtolTransitionProfile().mission, back = plan.tasks.find(t => t.id === 'back')!
66 if (back.kind !== 'vtol-mode') throw new Error('Expected the back mode task')
67 back.timeoutMs = 45000
68 expect(validateVehicleMission(plan).worstCaseMs).toBe(131520)
69 expect(validateVehicleMission(plan).requiredWh).toBeCloseTo(43.84, 8)
70 back.timeoutMs = 45001
71 expect(() => validateVehicleMission(plan)).toThrow('unsupported profile')
72 back.timeoutMs = 45000
73 const front = plan.tasks.find(t => t.id === 'front')!
74 if (front.kind !== 'vtol-mode') throw new Error('Expected the front mode task')
75 front.timeoutMs = 15001
76 expect(() => validateVehicleMission(plan)).toThrow('unsupported profile')
77 front.timeoutMs = 15000
78 plan.power.availableWh = 50
79 expect(() => validateVehicleMission(plan)).toThrow('preserve energy reserve')
80})
81
82
83test('the original short back-capture budget fails without retry or a fabricated landing', () => {
84 const profile = createVtolTransitionProfile(), back = profile.mission.tasks.find(t => t.id === 'back')!
85 if (back.kind !== 'vtol-mode') throw new Error('Expected the back mode task')
86 back.timeoutMs = 15000
87 const report = runVehicleSimulation(profile, { seconds: 60 })
88 expect(report.status).toBe('blocked')
89 expect(report.mission.reason).toBe('vtol-transition-outcome-uncertain')
90 expect(report.trace.rows.filter(r => r.observation.transition?.mode === 'hover' && r.observation.transition.outcome === 'confirmed')).toHaveLength(1)
91 expect(report.path.some(p => p.mission.taskId === 'land')).toBe(false)
92 expect(report.path.at(-1)!.measurement!.values.onSurface).toBe(false)
93 expect(report.board.expiresAtMs).toBeNull()
94 expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0])
95})
96
97
98test('a physically failed modeled pusher cannot certify wing support or trigger an automatic retry', () => {
99 const profile = createVtolTransitionProfile(), createSession = profile.createSession
100 profile.createSession = () => {
101 const session = createSession(), plant = session.createPlant()
102 return { ...session, createPlant: () => ({ snapshot: () => plant.snapshot(), step(actuators, dtS, environment) {
103 const applied = [...actuators]
104 applied[4] = 0
105 return plant.step(applied, dtS, environment)
106 } }) }
107 }
108 const report = runVehicleSimulation(profile, { seconds: 60 })
109 expect(report.status).toBe('blocked')
110 expect(report.mission.reason).toBe('vtol-transition-outcome-uncertain')
111 expect(report.path.some(p => p.actuators[4]! > 0.3)).toBe(true)
112 expect(report.path.every(p => p.measurement!.values.pusherThrustN === 0)).toBe(true)
113 expect(report.trace.rows.some(r => r.observation.transition?.mode === 'cruise' && r.observation.transition.outcome === 'confirmed')).toBe(false)
114 expect(report.transitions.filter(t => t.taskId === 'front' && t.state === 'running')).toHaveLength(1)
115 expect(report.path.some(p => p.mission.taskId === 'cruise-leg')).toBe(false)
116 expect(report.mission.transition).toBeNull()
117 expect(report.board.expiresAtMs).toBeNull()
118 expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0])
119})
120
121
122test('high ground speed in a co-moving-air fault fixture cannot substitute for relative wing airspeed', () => {
123 // This artificial world fault follows the independently observed body velocity. It is not a
124 // meteorological model, a fabricated air packet, or information supplied to the controller.
125 const profile = createVtolTransitionProfile(), createSession = profile.createSession
126 let active = false, wind: [number, number, number] = [0, 0, 0]
127 profile.createSession = () => {
128 const session = createSession(), controller = session.createController()
129 return { ...session, createController: () => ({ step(c) { if (c.mission.taskId === 'front') active = true; return controller.step(c) } }),
130 observeIndependent(state) { wind = [...state.velocityMps] } }
131 }
132 profile.faults = () => active ? { windMps: [...wind] } : {}
133 const report = runVehicleSimulation(profile, { seconds: 60 })
134 expect(report.status).toBe('blocked')
135 expect(report.mission.reason).toBe('vtol-transition-outcome-uncertain')
136 const fast = report.trace.rows.filter(r => r.result.taskId === 'front' && Math.hypot(...r.observation.velocityMps) > 12)
137 expect(fast.length).toBeGreaterThan(20)
138 const fastStart = fast[0]!.observation.nowMs
139 expect(report.path.filter(p => p.atMs >= fastStart).every(p => (p.measurement!.values.airspeedMps as number) < 0.1)).toBe(true)
140 expect(report.trace.rows.some(r => r.observation.transition?.mode === 'cruise' && r.observation.transition.outcome === 'confirmed')).toBe(false)
141 expect(report.path.some(p => p.mission.taskId === 'cruise-leg')).toBe(false)
142 expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0])
143 expect(report.board.expiresAtMs).toBeNull()
144})
145
146
147function actualTransitionInputs() {
148 const profile = createVtolTransitionProfile(), createSession = profile.createSession, inputs: VehicleControllerContext[] = []
149 profile.createSession = () => {
150 const session = createSession(), controller = session.createController()
151 return { ...session, createController: () => ({ step(c) { inputs.push(structuredClone(c)); return controller.step(c) } }) }
152 }
153 expect(runVehicleSimulation(profile, { seconds: 160 }).status).toBe('completed')
154 return inputs
155}
156
157test('front, cruise, back and post-back hover retain latching source, reference and clock guards', () => {
158 const inputs = actualTransitionInputs()
159 for (const [task, atMs] of [['front', 13000], ['cruise-leg', 20000], ['back', 30000], ['land', 68000]] as const) {
160 const index = inputs.findIndex(c => c.nowMs === atMs)
161 expect(index).toBeGreaterThan(0)
162 expect(inputs[index]!.mission.taskId).toBe(task)
163 for (const change of [
164 (c: VehicleControllerContext) => { c.controlRate!.capturedAtMs -= 1000 },
165 (c: VehicleControllerContext) => { c.controlRate!.reference = 'foreign-reference' },
166 (c: VehicleControllerContext) => { c.measurement!.capturedAtMs -= 1000 },
167 (c: VehicleControllerContext) => { c.measurement!.sequence -= 2 },
168 (c: VehicleControllerContext) => { c.measurementPose!.reference = 'foreign-reference' },
169 (c: VehicleControllerContext) => { c.measurementPose!.capturedAtMs -= 20 },
170 (c: VehicleControllerContext) => { c.measurementPose!.gyroCapturedAtMs = NaN },
171 (c: VehicleControllerContext) => { c.estimate.localPosition!.origin!.ellipsoidHeightM += 1 },
172 (c: VehicleControllerContext) => { c.estimate.attitude!.quaternionWxyz = [2, 0, 0, 0] },
173 (c: VehicleControllerContext) => { c.nowMs -= 20 },
174 ]) {
175 const controller = createVtolFlightController()
176 let warmed = true
177 for (const c of inputs.slice(0, index)) if (controller.step(c).inhibited) warmed = false
178 expect(warmed).toBe(true)
179 const invalid = structuredClone(inputs[index]!)
180 change(invalid)
181 const refused = controller.step(invalid)
182 expect(refused.inhibited).toBe(true)
183 expect(refused.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0])
184 expect(refused.transition).toBeUndefined()
185 const next = controller.step(inputs[index + 1]!)
186 expect(next.inhibited).toBe(true)
187 expect(next.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0])
188 }
189 }
190})
191
192
193for (const [task, atMs] of [['front', 13000], ['cruise-leg', 20000], ['back', 30000], ['land', 68000]] as const) {
194 for (const failure of ['dropImu', 'dropPosition', 'dropVehicleSensor', 'rcOverride', 'kill', 'hostLost'] as const) {
195 test(`VTOL ${task} withdraws independent output authority for ${failure}`, () => {
196 const profile = createVtolTransitionProfile()
197 profile.faults = (_scenario, nowMs) => nowMs >= atMs ? { [failure]: true } : {}
198 const report = runVehicleSimulation(profile, { seconds: 90 })
199 expect(report.status).toBe(failure === 'hostLost' ? 'host-lost' : 'blocked')
200 expect(report.path.filter(p => p.atMs < atMs).at(-1)!.mission.taskId).toBe(task)
201 expect(report.finalTruthPositionM[2]).toBeLessThan(0)
202 expect(report.mission.state).not.toBe('completed')
203 expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0])
204 expect(report.board.expiresAtMs).toBeNull()
205 if (failure === 'hostLost') {
206 expect(report.board.fault).toBe('output-lease-expired')
207 expect(report.authority.lastCheckedAtMs).toBeLessThan(atMs)
208 }
209 else {
210 expect(report.mission.state).toBe('blocked')
211 expect(report.mission.targetM).toBeNull()
212 expect(report.mission.transition).toBeNull()
213 expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission)
214 }
215 if (failure === 'rcOverride') expect(report.authority.events.some(event => event.to === 'manual')).toBe(true)
216 if (failure === 'kill') expect(report.authority.events.some(event => event.to === 'killed')).toBe(true)
217 })
218 }
219}
220
221
222test('a turning VTOL cruise leg and back capture retain the same NED position and heading contract', () => {
223 const profile = createVtolTransitionProfile()
224 for (const task of profile.mission.tasks) {
225 if (task.id === 'cruise-leg' || task.id === 'land') {
226 if (!('positionM' in task)) throw new Error('Expected a position task')
227 task.positionM[0] = 150; task.positionM[1] = 100
228 }
229 if (task.id === 'back' && task.kind === 'vtol-mode') task.headingRad = 1
230 }
231 const report = runVehicleSimulation(profile, { seconds: 160 })
232 expect(report.status).toBe('completed')
233 expect(report.path.some(p => p.truthPositionM[1] > 90)).toBe(true)
234 expect(Math.hypot(report.finalTruthPositionM[0] - 150, report.finalTruthPositionM[1] - 100)).toBeLessThan(0.3)
235 expect(report.finalTruthPositionM[2]).toBe(0)
236 expect(report.trace.rows.filter(r => r.observation.transition?.outcome === 'confirmed').map(r => r.observation.transition!.phase)).toEqual(['hover', 'cruise', 'hover'])
237 expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0])
238 expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission)
239})
240
241
242test('airflow capture attitude without its observed source epochs cannot drive the full controller', () => {
243 const inputs = actualTransitionInputs(), index = inputs.findIndex(c => c.nowMs === 13000), controller = createVtolFlightController()
244 for (const c of inputs.slice(0, index)) controller.step(c)
245 const invalid = structuredClone(inputs[index]!)
246 Reflect.deleteProperty(invalid.measurementPose!, 'attitudeCapturedAtMs')
247 Reflect.deleteProperty(invalid.measurementPose!, 'gyroCapturedAtMs')
248 const refused = controller.step(invalid)
249 expect(refused.inhibited).toBe(true)
250 expect(refused.transition).toBeUndefined()
251 expect(refused.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0])
252 expect(controller.step(inputs[index + 1]!).inhibited).toBe(true)
253})
254
255
256test('a first full-flight mode request cannot invent a negative issuance or an oversized control budget', () => {
257 const inputs = actualTransitionInputs(), index = inputs.findIndex(c => c.mission.transition?.mode === 'cruise')
258 expect(index).toBeGreaterThan(0)
259 for (const change of [
260 (c: VehicleControllerContext) => { c.mission.transition!.issuedAtMs = -1 },
261 (c: VehicleControllerContext) => { c.mission.transition!.expiresAtMs = c.mission.transition!.issuedAtMs + 15001 },
262 ]) {
263 const controller = createVtolFlightController()
264 for (const c of inputs.slice(0, index)) controller.step(c)
265 const invalid = structuredClone(inputs[index]!)
266 change(invalid)
267 const refused = controller.step(invalid)
268 expect(refused.inhibited).toBe(true)
269 expect(refused.transition).toBeUndefined()
270 expect(refused.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0])
271 expect(controller.step(inputs[index + 1]!).inhibited).toBe(true)
272 }
273})
274
275
276test('full-flight acknowledgements retain the oldest capture attitude and gyro actually consumed', () => {
277 const inputs = actualTransitionInputs(), controller = createVtolFlightController()
278 let fullFlight = false
279 const overclaims: number[] = [], preRequest: number[] = []
280 for (const c of inputs) {
281 fullFlight ||= c.mission.transition?.mode === 'cruise'
282 const response = controller.step(c), observation = response.transition
283 if (!fullFlight || !observation) continue
284 if (observation.measurementCapturedAtMs > c.measurementPose!.attitudeCapturedAtMs || observation.measurementCapturedAtMs > c.measurementPose!.gyroCapturedAtMs) overclaims.push(c.nowMs)
285 if (observation.measurementCapturedAtMs < c.mission.transition!.issuedAtMs) preRequest.push(c.nowMs)
286 }
287 expect(fullFlight).toBe(true)
288 expect(overclaims).toEqual([])
289 expect(preRequest).toEqual([])
290})
291
292
293test('back hover cannot confirm while a near-stationary body remains away from the requested heading', () => {
294 const inputs = actualTransitionInputs(), controller = createVtolFlightController()
295 const back = inputs.filter(c => c.mission.taskId === 'back' && c.mission.transition?.mode === 'hover')
296 expect(back.length).toBeGreaterThan(50)
297 const startMs = back.at(-1)!.nowMs - 800
298 const wrongYaw: [number, number, number, number] = [Math.cos(0.3 / 2), 0, 0, Math.sin(0.3 / 2)]
299 let qualifiedSamples = 0
300 const confirmations: number[] = []
301 for (const c of inputs) {
302 if (c.nowMs < startMs) { controller.step(c); continue }
303 if (c.mission.taskId !== 'back' || !c.mission.transition) break
304 // A measured yaw offset is a public input fixture, not a fabricated mode outcome.
305 // Preserve actual position, velocity, gyro, airflow and every capture epoch.
306 const offset = structuredClone(c)
307 offset.estimate.attitude!.quaternionWxyz = multiplyQuaternion(wrongYaw, offset.estimate.attitude!.quaternionWxyz)
308 offset.measurementPose!.quaternionWxyz = multiplyQuaternion(wrongYaw, offset.measurementPose!.quaternionWxyz)
309 const response = controller.step(offset)
310 expect(response.inhibited).toBe(false)
311 const yaw = quaternionEuler(offset.estimate.attitude!.quaternionWxyz)[2]
312 expect(Math.abs(Math.atan2(Math.sin(yaw - offset.mission.transition!.headingRad), Math.cos(yaw - offset.mission.transition!.headingRad)))).toBeGreaterThan(0.2)
313 if (Math.hypot(...offset.estimate.localPosition!.velocityMps) <= 0.2 && Math.hypot(...offset.controlRate!.angularRateRadS) < 0.1) qualifiedSamples++
314 if (response.transition?.outcome === 'confirmed') confirmations.push(c.nowMs)
315 }
316 expect(qualifiedSamples).toBeGreaterThan(25)
317 expect(confirmations).toEqual([])
318 // Continue the same controller with fresh observations of the recorded settled heading.
319 // The prior wrong-heading window cannot count toward the required 500 ms dwell.
320 const settled = back.at(-1)!, qualifiedAt: number[] = []
321 for (let tick = 1; tick <= 30; tick++) {
322 const c = structuredClone(settled), elapsedMs = tick * 20
323 c.nowMs += elapsedMs; c.estimate.nowMs += elapsedMs
324 c.estimate.attitude!.capturedAtMs! += elapsedMs
325 c.estimate.localPosition!.capturedAtMs! += elapsedMs
326 c.estimate.localPosition!.predictedToMs += elapsedMs
327 c.controlRate!.capturedAtMs += elapsedMs
328 c.measurement!.capturedAtMs += elapsedMs; c.measurement!.receivedAtMs += elapsedMs; c.measurement!.sequence += tick
329 for (const key of ['capturedAtMs', 'positionCapturedAtMs', 'positionPredictedToMs', 'attitudeCapturedAtMs', 'attitudePredictedToMs', 'gyroCapturedAtMs'] as const) c.measurementPose![key] += elapsedMs
330 const response = controller.step(c)
331 expect(response.inhibited).toBe(false)
332 if (response.transition?.outcome === 'confirmed') qualifiedAt.push(c.nowMs)
333 }
334 expect(qualifiedAt).toEqual([settled.nowMs + 520])
335})