also looking at this
fix: stabilize VTOL lateral flow and hover control
#91
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.
| @@ -36,20 +36,40 @@ and a 120 N/s slew limit. An independent 14 N pusher and the owned trainer's aer | ||
| 36 | 36 | share the rigid body. Rotor allocation preserves collective thrust first, then roll/pitch demand, |
| 37 | 37 | then yaw. No coefficient is measured from a purchased airframe. |
| 38 | 38 | |
| 39 | The VTOL wing uses the fictional `folded-incidence-v2` extension. Its forward pre-stall curve | |
| 40 | matches the trainer. In reversed chordwise flow, incidence is folded and the coefficient changes | |
| 41 | sign, with a cosine fade toward broadside. This removes the force/moment jump at the raw angle's | |
| 42 | positive/negative pi boundary and restores the local vertical response in the tested head/tail flow. | |
| 43 | Raw airflow angles remain available to the capture and envelope guards. The fixed-wing profile keeps | |
| 44 | its original trainer model. Neither curve represents measured reverse-flow or post-stall data. | |
| 39 | The VTOL wing uses the fictional `projected-incidence-v3` extension. At zero sideslip its forward | |
| 40 | pre-stall response exactly matches the trainer. Reversed chordwise incidence is folded, changes | |
| 41 | coefficient sign and fades toward broadside. Longitudinal lift, drag and static pitch use the fraction | |
| 42 | of dynamic pressure in the chord/vertical airflow plane. This removes the force/moment singularity | |
| 43 | where that plane's velocity goes to zero while spanwise flow stays finite. Total pressure and raw | |
| 44 | airflow angles remain available to capture and envelope guards. The separately declared lateral, | |
| 45 | surface and rate responses retain total pressure. The default fixed-wing trainer is unchanged. | |
| 46 | These are fictional coefficients, without measured post-stall, spanwise or reverse-flight data. | |
| 47 | Full-route CLI reports identify this curve with `aerodynamicsModel: 'projected-incidence-v3'`. | |
| 48 | ||
| 49 | The prior `folded-incidence-v2` removed the reverse-flow angle-wrap jump at zero spanwise velocity, | |
| 50 | but retained a 0.9555 N force jump between `[+1e-8, 2, 0]` and `[-1e-8, 2, 0]` m/s. Approaching the | |
| 51 | same lateral-flow axis from positive/negative vertical velocity produced a 4.0768 N vertical-force | |
| 52 | jump. The v3 checks cover these approach directions, finite zero-flow response and non-positive | |
| 53 | unpowered airflow work. They establish bounded mathematical properties, not a calibrated airframe. | |
| 54 | A predeclared 16-case runtime campaign introduces 2 m/s head, tail, east and west wind at observed | |
| 55 | front, cruise, back and landing phase entries. All complete their declared heading plans through | |
| 56 | source-qualified hover, contact and replay within the original budgets. Wind onset is applied on | |
| 57 | the next actual integration step; the CLI/workspace wind selectors still use the fixed 1000 ms onset. | |
| 58 | ||
| 59 | Hover control aligns the thrust direction separately from its heading correction. A large, saturated | |
| 60 | yaw error therefore cannot rotate the requested position correction into another axis. Opposite | |
| 61 | thrust directions have no unique shortest tilt axis; that unsupported condition inhibits and latches | |
| 62 | neutral output. Existing body-rate bounds, PID gains, source guards and allocation priorities remain. | |
| 45 | 63 | |
| 46 | 64 | Back-transition braking compares the linear pre-stall estimate with 41 pitch candidates in the |
| 47 | 65 | existing -0.08 to 0.3 rad range. It evaluates the declared nonlinear wing/rotor force balance against |
| 48 | 66 | qualified captured airflow and estimated ground motion. Candidates must admit 0 to 48 N of lift-rotor |
| 49 | 67 | thrust while supporting the requested vertical force; high-speed candidate incidence remains within |
| 50 | 68 | 0.3 rad. The finite transition deadline, source guards, capture dwell and independent authority still |
| 51 | apply. Replacing a correlated back request preserves rotary capture once reached, so recovery cannot | |
| 52 | restart forward-flight braking during a reverse position return. | |
| 69 | apply. Before low-speed rotary capture, the nose and candidate force search follow qualified measured | |
| 70 | airflow. The requested final hover heading remains the capture condition after braking. Replacing a | |
| 71 | correlated back request preserves rotary capture once reached, so recovery cannot restart forward-flight | |
| 72 | braking during a reverse position return. | |
| 53 | 73 | |
| 54 | 74 | ### Bounded wind cases |
| 55 | 75 | |
| @@ -92,8 +112,9 @@ profile.faults = (_scenario, atMs) => ({ windMps: atMs >= 1000 ? [2, 0, 0] : [0, | ||
| 92 | 112 | const result = runVehicleSimulation(profile, { seconds: 160 }) |
| 93 | 113 | ``` |
| 94 | 114 | |
| 95 | The original fixed north-facing east/west crosswind plans still reach the back-transition | |
| 96 | envelope guard without confirmed hover or landing. During the east case's reverse return at 38,000 ms, | |
| 115 | The fixed north-facing east/west crosswind plans still reach the back-transition envelope guard | |
| 116 | without confirmed hover or landing. The retained v2 east-case diagnosis, before the v3 flow projection, | |
| 117 | records a reverse return at 38,000 ms where | |
| 97 | 118 | the modeled aerodynamic yaw moment is approximately -0.729 Nm while the fully allocated opposing |
| 98 | 119 | rotor yaw moment is only +0.416 Nm, with the rudder at its +0.4 rad bound. The qualified airflow capture |
| 99 | 120 | is 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 | ||
| 105 | 126 | envelope, but changing gains cannot supply the missing torque in that condition. Neutral outputs |
| 106 | 127 | at the original airborne failure do not establish a safe landing. |
| 107 | 128 | |
| 108 | The two wind-facing plans change only the declared back-hover heading and plan identity. They use | |
| 109 | the existing `vtol-mode.headingRad` mission contract and unchanged controller, model, reserve and | |
| 110 | finite budgets. The front heading remains north. These plans complete the source-qualified modeled hover | |
| 129 | The two wind-facing plans differ from their fixed-heading counterparts only in the declared | |
| 130 | back-hover heading and plan identity. They use the existing `vtol-mode.headingRad` mission contract | |
| 131 | and the same shared controller, model, reserve and finite budgets. The front heading remains north. These plans complete the source-qualified modeled hover | |
| 111 | 132 | confirmation and contact route in the declared wind. Their extra `backHeadingRad` declaration and |
| 112 | 133 | `backHeadingMatched` check bind qualification to the actual recorded plan; a completed route under |
| 113 | a different heading remains unmatched. No automatic heading substitution or retry is performed. | |
| 134 | a different heading remains unmatched. The plan retains its declared final heading, and there are no automatic retries. | |
| 114 | 135 | The original `*-limit-2mps` cases keep their prior plans and blocked outcomes. |
| 115 | 136 | |
| 116 | Wider wind directions, onset phases, a general feasibility planner and full-angle model qualification | |
| 137 | Wider wind directions, onset phases, a general feasibility planner and measured full-angle model qualification | |
| 117 | 138 | remain in [#87](https://github.com/stacksjs/drone/issues/87). No arbitrary-wind, measured-airframe or |
| 118 | 139 | physical flight acceptance is claimed. |
| 119 | 140 | |
| @@ -1,7 +1,8 @@ | ||
| 1 | 1 | import type { EstimatorSnapshot } from '../estimation/estimator' |
| 2 | 2 | import type { SensorVector } from '../sensors/types' |
| 3 | 3 | import type { Motors, Wrench } from './model' |
| 4 | import { multiplyQuaternion, quaternionFromTilt } from '../estimation/frames' | |
| 4 | import { multiplyQuaternion, normalizeQuaternion, quaternionFromTilt } from '../estimation/frames' | |
| 5 | import { rotateBodyToNed } from './physics' | |
| 5 | 6 | import { createBoundedPid } from './pid' |
| 6 | 7 | import { clampControl, validVector } from './model' |
| 7 | 8 | export interface SimulatedControlInput { |
| @@ -66,9 +67,18 @@ export function createSimulatedLiftController(options: { headingAlignment: 'init | ||
| 66 | 67 | 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] |
| 67 | 68 | const vertical = model.gravityMps2 - acceleration[2] |
| 68 | 69 | 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 | |
| 72 | 82 | const torque = rates.map((n, i) => ratePid[i]!.step(n - input.angularRateRadS[i]!, input.angularRateRadS[i]!, dt)) as SensorVector |
| 73 | 83 | const desiredWrench = { thrustN: model.massKg * Math.hypot(...[acceleration[0], acceleration[1], vertical]), torqueNm: torque } |
| 74 | 84 | const allocation = allocate(desiredWrench, previous, dt) |
| @@ -19,6 +19,11 @@ export function airAerodynamics(relativeBodyMps: SensorVector, rateRadS: SensorV | ||
| 19 | 19 | const [u, v, w] = relativeBodyMps |
| 20 | 20 | const speedMps = Math.hypot(u, v, w), alphaRad = Math.atan2(w, u), betaRad = Math.atan2(v, Math.hypot(u, w)) |
| 21 | 21 | 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 | |
| 22 | 27 | // Explicit VTOL extension: fold the incidence and project stalled flow along |
| 23 | 28 | // the chord. Preserve the forward pre-stall curve; fade to zero at broadside |
| 24 | 29 | // and reverse the coefficient sign for flow over the trailing edge. |
| @@ -29,13 +34,13 @@ export function airAerodynamics(relativeBodyMps: SensorVector, rateRadS: SensorV | ||
| 29 | 34 | // Bounded post-stall loss, retaining drag. No claim of dynamic stall or spin modeling. |
| 30 | 35 | const liftCoefficient = chordDirection * clamp(linearLift, -1.1, 1.4) * Math.max(0.15, 1 - Math.max(0, Math.abs(coefficientAlphaRad) - airModel.stallAlphaRad) * 2.5) |
| 31 | 36 | 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 | |
| 33 | 38 | const ca = Math.cos(alphaRad), sa = Math.sin(alphaRad) |
| 34 | 39 | const forceN: SensorVector = [actuators[0]! * airModel.maxThrustN - drag * ca + lift * sa, -dynamicPressurePa * airModel.areaM2 * 0.6 * betaRad, -drag * sa - lift * ca] |
| 35 | 40 | const rateScale = Math.max(1, speedMps) |
| 36 | 41 | const torqueNm: SensorVector = [ |
| 37 | 42 | 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)), | |
| 39 | 44 | dynamicPressurePa * airModel.areaM2 * airModel.spanM * (0.08 * actuators[3]! + 0.12 * betaRad - 0.25 * rateRadS[2] * airModel.spanM / (2 * rateScale)), |
| 40 | 45 | ] |
| 41 | 46 | return { forceN, torqueNm, speedMps, alphaRad, betaRad, liftCoefficient, dragCoefficient, dynamicPressurePa, stalled: Math.abs(alphaRad) > airModel.stallAlphaRad } |
| @@ -138,6 +138,9 @@ export function createVtolFlightController(): VehicleController { | ||
| 138 | 138 | if (rotary) { |
| 139 | 139 | backSlow ||= speed < 3 |
| 140 | 140 | 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]) | |
| 141 | 144 | // The linear pre-stall estimate seeds a bounded search of the actual fictional |
| 142 | 145 | // wing/rotor force balance, including stalled drag. Keep the original pitch limits. |
| 143 | 146 | const optimalAlpha = Math.max(0, (requiredUpN / Math.max(1, pressureArea) - 0.25) / (2 * 4.8)) |
| @@ -9,9 +9,9 @@ import { numericPacket } from './packets' | ||
| 9 | 9 | import { vtolElectricalLoad, vtolElectricalMaximumW, vtolElectricalModel, vtolElectricalSupply } from './power' |
| 10 | 10 | |
| 11 | 11 | /** Fictional lift-and-cruise model. No measured hardware coefficients or flight qualification. */ |
| 12 | export 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 }) | |
| 12 | export 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 }) | |
| 13 | 13 | |
| 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. */ | |
| 15 | 15 | export function vtolAerodynamics(relativeBodyMps: SensorVector, rateRadS: SensorVector, actuators: number[]) { |
| 16 | 16 | return airAerodynamics(relativeBodyMps, rateRadS, actuators, true) |
| 17 | 17 | } |
| @@ -1,4 +1,5 @@ | ||
| 1 | 1 | import { createVtolTransitionProfile } from './transition-profile' |
| 2 | import { vtolModel } from './plant' | |
| 2 | 3 | import { runVehicleSimulation } from '../runtime' |
| 3 | 4 | import type { VehicleMissionAction } from '../types' |
| 4 | 5 | import type { SensorVector } from '../../sensors/types' |
| @@ -170,7 +171,7 @@ export function createVtolSimulationCase(scenario = 'nominal', options: VtolSimu | ||
| 170 | 171 | const backHeadingMatched = backHeadingRad === null || backTask?.kind === 'vtol-mode' && backTask.mode === 'hover' && backTask.headingRad === backHeadingRad |
| 171 | 172 | 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) |
| 172 | 173 | 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, | |
| 174 | 175 | ...(phase ? { scenarioEvidence: { phase: phase.phase, injectedAtMs: phase.atMs, taskId: before?.mission.taskId ?? null, faultObserved, sources: capturedSources } } : {}), |
| 175 | 176 | ...(wind ? { windEvidence: { ...structuredClone(wind), ...exposure, worldMatched, outcomeMatched: windOutcomeMatched, ...(backHeadingRad === null ? {} : { backHeadingMatched }) } } : {}), |
| 176 | 177 | ...(actions ? { operatorEvidence: { actions: actualActions, tasksBeforeActions, phaseMatched: operatorPhaseMatched, observedPaused, matched: operatorMatched, ...(operator?.recovery ? { recoveryCorrelated } : {}), ...(operator?.openCircuit ? { pusherOpenCircuitObserved } : {}), ...(operator?.outcome === 'hold-expiry' ? { globalEnduranceExpired, landingStarted } : {}) } } : {}) } |
| @@ -64,3 +64,31 @@ test('the VTOL body follows a hover position step using raw IMU and GNSS rather | ||
| 64 | 64 | expect(estimate.health.localPosition).toBe(true) |
| 65 | 65 | expect(estimate.health.absoluteHeading).toBe(false) |
| 66 | 66 | }) |
| 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. | |
| 70 | test('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 | ||
| 84 | test('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 | }) | |
| @@ -0,0 +1,30 @@ | ||
| 1 | import { expect, test } from 'bun:test' | |
| 2 | import { 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. | |
| 6 | test('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 | ||
| 12 | test('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 | ||
| 23 | test('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 | }) | |
| @@ -28,6 +28,7 @@ test('native VTOL full-route command reports qualified initial hover, cruise, ba | ||
| 28 | 28 | const report = JSON.parse(result.stdout) |
| 29 | 29 | expect(report.status).toBe('completed') |
| 30 | 30 | expect(report.scope).toBe('development-lift-and-cruise') |
| 31 | expect(report.aerodynamicsModel).toBe('projected-incidence-v3') | |
| 31 | 32 | expect(report.demonstrationMatched).toBe(true) |
| 32 | 33 | expect(report.transitionEvidence.map((row: { taskId: string, phase: string }) => [row.taskId, row.phase])).toEqual([ |
| 33 | 34 | ['initial-hover', 'hover'], ['front', 'cruise'], ['back', 'hover'], |
| @@ -0,0 +1,59 @@ | ||
| 1 | import { expect, test } from 'bun:test' | |
| 2 | import { createVtolTransitionProfile } from '../../resources/functions/drone/vehicles/vtol/transition-profile' | |
| 3 | import { runVehicleSimulation } from '../../resources/functions/drone/vehicles/runtime' | |
| 4 | import { replayVehicleMission } from '../../resources/functions/drone/missions/vehicle' | |
| 5 | import type { SensorVector } from '../../resources/functions/drone/sensors/types' | |
| 6 | ||
| 7 | const 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 | |
| 13 | const 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. | |
| 17 | for (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 | }) | |