also looking at this
feat: add bounded VTOL wind braking
#88
13 files
+447
-25
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,6 +36,44 @@ 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. | |
| 45 | ||
| 46 | Back-transition braking compares the linear pre-stall estimate with 41 pitch candidates in the | |
| 47 | existing -0.08 to 0.3 rad range. It evaluates the declared nonlinear wing/rotor force balance against | |
| 48 | qualified captured airflow and estimated ground motion. Candidates must admit 0 to 48 N of lift-rotor | |
| 49 | thrust while supporting the requested vertical force; high-speed candidate incidence remains within | |
| 50 | 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. | |
| 53 | ||
| 54 | ### Bounded library wind experiment | |
| 55 | ||
| 56 | The library campaign applies sustained 2 m/s headwind or tailwind to the actual world model from | |
| 57 | 1000 ms, including takeoff, transitions, cruise and landing. Both routes complete within their | |
| 58 | original budgets using the raw sensor/estimator/controller runtime. This does not add wind selectors | |
| 59 | to the 14 named CLI/workspace cases below. A tailwind library run can be constructed explicitly: | |
| 60 | ||
| 61 | ```ts | |
| 62 | import { createVtolTransitionProfile } from '../resources/functions/drone/vehicles/vtol/transition-profile' | |
| 63 | import { runVehicleSimulation } from '../resources/functions/drone/vehicles/runtime' | |
| 64 | ||
| 65 | const profile = createVtolTransitionProfile({ electrical: true }) | |
| 66 | profile.faults = (_scenario, atMs) => ({ windMps: atMs >= 1000 ? [2, 0, 0] : [0, 0, 0] }) | |
| 67 | const result = runVehicleSimulation(profile, { seconds: 160 }) | |
| 68 | ``` | |
| 69 | ||
| 70 | The same `[0, 2, 0]` m/s crosswind experiment still reaches the back-transition envelope guard | |
| 71 | without confirmed hover or landing. Its yaw excursion and eventual pitch limit remain an unsupported | |
| 72 | case. Neutral outputs at that airborne failure do not establish a safe landing. Wider wind directions, | |
| 73 | onset phases, planning/heading feasibility and named native/workspace cases remain in | |
| 74 | [#87](https://github.com/stacksjs/drone/issues/87). No arbitrary-wind, measured-airframe or physical | |
| 75 | flight acceptance is claimed. | |
| 76 | ||
| 39 | 77 | The hover demonstration commands the pusher and three surfaces to zero. Calibrated modeled raw |
| 40 | 78 | IMU/GNSS samples feed the owned estimator. The controller receives estimated pose, a current qualified |
| 41 | 79 | raw gyro capture and delayed contact measurements. Plant truth is used for sensor synthesis and |
| @@ -262,9 +300,11 @@ route. A timeout/controller rejection that recovers to contact ends with `vtol-r | ||
| 262 | 300 | |
| 263 | 301 | Landing holds the captured estimated approach height until horizontal position and speed qualify |
| 264 | 302 | descent to the selected site. Actual failed-pusher and expired-back campaigns retain rotor-supported |
| 265 | hover and fresh contact. The original 25-second back-recovery approach/landing case still times out | |
| 266 | airborne. A separately declared 35-second allowance completes after about 26.94 seconds, charged to | |
| 267 | the complete 156,520 ms / 52.1733 Wh worst-case plan. A normal 45-second back plus 45-second recovery | |
| 303 | hover and fresh contact. With nonlinear braking, both the 25-second and 35-second back-recovery | |
| 304 | approach/landing cases reach contact within their declared allowances. They retain complete | |
| 305 | 146,520 ms / 48.84 Wh and 156,520 ms / 52.1733 Wh worst-case plans, respectively. A stricter | |
| 306 | 20-second landing case expires at its exact deadline while still airborne, with a complete | |
| 307 | 141,520 ms / 47.1733 Wh plan. A normal 45-second back plus 45-second recovery | |
| 268 | 308 | hover and 35-second landing exceeds the unchanged 180-second graph cap and is rejected. The phase |
| 269 | 309 | abort/pause/recover campaigns instead declare 40-second recovery hover and 30-second landing, fitting |
| 270 | 310 | 176,520 ms / 58.84 Wh. These remain conservative 1200 W planning charges, without identified electrical |
| @@ -12,15 +12,22 @@ export function validateAirActuators(a: number[]) { | ||
| 12 | 12 | if (!Array.isArray(a) || a.length !== 5 || !a.every(Number.isFinite) || a[0]! < 0 || a[0]! > 1 || a.slice(1, 4).some(n => Math.abs(n) > 0.4) || a[4]! < 0 || a[4]! > 1) throw new RangeError('Invalid fixed-wing throttle, aileron, elevator, rudder or wheel brake') |
| 13 | 13 | } |
| 14 | 14 | /** Quasi-steady lift/drag and rate-damped surface moments. Not firmware coefficients. */ |
| 15 | export function airAerodynamics(relativeBodyMps: SensorVector, rateRadS: SensorVector, actuators: number[]) { | |
| 15 | export function airAerodynamics(relativeBodyMps: SensorVector, rateRadS: SensorVector, actuators: number[], foldReverseFlow = false) { | |
| 16 | if (typeof foldReverseFlow !== 'boolean') throw new TypeError('Invalid aerodynamic incidence model') | |
| 16 | 17 | if (!validVector(relativeBodyMps) || !validVector(rateRadS)) throw new RangeError('Invalid aerodynamic vector') |
| 17 | 18 | validateAirActuators(actuators) |
| 18 | 19 | const [u, v, w] = relativeBodyMps |
| 19 | 20 | const speedMps = Math.hypot(u, v, w), alphaRad = Math.atan2(w, u), betaRad = Math.atan2(v, Math.hypot(u, w)) |
| 20 | 21 | const dynamicPressurePa = 0.5 * airModel.densityKgM3 * speedMps ** 2 |
| 21 | const linearLift = 0.25 + 4.8 * alphaRad | |
| 22 | // Explicit VTOL extension: fold the incidence and project stalled flow along | |
| 23 | // the chord. Preserve the forward pre-stall curve; fade to zero at broadside | |
| 24 | // and reverse the coefficient sign for flow over the trailing edge. | |
| 25 | // Raw airflow angles remain unchanged for capture/envelope checks. | |
| 26 | const coefficientAlphaRad = foldReverseFlow ? Math.atan2(w, Math.abs(u)) : alphaRad | |
| 27 | const chordDirection = foldReverseFlow ? Math.sign(u) * Math.min(1, Math.abs(Math.cos(alphaRad)) / Math.cos(airModel.stallAlphaRad)) : 1 | |
| 28 | const linearLift = 0.25 + 4.8 * coefficientAlphaRad | |
| 22 | 29 | // Bounded post-stall loss, retaining drag. No claim of dynamic stall or spin modeling. |
| 23 | const liftCoefficient = clamp(linearLift, -1.1, 1.4) * Math.max(0.15, 1 - Math.max(0, Math.abs(alphaRad) - airModel.stallAlphaRad) * 2.5) | |
| 30 | const liftCoefficient = chordDirection * clamp(linearLift, -1.1, 1.4) * Math.max(0.15, 1 - Math.max(0, Math.abs(coefficientAlphaRad) - airModel.stallAlphaRad) * 2.5) | |
| 24 | 31 | const dragCoefficient = 0.04 + 0.06 * liftCoefficient ** 2 + 0.8 * Math.sin(alphaRad) ** 2 + 0.2 * Math.sin(betaRad) ** 2 |
| 25 | 32 | const lift = dynamicPressurePa * airModel.areaM2 * liftCoefficient, drag = dynamicPressurePa * airModel.areaM2 * dragCoefficient |
| 26 | 33 | const ca = Math.cos(alphaRad), sa = Math.sin(alphaRad) |
| @@ -28,7 +35,7 @@ export function airAerodynamics(relativeBodyMps: SensorVector, rateRadS: SensorV | ||
| 28 | 35 | const rateScale = Math.max(1, speedMps) |
| 29 | 36 | const torqueNm: SensorVector = [ |
| 30 | 37 | dynamicPressurePa * airModel.areaM2 * airModel.spanM * (0.12 * actuators[1]! - 0.3 * rateRadS[0] * airModel.spanM / (2 * rateScale)), |
| 31 | dynamicPressurePa * airModel.areaM2 * airModel.chordM * (0.8 * actuators[2]! - 0.15 * alphaRad - 2 * rateRadS[1] * airModel.chordM / (2 * rateScale)), | |
| 38 | dynamicPressurePa * airModel.areaM2 * airModel.chordM * (0.8 * actuators[2]! - 0.15 * coefficientAlphaRad * chordDirection - 2 * rateRadS[1] * airModel.chordM / (2 * rateScale)), | |
| 32 | 39 | dynamicPressurePa * airModel.areaM2 * airModel.spanM * (0.08 * actuators[3]! + 0.12 * betaRad - 0.25 * rateRadS[2] * airModel.spanM / (2 * rateScale)), |
| 33 | 40 | ] |
| 34 | 41 | return { forceN, torqueNm, speedMps, alphaRad, betaRad, liftCoefficient, dragCoefficient, dynamicPressurePa, stalled: Math.abs(alphaRad) > airModel.stallAlphaRad } |
| @@ -5,11 +5,11 @@ import { createVehicleMeasurementQualifier, vehicleReference } from '../measurem | ||
| 5 | 5 | import { quaternionEuler, quaternionFromTilt, multiplyQuaternion, normalizeQuaternion } from '../../estimation/frames' |
| 6 | 6 | import { rotateBodyToNed, rotateNedToBody } from '../../controls/physics' |
| 7 | 7 | import { createBoundedPid } from '../../controls/pid' |
| 8 | import { airAerodynamics, airModel } from '../air/plant' | |
| 8 | import { airModel } from '../air/plant' | |
| 9 | 9 | import { createVtolVehicleHoverController } from './controller' |
| 10 | 10 | import { numericPacket } from './packets' |
| 11 | 11 | import { allocateVtolLift } from './allocation' |
| 12 | import { vtolModel } from './plant' | |
| 12 | import { vtolAerodynamics, vtolModel } from './plant' | |
| 13 | 13 | |
| 14 | 14 | const clamp = (n: number, low: number, high: number) => Math.max(low, Math.min(high, n)) |
| 15 | 15 | const wrap = (n: number) => Math.atan2(Math.sin(n), Math.cos(n)) |
| @@ -89,7 +89,10 @@ export function createVtolFlightController(): VehicleController { | ||
| 89 | 89 | } |
| 90 | 90 | if (request?.eventId !== incoming.eventId && incoming.mode === 'hover' && hasWingControl) { |
| 91 | 91 | if (phase !== 'cruise' && !superseding) return neutral(c, 'vtol-back-requires-observed-cruise') |
| 92 | request = structuredClone(incoming); signature = encoded; phase = 'back-transition'; backSlow = false; stableSource = null; stableDecision = null | |
| 92 | // A correlated replacement keeps rotary capture once reached. Restarting | |
| 93 | // wing braking here would assume forward flow during a position return. | |
| 94 | backSlow = (phase === 'back-transition' || phase === 'hover') && backSlow | |
| 95 | request = structuredClone(incoming); signature = encoded; phase = 'back-transition'; stableSource = null; stableDecision = null | |
| 93 | 96 | for (const pid of velocityPid) pid.reset() |
| 94 | 97 | } |
| 95 | 98 | } |
| @@ -131,15 +134,30 @@ export function createVtolFlightController(): VehicleController { | ||
| 131 | 134 | let requiredUpN = vtolModel.massKg * (vtolModel.gravityMps2 - downAcceleration) |
| 132 | 135 | const alphaTarget = clamp((requiredUpN / Math.max(1, pressureArea * Math.cos(bankTarget)) - 0.25) / 4.8, -0.08, 0.18) |
| 133 | 136 | let pitchTarget = clamp(gamma + alphaTarget, -0.2, 0.25) * clamp((speed - 10) / 2, 0, 1) |
| 134 | const zero = airAerodynamics(relative, rates!, [0, 0, 0, 0, 0]) | |
| 137 | const zero = vtolAerodynamics(relative, rates!, [0, 0, 0, 0, 0]) | |
| 135 | 138 | if (rotary) { |
| 136 | 139 | backSlow ||= speed < 3 |
| 137 | 140 | if (!backSlow && phase === 'back-transition') { |
| 138 | // With the pusher off, high-speed braking is bounded by wing drag and the lift surplus. | |
| 139 | // The linear pre-stall lift slope gives the pitch that maximizes supported rotor braking. | |
| 141 | // The linear pre-stall estimate seeds a bounded search of the actual fictional | |
| 142 | // wing/rotor force balance, including stalled drag. Keep the original pitch limits. | |
| 140 | 143 | const optimalAlpha = Math.max(0, (requiredUpN / Math.max(1, pressureArea) - 0.25) / (2 * 4.8)) |
| 141 | 144 | pitchTarget = clamp(gamma + optimalAlpha, -0.08, 0.3) |
| 142 | 145 | bankTarget = 0 |
| 146 | let bestBraking = -Infinity | |
| 147 | const direction = groundSpeed > 0.2 ? [velocity![0] / groundSpeed, velocity![1] / groundSpeed] : [Math.cos(heading), Math.sin(heading)] | |
| 148 | for (const candidate of [pitchTarget, ...Array.from({ length: 41 }, (_, i) => -0.08 + 0.38 * i / 40)]) { | |
| 149 | const pose = multiplyQuaternion([Math.cos(heading / 2), 0, 0, Math.sin(heading / 2)], quaternionFromTilt(0, candidate)) | |
| 150 | const body = rotateNedToBody(pose, airNed) | |
| 151 | const wing = vtolAerodynamics(body, [0, 0, 0], [0, 0, 0, 0, 0]) | |
| 152 | if (speed > 8 && Math.abs(wing.alphaRad) > 0.3) continue | |
| 153 | const force = rotateBodyToNed(pose, wing.forceN) | |
| 154 | const thrust = (requiredUpN + force[2]) / Math.cos(candidate) | |
| 155 | if (thrust < 0 || thrust > 48) continue | |
| 156 | const rotor = rotateBodyToNed(pose, [0, 0, -thrust]) | |
| 157 | const braking = -(direction[0]! * (force[0] + rotor[0]) + direction[1]! * (force[1] + rotor[1])) | |
| 158 | if (braking > bestBraking) { bestBraking = braking; pitchTarget = candidate } | |
| 159 | } | |
| 160 | if (bestBraking === -Infinity) return neutral(c, 'vtol-braking-no-feasible-pitch') | |
| 143 | 161 | } |
| 144 | 162 | else { |
| 145 | 163 | const goal = [...target!] as SensorVector |
| @@ -176,7 +194,7 @@ export function createVtolFlightController(): VehicleController { | ||
| 176 | 194 | const pusher = rotary ? 0 : clamp((-zero.forceN[0] + vtolModel.massKg * clamp((13 - speed) * 1.2, -2.5, 2.5)) / vtolModel.maxPusherThrustN, 0, 1) |
| 177 | 195 | const coefficients = [pressureArea * airModel.spanM * 0.12, pressureArea * airModel.chordM * 0.8, pressureArea * airModel.spanM * 0.08] |
| 178 | 196 | const surfaces = torque.map((n, i) => coefficients[i]! > 1e-6 ? clamp((n - zero.torqueNm[i]!) / coefficients[i]!, -0.4, 0.4) : 0) |
| 179 | const aero = airAerodynamics(relative, rates!, [pusher, ...surfaces, 0]) | |
| 197 | const aero = vtolAerodynamics(relative, rates!, [pusher, ...surfaces, 0]) | |
| 180 | 198 | const aeroNed = rotateBodyToNed(q!, aero.forceN) |
| 181 | 199 | const collective = clamp((requiredUpN + aeroNed[2]) / Math.max(0.7, Math.cos(roll) * Math.cos(pitch)), 0, 48) |
| 182 | 200 | const allocation = allocateVtolLift({ thrustN: collective, torqueNm: torque.map((n, i) => n - aero.torqueNm[i]!) as SensorVector }) |
| @@ -9,7 +9,12 @@ 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({ 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: '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 }) | |
| 13 | ||
| 14 | /** Fictional continuous reverse-flow extension, not measured post-stall or reverse-flight data. */ | |
| 15 | export function vtolAerodynamics(relativeBodyMps: SensorVector, rateRadS: SensorVector, actuators: number[]) { | |
| 16 | return airAerodynamics(relativeBodyMps, rateRadS, actuators, true) | |
| 17 | } | |
| 13 | 18 | |
| 14 | 19 | /** Independent lift rotors and the owned trainer wing/pusher share one rigid body. */ |
| 15 | 20 | export function createVtolPlant(initial: { positionM?: SensorVector, velocityMps?: SensorVector, quaternionWxyz?: Quaternion, rotorsN?: [number, number, number, number], pusherThrustN?: number, electricalCapacityWh?: number, energyUsedWh?: number } = {}): VehiclePlant { |
| @@ -54,7 +59,7 @@ export function createVtolPlant(initial: { positionM?: SensorVector, velocityMps | ||
| 54 | 59 | const pusherChangeN = (actuators[4]! * vtolModel.maxPusherThrustN - pusherThrustN) * (1 - Math.exp(-dtS / vtolModel.pusherTimeConstantS)) |
| 55 | 60 | const pusherMaximumChangeN = vtolModel.pusherSlewNPerS * dtS |
| 56 | 61 | const nextPusherThrustN = pusherThrustN + Math.max(-pusherMaximumChangeN, Math.min(pusherMaximumChangeN, pusherChangeN)) |
| 57 | const aero = airAerodynamics(relative, angularRateRadS, [nextPusherThrustN / vtolModel.maxPusherThrustN, actuators[5]!, actuators[6]!, actuators[7]!, 0]) | |
| 62 | const aero = vtolAerodynamics(relative, angularRateRadS, [nextPusherThrustN / vtolModel.maxPusherThrustN, actuators[5]!, actuators[6]!, actuators[7]!, 0]) | |
| 58 | 63 | // Front-left, front-right, rear-right, rear-left. Rotor reaction signs are +, -, +, - about BODY_FRD z. |
| 59 | 64 | const torqueNm: SensorVector = [ |
| 60 | 65 | aero.torqueNm[0] + vtolModel.rotorArmProjectionM * (nextRotorsN[0]! - nextRotorsN[1]! - nextRotorsN[2]! + nextRotorsN[3]!), |
| @@ -176,7 +176,12 @@ test('an early resume and repeated recovery cannot skip observed hover or extend | ||
| 176 | 176 | expect(new Set(requests.map(request => request.eventId)).size).toBe(1) |
| 177 | 177 | expect(requests.every(request => request.issuedAtMs === 13000 && request.expiresAtMs === 58000)).toBe(true) |
| 178 | 178 | expect(report.path.find(row => row.atMs === 13200)!.mission.state).toBe('running') |
| 179 | expect(report.path.find(row => row.atMs === 20000)!.mission.state).toBe('paused') | |
| 179 | const observedHover = report.trace.rows.find(row => row.observation.transition?.taskId === 'recover-hover' && row.observation.transition.outcome === 'confirmed') | |
| 180 | expect(observedHover).toBeDefined() | |
| 181 | expect(observedHover!.observation.nowMs).toBeGreaterThan(13400) | |
| 182 | expect(observedHover!.observation.nowMs).toBeLessThan(40000) | |
| 183 | expect(observedHover!.result.state).toBe('paused') | |
| 184 | expect(report.path.filter(row => row.atMs >= 13200 && row.atMs < observedHover!.observation.nowMs).every(row => row.mission.state === 'running' && row.mission.taskId === 'recover-hover')).toBe(true) | |
| 180 | 185 | expect(report.path.find(row => row.atMs === 39980)!.mission.state).toBe('paused') |
| 181 | 186 | expect(report.mission.reason).toBe('vtol-recovery-landed') |
| 182 | 187 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) |
| @@ -193,7 +198,11 @@ for (const abortAtMs of [15000, 20000]) test(`an abort at ${abortAtMs} upgrades | ||
| 193 | 198 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) |
| 194 | 199 | }) |
| 195 | 200 | |
| 196 | for (const [sourceTask, atMs] of [['initial-hover', 620], ['front', 11320], ['back', 27400]] as const) test(`a ${sourceTask} abort before its first command dispatch does not invent a cancelled event`, () => { | |
| 201 | for (const sourceTask of ['initial-hover', 'front', 'back'] as const) test(`a ${sourceTask} abort before its first command dispatch does not invent a cancelled event`, () => { | |
| 202 | const baseline = runVehicleSimulation(declaredRecovery(sourceTask), { seconds: 160 }) | |
| 203 | const firstRequest = baseline.path.find(row => row.mission.transition?.taskId === sourceTask) | |
| 204 | expect(firstRequest).toBeDefined() | |
| 205 | const atMs = firstRequest!.atMs | |
| 197 | 206 | const report = runVehicleSimulation(declaredRecovery(sourceTask), { seconds: 180, missionActions: [{ atMs, action: 'abort' }] }) |
| 198 | 207 | expect(report.mission.reason).toBe('vtol-abort-landed') |
| 199 | 208 | expect(report.mission.recovery?.failedEventId).toBeNull() |
| @@ -235,7 +244,7 @@ test('a declared front-transition failure route cancels failed wing support, obs | ||
| 235 | 244 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) |
| 236 | 245 | }) |
| 237 | 246 | |
| 238 | for (const landingTimeoutMs of [25000, 35000]) test(`back-transition recovery retains the declared ${landingTimeoutMs} ms approach and landing budget`, () => { | |
| 247 | for (const [landingTimeoutMs, worstCaseMs, requiredWh] of [[20000, 141520, 47.17333333333333], [25000, 146520, 48.84], [35000, 156520, 52.17333333333333]] as const) test(`back-transition recovery retains the declared ${landingTimeoutMs} ms approach and landing budget`, () => { | |
| 239 | 248 | const profile = declaredRecovery('back'), back = profile.mission.tasks.find(task => task.id === 'back')! |
| 240 | 249 | if (back.kind !== 'vtol-mode') throw new Error('Expected a VTOL mode task') |
| 241 | 250 | back.timeoutMs = 15000 |
| @@ -244,18 +253,22 @@ for (const landingTimeoutMs of [25000, 35000]) test(`back-transition recovery re | ||
| 244 | 253 | hover.timeoutMs = 45000; land.timeoutMs = landingTimeoutMs |
| 245 | 254 | const report = runVehicleSimulation(profile, { seconds: 160 }) |
| 246 | 255 | console.log('VTOL_BACK_TIMEOUT_RECOVERY', JSON.stringify({ landingTimeoutMs, status: report.status, fault: report.fault, mission: report.mission, budget: report.budget, finalPositionM: report.finalTruthPositionM, transitions: report.transitions })) |
| 247 | if (landingTimeoutMs === 25000) { | |
| 256 | expect(report.budget.worstCaseMs).toBe(worstCaseMs) | |
| 257 | expect(report.budget.requiredWh).toBeCloseTo(requiredWh, 8) | |
| 258 | const landingStart = report.transitions.find(row => row.taskId === 'recover-land' && row.state === 'running')!.atMs | |
| 259 | if (landingTimeoutMs === 20000) { | |
| 248 | 260 | expect(report.status).toBe('blocked') |
| 249 | 261 | expect(report.mission.reason).toBe('task-timeout') |
| 250 | 262 | expect(report.path.at(-1)!.measurement!.values.onSurface).toBe(false) |
| 251 | 263 | expect(report.finalTruthPositionM[2]).toBeLessThan(-1) |
| 252 | 264 | expect(report.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) |
| 265 | expect(report.board.expiresAtMs).toBeNull() | |
| 266 | expect(report.path.at(-1)!.atMs - landingStart).toBe(landingTimeoutMs) | |
| 253 | 267 | expect(replayVehicleMission(report.trace).snapshot()).toEqual(report.mission) |
| 254 | 268 | return |
| 255 | 269 | } |
| 256 | 270 | expect(report.status).toBe('completed') |
| 257 | expect(report.budget.worstCaseMs).toBe(156520) | |
| 258 | expect(report.budget.requiredWh).toBeCloseTo(52.17333333333333, 8) | |
| 271 | expect(report.path.at(-1)!.atMs - landingStart).toBeLessThan(landingTimeoutMs) | |
| 259 | 272 | expect(report.mission.reason).toBe('vtol-recovery-landed') |
| 260 | 273 | expect(report.mission.recovery).toMatchObject({ failedTaskId: 'back', hoverTaskId: 'recover-hover', cause: 'timeout' }) |
| 261 | 274 | const original = report.path.find(row => row.mission.taskId === 'back' && row.mission.transition)!.mission.transition! |
| @@ -2,7 +2,7 @@ import { expect, test } from 'bun:test' | ||
| 2 | 2 | import { createVtolTransitionProfile } from '../../resources/functions/drone/vehicles/vtol/transition-profile' |
| 3 | 3 | import { runVehicleSimulation } from '../../resources/functions/drone/vehicles/runtime' |
| 4 | 4 | import { createVtolFlightController } from '../../resources/functions/drone/vehicles/vtol/flight-controller' |
| 5 | import { multiplyQuaternion, quaternionEuler } from '../../resources/functions/drone/estimation/frames' | |
| 5 | import { multiplyQuaternion, quaternionEuler, quaternionFromTilt } from '../../resources/functions/drone/estimation/frames' | |
| 6 | 6 | import { replayVehicleMission, validateVehicleMission } from '../../resources/functions/drone/missions/vehicle' |
| 7 | 7 | import type { VehicleControllerContext, VehiclePlantState } from '../../resources/functions/drone/vehicles/types' |
| 8 | 8 | |
| @@ -333,3 +333,62 @@ test('back hover cannot confirm while a near-stationary body remains away from t | ||
| 333 | 333 | } |
| 334 | 334 | expect(qualifiedAt).toEqual([settled.nowMs + 520]) |
| 335 | 335 | }) |
| 336 | ||
| 337 | function unachievableBrakeObservation(context: VehicleControllerContext) { | |
| 338 | const input = structuredClone(context), pose = quaternionFromTilt(0, -0.4) | |
| 339 | input.estimate.attitude!.quaternionWxyz = [...pose] | |
| 340 | input.estimate.attitude!.eulerRad = [0, -0.4, 0] | |
| 341 | input.measurementPose!.quaternionWxyz = [...pose] | |
| 342 | input.controlRate!.angularRateRadS = [0, 0, 0] | |
| 343 | Object.assign(input.measurement!.values, { airspeedMps: 13, differentialPressurePa: 103.5125, alphaRad: 0.1, betaRad: 0 }) | |
| 344 | return input | |
| 345 | } | |
| 346 | ||
| 347 | test('back braking refuses and latches when qualified airflow admits no pitch inside the declared bounds', () => { | |
| 348 | const inputs = actualTransitionInputs(), index = inputs.findIndex(c => c.mission.transition?.taskId === 'back') | |
| 349 | expect(index).toBeGreaterThan(0) | |
| 350 | const controller = createVtolFlightController() | |
| 351 | let warmed = true | |
| 352 | for (const input of inputs.slice(0, index)) if (controller.step(input).inhibited) warmed = false | |
| 353 | expect(warmed).toBe(true) | |
| 354 | const refused = controller.step(unachievableBrakeObservation(inputs[index]!)) | |
| 355 | expect(refused.inhibited).toBe(true) | |
| 356 | expect(refused.reason).toBe('vtol-braking-no-feasible-pitch') | |
| 357 | expect(refused.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 358 | expect(refused.transition).toBeUndefined() | |
| 359 | expect(controller.step(inputs[index + 1]!).inhibited).toBe(true) | |
| 360 | }) | |
| 361 | ||
| 362 | function advanceControlEpoch(context: VehicleControllerContext) { | |
| 363 | const input = structuredClone(context) | |
| 364 | input.nowMs += 20; input.estimate.nowMs += 20; input.mission.elapsedMs += 20 | |
| 365 | input.estimate.attitude!.capturedAtMs! += 20 | |
| 366 | input.estimate.localPosition!.capturedAtMs! += 20 | |
| 367 | input.estimate.localPosition!.predictedToMs += 20 | |
| 368 | input.controlRate!.capturedAtMs += 20 | |
| 369 | input.measurement!.capturedAtMs += 20; input.measurement!.receivedAtMs += 20; input.measurement!.sequence++ | |
| 370 | for (const key of ['capturedAtMs', 'positionCapturedAtMs', 'positionPredictedToMs', 'attitudeCapturedAtMs', 'attitudePredictedToMs', 'gyroCapturedAtMs'] as const) input.measurementPose![key] += 20 | |
| 371 | return input | |
| 372 | } | |
| 373 | ||
| 374 | test('a later front recovery cannot inherit a previous back leg rotary-capture latch', () => { | |
| 375 | const inputs = actualTransitionInputs(), index = inputs.findIndex(c => c.mission.taskId === 'land') | |
| 376 | expect(index).toBeGreaterThan(0) | |
| 377 | const controller = createVtolFlightController() | |
| 378 | let warmed = true | |
| 379 | for (const input of inputs.slice(0, index)) if (controller.step(input).inhibited) warmed = false | |
| 380 | expect(warmed).toBe(true) | |
| 381 | const front = advanceControlEpoch(inputs[index - 1]!), anchor = [...front.estimate.localPosition!.positionM] as [number, number, number] | |
| 382 | front.mission.taskId = 'front-second'; front.mission.taskKind = 'vtol-mode'; front.mission.targetM = anchor | |
| 383 | front.mission.transition = { eventId: 'front-second', taskId: 'front-second', mode: 'cruise', headingRad: 0, issuedAtMs: front.nowMs, expiresAtMs: front.nowMs + 15000, positionM: anchor, reference: front.measurement!.reference } | |
| 384 | expect(controller.step(front).inhibited).toBe(false) | |
| 385 | const recovery = unachievableBrakeObservation(advanceControlEpoch(front)) | |
| 386 | recovery.mission.taskId = 'recover-second' | |
| 387 | recovery.mission.recovery = { failedEventId: 'front-second', failedTaskId: 'front-second', hoverTaskId: 'recover-second', triggeredAtMs: recovery.nowMs, cause: 'operator-recover' } | |
| 388 | recovery.mission.transition = { eventId: 'recover-second', taskId: 'recover-second', mode: 'hover', headingRad: 0, issuedAtMs: recovery.nowMs, expiresAtMs: recovery.nowMs + 45000, positionM: anchor, reference: recovery.measurement!.reference, supersedesEventId: 'front-second' } | |
| 389 | const refused = controller.step(recovery) | |
| 390 | expect(refused.inhibited).toBe(true) | |
| 391 | expect(refused.reason).toBe('vtol-braking-no-feasible-pitch') | |
| 392 | expect(refused.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 393 | expect(refused.transition).toBeUndefined() | |
| 394 | }) | |
| @@ -0,0 +1,49 @@ | ||
| 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 { airAerodynamics } from '../../resources/functions/drone/vehicles/air/plant' | |
| 5 | import { vtolAerodynamics } from '../../resources/functions/drone/vehicles/vtol/plant' | |
| 6 | import { quaternionFromTilt } from '../../resources/functions/drone/estimation/frames' | |
| 7 | import { rotateNedToBody } from '../../resources/functions/drone/controls/physics' | |
| 8 | ||
| 9 | // Agreed actual shared-runtime function seam: force-driven wind, raw IMU/GNSS, estimator, | |
| 10 | // mission, controller and independent board leases. It does not qualify measured airframes. | |
| 11 | test('the owned VTOL completes its finite route in a declared sustained 2 m/s tailwind', () => { | |
| 12 | const profile = createVtolTransitionProfile({ electrical: true }) | |
| 13 | profile.faults = (_scenario, atMs) => ({ windMps: atMs >= 1000 ? [2, 0, 0] : [0, 0, 0] }) | |
| 14 | const result = runVehicleSimulation(profile, { seconds: 160 }) | |
| 15 | expect(result.status, JSON.stringify({ status: result.status, fault: result.fault, task: result.mission.taskId, reason: result.mission.reason })).toBe('completed') | |
| 16 | expect(result.mission.reason).toBe('landed') | |
| 17 | expect(result.path.at(-1)?.measurement?.values.onSurface).toBe(true) | |
| 18 | expect(result.board.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0]) | |
| 19 | expect(result.board.expiresAtMs).toBeNull() | |
| 20 | expect(result.board.physicalActuationEnabled).toBe(false) | |
| 21 | }) | |
| 22 | ||
| 23 | test('the fictional VTOL wing opposes small vertical motion in either chordwise flow direction', () => { | |
| 24 | for (const u of [-2, 2]) { | |
| 25 | const up = vtolAerodynamics([u, 0, -0.01], [0, 0, 0], [0, 0, 0, 0, 0]) | |
| 26 | const down = vtolAerodynamics([u, 0, 0.01], [0, 0, 0], [0, 0, 0, 0, 0]) | |
| 27 | expect(down.forceN[2] - up.forceN[2], `body U ${u}`).toBeLessThan(0) | |
| 28 | const pitched = rotateNedToBody(quaternionFromTilt(0, 0.01), [u, 0, 0]) | |
| 29 | expect(vtolAerodynamics(pitched, [0, 0, 0], [0, 0, 0, 0, 0]).torqueNm[1], `body U ${u}`).toBeLessThan(0) | |
| 30 | } | |
| 31 | }) | |
| 32 | ||
| 33 | test('the VTOL extension preserves the declared forward pre-stall trainer curve exactly', () => { | |
| 34 | for (const alpha of [-0.28, -0.1, 0, 0.1, 0.28]) { | |
| 35 | const relative: [number, number, number] = [13 * Math.cos(alpha), 0, 13 * Math.sin(alpha)] | |
| 36 | expect(vtolAerodynamics(relative, [0.01, -0.02, 0.03], [0.2, 0.1, -0.1, 0.2, 0])).toEqual(airAerodynamics(relative, [0.01, -0.02, 0.03], [0.2, 0.1, -0.1, 0.2, 0])) | |
| 37 | } | |
| 38 | }) | |
| 39 | ||
| 40 | test('the VTOL folded-incidence curve has continuous force and pitch moment across reverse-flow angle wrapping', () => { | |
| 41 | for (const speed of [2, 8]) { | |
| 42 | const above = airAerodynamics([-speed, 0, 1e-8], [0, 0, 0], [0, 0, 0, 0, 0], true) | |
| 43 | const below = airAerodynamics([-speed, 0, -1e-8], [0, 0, 0], [0, 0, 0, 0, 0], true) | |
| 44 | expect(Math.hypot(...above.forceN.map((n, i) => n - below.forceN[i]!))).toBeLessThan(1e-5) | |
| 45 | expect(Math.hypot(...above.torqueNm.map((n, i) => n - below.torqueNm[i]!))).toBeLessThan(1e-5) | |
| 46 | expect(above.alphaRad).toBeGreaterThan(3) | |
| 47 | expect(below.alphaRad).toBeLessThan(-3) | |
| 48 | } | |
| 49 | }) | |