also looking at this
feat: add owned VTOL hover development
#76
30 files
+780
-111
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.
| @@ -0,0 +1,28 @@ | ||
| 1 | import type { Motors, Wrench } from '../../controls/model' | |
| 2 | import { validVector } from '../../controls/model' | |
| 3 | import { vtolModel as model } from './plant' | |
| 4 | import { numericPacket } from './packets' | |
| 5 | ||
| 6 | /** Front-left, front-right, rear-right, rear-left; BODY_FRD moments. */ | |
| 7 | export function allocateVtolLift(wrench: Wrench) { | |
| 8 | if (!wrench || !Array.isArray(wrench.torqueNm)) throw new RangeError('Invalid VTOL lift wrench') | |
| 9 | const thrustN = wrench.thrustN, torqueNm = numericPacket(wrench.torqueNm, 3) | |
| 10 | if (!Number.isFinite(thrustN) || !validVector(torqueNm)) throw new RangeError('Invalid VTOL lift wrench') | |
| 11 | const t = Math.max(0, Math.min(model.maxRotorThrustN, thrustN / 4)), r = torqueNm[0] / (4 * model.rotorArmProjectionM), p = torqueNm[1] / (4 * model.rotorArmProjectionM), y = torqueNm[2] / (4 * model.reactionTorqueM) | |
| 12 | function addWithinBounds(base: number[], differential: number[]) { | |
| 13 | let scale = 1 | |
| 14 | for (let i = 0; i < 4; i++) { | |
| 15 | const change = differential[i]! | |
| 16 | if (change > 0) scale = Math.min(scale, (model.maxRotorThrustN - base[i]!) / change) | |
| 17 | else if (change < 0) scale = Math.min(scale, -base[i]! / change) | |
| 18 | } | |
| 19 | return base.map((n, i) => Math.max(0, Math.min(model.maxRotorThrustN, n + scale * differential[i]!))) | |
| 20 | } | |
| 21 | // Preserve collective first, then the roll/pitch vector, then yaw. Report the achieved wrench for anti-windup. | |
| 22 | const rollPitch = addWithinBounds([t, t, t, t], [r + p, -r + p, -r - p, r - p]) | |
| 23 | const rotorsN = addWithinBounds(rollPitch, [y, -y, y, -y]) as Motors | |
| 24 | const [a, b, c, d] = rotorsN | |
| 25 | const achieved: Wrench = { thrustN: a + b + c + d, torqueNm: [model.rotorArmProjectionM * (a - b - c + d), model.rotorArmProjectionM * (a + b - c - d), model.reactionTorqueM * (a - b + c - d)] } | |
| 26 | const limited = Math.abs(achieved.thrustN - thrustN) > 1e-9 || achieved.torqueNm.some((n, i) => Math.abs(n - torqueNm[i]!) > 1e-9) | |
| 27 | return { rotorsN, achieved, limited } | |
| 28 | } | |
| @@ -0,0 +1,11 @@ | ||
| 1 | /** Capture own numeric data without invoking array iterators or index accessors. */ | |
| 2 | export function numericPacket(value: unknown, length: number): number[] { | |
| 3 | if (!Array.isArray(value) || value.length !== length) throw new RangeError('Invalid VTOL numeric packet') | |
| 4 | const packet: number[] = [] | |
| 5 | for (let index = 0; index < length; index++) { | |
| 6 | const field = Object.getOwnPropertyDescriptor(value, String(index)) | |
| 7 | if (!field?.enumerable || !Object.hasOwn(field, 'value') || typeof field.value !== 'number' || !Number.isFinite(field.value)) throw new RangeError('Invalid VTOL numeric packet') | |
| 8 | packet.push(field.value) | |
| 9 | } | |
| 10 | return packet | |
| 11 | } | |
| @@ -5,21 +5,11 @@ import { validVector } from '../../controls/model' | ||
| 5 | 5 | import { multiplyQuaternion, normalizeQuaternion } from '../../estimation/frames' |
| 6 | 6 | import { rotateBodyToNed, rotateNedToBody } from '../../controls/physics' |
| 7 | 7 | import { airAerodynamics, airModel } from '../air/plant' |
| 8 | import { numericPacket } from './packets' | |
| 8 | 9 | |
| 9 | 10 | /** Fictional lift-and-cruise model. No measured hardware coefficients or flight qualification. */ |
| 10 | 11 | export const vtolModel = Object.freeze({ massKg: 2.5, gravityMps2: 9.80665, maxRotorThrustN: 12, rotorTimeConstantS: 0.06, rotorSlewNPerS: 120, rotorArmProjectionM: 0.4, reactionTorqueM: 0.02, inertiaKgM2: Object.freeze([0.3, 0.4, 0.6] as const), maxPusherThrustN: airModel.maxThrustN, pusherTimeConstantS: 0.08, pusherSlewNPerS: 60, wing: airModel }) |
| 11 | 12 | |
| 12 | function numericPacket(value: unknown, length: number): number[] { | |
| 13 | if (!Array.isArray(value) || value.length !== length) throw new RangeError('Invalid VTOL numeric packet') | |
| 14 | const packet: number[] = [] | |
| 15 | for (let index = 0; index < length; index++) { | |
| 16 | const field = Object.getOwnPropertyDescriptor(value, String(index)) | |
| 17 | if (!field?.enumerable || !Object.hasOwn(field, 'value') || typeof field.value !== 'number' || !Number.isFinite(field.value)) throw new RangeError('Invalid VTOL numeric packet') | |
| 18 | packet.push(field.value) | |
| 19 | } | |
| 20 | return packet | |
| 21 | } | |
| 22 | ||
| 23 | 13 | /** Independent lift rotors and the owned trainer wing/pusher share one rigid body. */ |
| 24 | 14 | export function createVtolPlant(initial: { positionM?: SensorVector, velocityMps?: SensorVector, quaternionWxyz?: Quaternion, rotorsN?: [number, number, number, number], pusherThrustN?: number } = {}): VehiclePlant { |
| 25 | 15 | let positionM = numericPacket(initial.positionM === undefined ? [0, 0, 0] : initial.positionM, 3) as SensorVector |
| @@ -1,5 +1,6 @@ | ||
| 1 | 1 | import { expect, test } from 'bun:test' |
| 2 | 2 | import { createVtolPlant } from '../../resources/functions/drone/vehicles/vtol/plant' |
| 3 | import { allocateVtolLift } from '../../resources/functions/drone/vehicles/vtol/allocation' | |
| 3 | 4 | |
| 4 | 5 | // Public plant seam: checks vertical force integration without forward airspeed; it does not qualify a controller or physical aircraft. |
| 5 | 6 | test('lift-and-cruise rotors support the declared airframe without forward airspeed or an imposed position', () => { |
| @@ -130,3 +131,43 @@ test('a divergent integration leaves the retained body intact and the next ordin | ||
| 130 | 131 | expect(plant.step(command, 0.004, environment)).toEqual(reference.step(command, 0.004, environment)) |
| 131 | 132 | } |
| 132 | 133 | }) |
| 134 | ||
| 135 | test('VTOL lift allocation produces the declared BODY_FRD wrench in front-left rotor order', () => { | |
| 136 | // Independent inverse: [10, 6, 4, 4] N produces 24 N and [1.6, 3.2, 0.08] Nm. | |
| 137 | const result = allocateVtolLift({ thrustN: 24, torqueNm: [1.6, 3.2, 0.08] }) | |
| 138 | expect(result.rotorsN).toEqual([10, 6, 4, 4]) | |
| 139 | expect(result.achieved.thrustN).toBe(24) | |
| 140 | expect(result.achieved.torqueNm[0]).toBeCloseTo(1.6, 10) | |
| 141 | expect(result.achieved.torqueNm[1]).toBeCloseTo(3.2, 10) | |
| 142 | expect(result.achieved.torqueNm[2]).toBeCloseTo(0.08, 10) | |
| 143 | expect(result.limited).toBe(false) | |
| 144 | }) | |
| 145 | ||
| 146 | test('saturated lift allocation preserves collective before roll and pitch and sacrifices yaw first', () => { | |
| 147 | const result = allocateVtolLift({ thrustN: 44, torqueNm: [3.2, 0, 0.08] }) | |
| 148 | expect(result.rotorsN).toEqual([12, 10, 10, 12]) | |
| 149 | expect(result.achieved.thrustN).toBe(44) | |
| 150 | expect(result.achieved.torqueNm).toEqual([1.6, 0, 0]) | |
| 151 | expect(result.limited).toBe(true) | |
| 152 | }) | |
| 153 | ||
| 154 | test('lift allocation refuses malformed and nonfinite wrench packets instead of returning invalid rotor commands', () => { | |
| 155 | const sparse: number[] = [] | |
| 156 | sparse.length = 3 | |
| 157 | sparse.fill(0, 1) | |
| 158 | for (const wrench of [{ thrustN: Number.NaN, torqueNm: [0, 0, 0] }, { thrustN: 24, torqueNm: sparse }, { thrustN: 24, torqueNm: [0, 0, Infinity] }, { thrustN: 24, torqueNm: [0, 0, 0, 1] }, { thrustN: 24, torqueNm: null }]) { | |
| 159 | expect(() => allocateVtolLift(wrench as never)).toThrow() | |
| 160 | } | |
| 161 | }) | |
| 162 | ||
| 163 | test('inherited slots and custom iterators cannot disguise malformed lift torque packets', () => { | |
| 164 | const sparse: number[] = [] | |
| 165 | sparse.length = 3 | |
| 166 | sparse.fill(0, 1) | |
| 167 | Object.setPrototypeOf(sparse, Object.assign(Object.create(Array.prototype), { 0: 0 })) | |
| 168 | let iteratorCalls = 0 | |
| 169 | const rewritten = [NaN, 0, 0] | |
| 170 | Object.defineProperty(rewritten, Symbol.iterator, { value: function* () { iteratorCalls++; yield 0; yield 0; yield 0 } }) | |
| 171 | for (const torqueNm of [sparse, rewritten]) expect(() => allocateVtolLift({ thrustN: 24, torqueNm } as never)).toThrow() | |
| 172 | expect(iteratorCalls).toBe(0) | |
| 173 | }) | |