feat: add owned VTOL hover development

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.

resources/functions/drone/vehicles/vtol/plant.tsadded+84-0

Changes to resources/functions/drone/vehicles/vtol/plant.ts

		@@ -0,0 +1,84 @@
	1	+import type { SensorVector } from '../../sensors/types'
	2	+import type { Quaternion } from '../../estimation/frames'
	3	+import type { VehiclePlant, VehiclePlantState } from '../types'
	4	+import { validVector } from '../../controls/model'
	5	+import { multiplyQuaternion, normalizeQuaternion } from '../../estimation/frames'
	6	+import { rotateBodyToNed, rotateNedToBody } from '../../controls/physics'
	7	+import { airAerodynamics, airModel } from '../air/plant'
	8	+
	9	+/** Fictional lift-and-cruise model. No measured hardware coefficients or flight qualification. */
	10	+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	+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	+/** Independent lift rotors and the owned trainer wing/pusher share one rigid body. */
	24	+export function createVtolPlant(initial: { positionM?: SensorVector, velocityMps?: SensorVector, quaternionWxyz?: Quaternion, rotorsN?: [number, number, number, number], pusherThrustN?: number } = {}): VehiclePlant {
	25	+ let positionM = numericPacket(initial.positionM === undefined ? [0, 0, 0] : initial.positionM, 3) as SensorVector
	26	+ let rotorsN = numericPacket(initial.rotorsN === undefined ? [0, 0, 0, 0] : initial.rotorsN, 4)
	27	+ let pusherThrustN = initial.pusherThrustN === undefined ? 0 : initial.pusherThrustN
	28	+ let velocityMps = numericPacket(initial.velocityMps === undefined ? [0, 0, 0] : initial.velocityMps, 3) as SensorVector, accelerationMps2: SensorVector = [0, 0, 0]
	29	+ let windMps: SensorVector = [0, 0, 0]
	30	+ let quaternionWxyz = normalizeQuaternion(numericPacket(initial.quaternionWxyz === undefined ? [1, 0, 0, 0] : initial.quaternionWxyz, 4) as Quaternion), angularRateRadS: SensorVector = [0, 0, 0]
	31	+ if (!validVector(positionM) || !validVector(velocityMps) || positionM[2] > 0 || rotorsN.length !== 4 || rotorsN.some(n => !Number.isFinite(n) || n < 0 || n > vtolModel.maxRotorThrustN) || !Number.isFinite(pusherThrustN) || pusherThrustN < 0 || pusherThrustN > vtolModel.maxPusherThrustN) throw new RangeError('Invalid initial VTOL model')
	32	+ let onSurface = positionM[2] === 0
	33	+ function snapshot(): VehiclePlantState {
	34	+ const relative = rotateNedToBody(quaternionWxyz, velocityMps.map((n, i) => n - windMps[i]!) as SensorVector)
	35	+ const airspeedMps = Math.hypot(...relative)
	36	+ return { positionM: [...positionM], velocityMps: [...velocityMps], accelerationMps2: [...accelerationMps2], quaternionWxyz: [...quaternionWxyz], angularRateRadS: [...angularRateRadS], onSurface, details: { airspeedMps, alphaRad: Math.atan2(relative[2], relative[0]), betaRad: Math.atan2(relative[1], Math.hypot(relative[0], relative[2])), dynamicPressurePa: 0.5 * airModel.densityKgM3 * airspeedMps ** 2, rotorThrustN: rotorsN.reduce((a, b) => a + b, 0), pusherThrustN } }
	37	+ }
	38	+ return {
	39	+ snapshot,
	40	+ step(inputActuators, dtS, environment) {
	41	+ const actuators = numericPacket(inputActuators, 8)
	42	+ const nextWindMps = numericPacket(environment.windMps, 3) as SensorVector, currentMps = numericPacket(environment.currentMps, 3) as SensorVector
	43	+ if (actuators.slice(0, 4).some(n => n < 0 || n > vtolModel.maxRotorThrustN) || actuators[4]! < 0 || actuators[4]! > 1 || actuators.slice(5).some(n => Math.abs(n) > 0.4)) throw new RangeError('Invalid VTOL actuator channels')
	44	+ if (!Number.isFinite(dtS) || dtS <= 0 || dtS > 0.01 || !validVector(nextWindMps) || !validVector(currentMps) || currentMps.some(n => n !== 0)) throw new RangeError('Invalid VTOL environment')
	45	+ const relative = rotateNedToBody(quaternionWxyz, velocityMps.map((n, i) => n - nextWindMps[i]!) as SensorVector)
	46	+ const response = 1 - Math.exp(-dtS / vtolModel.rotorTimeConstantS), maximumChangeN = vtolModel.rotorSlewNPerS * dtS
	47	+ const nextRotorsN = rotorsN.map((n, i) => n + Math.max(-maximumChangeN, Math.min(maximumChangeN, (actuators[i]! - n) * response)))
	48	+ const pusherChangeN = (actuators[4]! * vtolModel.maxPusherThrustN - pusherThrustN) * (1 - Math.exp(-dtS / vtolModel.pusherTimeConstantS))
	49	+ const pusherMaximumChangeN = vtolModel.pusherSlewNPerS * dtS
	50	+ const nextPusherThrustN = pusherThrustN + Math.max(-pusherMaximumChangeN, Math.min(pusherMaximumChangeN, pusherChangeN))
	51	+ const aero = airAerodynamics(relative, angularRateRadS, [nextPusherThrustN / vtolModel.maxPusherThrustN, actuators[5]!, actuators[6]!, actuators[7]!, 0])
	52	+ // Front-left, front-right, rear-right, rear-left. Rotor reaction signs are +, -, +, - about BODY_FRD z.
	53	+ const torqueNm: SensorVector = [
	54	+ aero.torqueNm[0] + vtolModel.rotorArmProjectionM * (nextRotorsN[0]! - nextRotorsN[1]! - nextRotorsN[2]! + nextRotorsN[3]!),
	55	+ aero.torqueNm[1] + vtolModel.rotorArmProjectionM * (nextRotorsN[0]! + nextRotorsN[1]! - nextRotorsN[2]! - nextRotorsN[3]!),
	56	+ aero.torqueNm[2] + vtolModel.reactionTorqueM * (nextRotorsN[0]! - nextRotorsN[1]! + nextRotorsN[2]! - nextRotorsN[3]!),
	57	+ ]
	58	+ const momentum = angularRateRadS.map((n, i) => n * vtolModel.inertiaKgM2[i]!)
	59	+ const cross: SensorVector = [angularRateRadS[1] * momentum[2]! - angularRateRadS[2] * momentum[1]!, angularRateRadS[2] * momentum[0]! - angularRateRadS[0] * momentum[2]!, angularRateRadS[0] * momentum[1]! - angularRateRadS[1] * momentum[0]!]
	60	+ const nextAngularRateRadS = angularRateRadS.map((n, i) => n + dtS * (torqueNm[i]! - cross[i]!) / vtolModel.inertiaKgM2[i]!) as SensorVector
	61	+ if (!nextAngularRateRadS.every(Number.isFinite) || Math.hypot(...nextAngularRateRadS) > 40) throw new RangeError('VTOL rotor model diverged')
	62	+ const angle = Math.hypot(...nextAngularRateRadS) * dtS, scale = angle < 1e-10 ? dtS / 2 : Math.sin(angle / 2) * dtS / angle
	63	+ const nextQuaternionWxyz = normalizeQuaternion(multiplyQuaternion(quaternionWxyz, [Math.cos(angle / 2), nextAngularRateRadS[0] * scale, nextAngularRateRadS[1] * scale, nextAngularRateRadS[2] * scale]))
	64	+ const forceN = rotateBodyToNed(nextQuaternionWxyz, [aero.forceN[0], aero.forceN[1], aero.forceN[2] - nextRotorsN.reduce((a, b) => a + b, 0)])
	65	+ const nextAccelerationMps2 = forceN.map((n, i) => n / vtolModel.massKg + (i === 2 ? vtolModel.gravityMps2 : 0)) as SensorVector
	66	+ const nextVelocityMps = velocityMps.map((n, i) => n + nextAccelerationMps2[i]! * dtS) as SensorVector
	67	+ const nextPositionM = positionM.map((n, i) => n + nextVelocityMps[i]! * dtS) as SensorVector
	68	+ if (![...nextAccelerationMps2, ...nextVelocityMps, ...nextPositionM].every(Number.isFinite)) throw new RangeError('VTOL rotor model diverged')
	69	+ const nextOnSurface = nextPositionM[2] >= 0
	70	+ if (nextOnSurface) { nextPositionM[2] = 0; nextVelocityMps[2] = Math.min(0, nextVelocityMps[2]); nextAccelerationMps2[2] = 0 }
	71	+ // Reject the entire integration before committing any retained state.
	72	+ windMps = nextWindMps
	73	+ rotorsN = nextRotorsN
	74	+ pusherThrustN = nextPusherThrustN
	75	+ angularRateRadS = nextAngularRateRadS
	76	+ quaternionWxyz = nextQuaternionWxyz
	77	+ accelerationMps2 = nextAccelerationMps2
	78	+ velocityMps = nextVelocityMps
	79	+ positionM = nextPositionM
	80	+ onSurface = nextOnSurface
	81	+ return snapshot()
	82	+ },
	83	+ }
	84	+}

tests/tsconfig.runtime.jsonmodified+1-0

Changes to tests/tsconfig.runtime.json

		⋯71 linesShow the 71 lines above this hunk@@ -72,6 +72,7 @@
72	72	"./unit/vehicle-authority.test.ts",
73	73	"./unit/vehicle-runtime.test.ts",
74	74	"./unit/air.test.ts",
	75	+ "./unit/vtol.test.ts",
75	76	"./unit/ground.test.ts",
76	77	"./unit/surface.test.ts",
77	78	"./unit/mission-workspace.test.ts",

tests/unit/vtol.test.tsadded+132-0

Changes to tests/unit/vtol.test.ts

		@@ -0,0 +1,132 @@
	1	+import { expect, test } from 'bun:test'
	2	+import { createVtolPlant } from '../../resources/functions/drone/vehicles/vtol/plant'
	3	+
	4	+// Public plant seam: checks vertical force integration without forward airspeed; it does not qualify a controller or physical aircraft.
	5	+test('lift-and-cruise rotors support the declared airframe without forward airspeed or an imposed position', () => {
	6	+ // Fictional 2.5 kg model: each of four rotors supplies 6.12915625 N in a level hover.
	7	+ const rotorsN: [number, number, number, number] = [6.12915625, 6.12915625, 6.12915625, 6.12915625]
	8	+ const supported = createVtolPlant({ positionM: [0, 0, -10], rotorsN })
	9	+ const environment = { windMps: [0, 0, 0] as [number, number, number], currentMps: [0, 0, 0] as [number, number, number] }
	10	+ let state = supported.snapshot()
	11	+ // Channels are four rotor thrusts in N, normalized pusher throttle, then three surfaces in radians.
	12	+ for (let step = 0; step < 250; step++) state = supported.step([...rotorsN, 0, 0, 0, 0], 0.004, environment)
	13	+ expect(state.positionM[2]).toBeCloseTo(-10, 8)
	14	+ expect(state.velocityMps[2]).toBeCloseTo(0, 8)
	15	+ expect(state.accelerationMps2[2]).toBeCloseTo(0, 8)
	16	+ expect(state.details.airspeedMps).toBeCloseTo(0, 8)
	17	+ expect(state.onSurface).toBe(false)
	18	+
	19	+ const unsupported = createVtolPlant({ positionM: [0, 0, -10] })
	20	+ const falling = unsupported.step([0, 0, 0, 0, 0, 0, 0, 0], 0.004, environment)
	21	+ expect(falling.accelerationMps2[2]).toBeCloseTo(9.80665, 8)
	22	+ expect(falling.velocityMps[2]).toBeGreaterThan(0)
	23	+ expect(falling.positionM[2]).toBeGreaterThan(-10)
	24	+})
	25	+
	26	+test('rotor thrust spools up and decays after command release instead of changing instantaneously', () => {
	27	+ const plant = createVtolPlant({ positionM: [0, 0, -10] })
	28	+ const spooling = plant.step([12, 12, 12, 12, 0, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] })
	29	+ expect(spooling.accelerationMps2[2]).toBeGreaterThan(8)
	30	+ expect(spooling.accelerationMps2[2]).toBeLessThan(9.80665)
	31	+ const released = plant.step([0, 0, 0, 0, 0, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] })
	32	+ expect(released.accelerationMps2[2]).toBeGreaterThan(8)
	33	+ expect(released.accelerationMps2[2]).toBeLessThan(9.80665)
	34	+ expect(released.details.rotorThrustN).toBeGreaterThan(0)
	35	+ expect(released.details.rotorThrustN).toBeLessThan(spooling.details.rotorThrustN as number)
	36	+})
	37	+
	38	+test('independent lift rotors generate body moments without forward airspeed', () => {
	39	+ // Rotor order: front-left, front-right, rear-right, rear-left in BODY_FRD.
	40	+ const oneRotor = createVtolPlant({ positionM: [0, 0, -10] })
	41	+ const turning = oneRotor.step([12, 0, 0, 0, 0, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] })
	42	+ expect(turning.angularRateRadS.every(n => n > 0)).toBe(true)
	43	+ expect(turning.quaternionWxyz.slice(1).every(n => n > 0)).toBe(true)
	44	+ const frontPair = createVtolPlant({ positionM: [0, 0, -10] })
	45	+ const pitching = frontPair.step([12, 12, 0, 0, 0, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] })
	46	+ expect(pitching.angularRateRadS[0]).toBeCloseTo(0, 10)
	47	+ expect(pitching.angularRateRadS[1]).toBeGreaterThan(0)
	48	+ expect(pitching.angularRateRadS[2]).toBeCloseTo(0, 10)
	49	+})
	50	+
	51	+test('wing lift and the independent pusher act on the same body using relative airflow', () => {
	52	+ const still = createVtolPlant({ positionM: [0, 0, -20], velocityMps: [10, 0, 0] })
	53	+ const glide = still.step([0, 0, 0, 0, 0, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] })
	54	+ // At 10 m/s the declared trainer wing supplies 12.25 N of lift and 2.14375 N of drag.
	55	+ expect(glide.accelerationMps2[2]).toBeCloseTo(4.90665, 8)
	56	+ expect(glide.accelerationMps2[0]).toBeCloseTo(-0.8575, 8)
	57	+ const powered = createVtolPlant({ positionM: [0, 0, -20], velocityMps: [10, 0, 0], pusherThrustN: 7 })
	58	+ const thrust = powered.step([0, 0, 0, 0, 0.5, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] })
	59	+ expect(thrust.accelerationMps2[0]).toBeCloseTo(1.9425, 8)
	60	+ const windy = createVtolPlant({ positionM: [0, 0, -20], velocityMps: [10, 0, 0] })
	61	+ const headwind = windy.step([0, 0, 0, 0, 0, 0, 0, 0], 0.004, { windMps: [-5, 0, 0], currentMps: [0, 0, 0] })
	62	+ expect(headwind.accelerationMps2[2]).toBeLessThan(glide.accelerationMps2[2])
	63	+ expect(headwind.details.airspeedMps).toBeGreaterThan(14.9)
	64	+ expect(headwind.details.airspeedMps).toBeLessThan(15.1)
	65	+})
	66	+
	67	+test('aerodynamic surfaces gain moment authority with airflow while rotor controls remain independent', () => {
	68	+ const resting = createVtolPlant({ positionM: [0, 0, -20] })
	69	+ const noAir = resting.step([0, 0, 0, 0, 0, 0.4, 0.4, 0.4], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] })
	70	+ expect(noAir.angularRateRadS).toEqual([0, 0, 0])
	71	+ const moving = createVtolPlant({ positionM: [0, 0, -20], velocityMps: [10, 0, 0] })
	72	+ const controlled = moving.step([0, 0, 0, 0, 0, 0.4, 0.4, 0.4], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] })
	73	+ // Independently worked first-step surface moments: 5.6448, 5.22144 and 3.7632 Nm.
	74	+ expect(controlled.angularRateRadS[0]).toBeCloseTo(0.075264, 8)
	75	+ expect(controlled.angularRateRadS[1]).toBeCloseTo(0.0522144, 8)
	76	+ expect(controlled.angularRateRadS[2]).toBeCloseTo(0.025088, 8)
	77	+})
	78	+
	79	+test('body-relative airflow and forces rotate consistently into the fixed NED frame', () => {
	80	+ const eastFacing = createVtolPlant({ positionM: [0, 0, -20], velocityMps: [0, 10, 0], quaternionWxyz: [Math.SQRT1_2, 0, 0, Math.SQRT1_2], pusherThrustN: 7 })
	81	+ const east = eastFacing.step([0, 0, 0, 0, 0.5, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] })
	82	+ expect(east.accelerationMps2[0]).toBeCloseTo(0, 8)
	83	+ expect(east.accelerationMps2[1]).toBeCloseTo(1.9425, 8)
	84	+ expect(east.accelerationMps2[2]).toBeCloseTo(4.90665, 8)
	85	+ expect(east.details.betaRad).toBeCloseTo(0, 8)
	86	+})
	87	+
	88	+test('malformed numeric packets are refused before changing the owned body or wind', () => {
	89	+ const plant = createVtolPlant({ positionM: [0, 0, -20] })
	90	+ const before = plant.snapshot()
	91	+ const sparse: number[] = []
	92	+ sparse.length = 8
	93	+ sparse.fill(0, 1)
	94	+ expect(() => plant.step(sparse, 0.004, { windMps: [-2, 0, 0], currentMps: [0, 0, 0] })).toThrow()
	95	+ expect(plant.snapshot()).toEqual(before)
	96	+ expect(() => createVtolPlant({ quaternionWxyz: [1, 0, 0, 0, 7] as never })).toThrow()
	97	+ expect(() => plant.step([13, 0, 0, 0, 0, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] })).toThrow()
	98	+ expect(plant.snapshot()).toEqual(before)
	99	+})
	100	+
	101	+test('explicit null initial numeric fields cannot become a default vehicle state', () => {
	102	+ for (const field of ['positionM', 'velocityMps', 'quaternionWxyz', 'rotorsN', 'pusherThrustN']) {
	103	+ expect(() => createVtolPlant({ [field]: null } as never)).toThrow()
	104	+ }
	105	+ expect(createVtolPlant().snapshot().positionM).toEqual([0, 0, 0])
	106	+})
	107	+
	108	+test('the independent pusher spools up and retains decaying thrust after release', () => {
	109	+ const plant = createVtolPlant({ positionM: [0, 0, -20] })
	110	+ const spooling = plant.step([0, 0, 0, 0, 1, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] })
	111	+ expect(spooling.accelerationMps2[0]).toBeGreaterThan(0)
	112	+ expect(spooling.accelerationMps2[0]).toBeLessThan(1)
	113	+ expect(spooling.details.pusherThrustN).toBeGreaterThan(0)
	114	+ expect(spooling.details.pusherThrustN).toBeLessThan(14)
	115	+ const released = plant.step([0, 0, 0, 0, 0, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] })
	116	+ expect(released.details.pusherThrustN).toBeGreaterThan(0)
	117	+ expect(released.details.pusherThrustN).toBeLessThan(spooling.details.pusherThrustN as number)
	118	+})
	119	+
	120	+test('a divergent integration leaves the retained body intact and the next ordinary step usable', () => {
	121	+ // Public step/snapshot catches rejected integration poisoning; it does not qualify a flight controller or recovery policy.
	122	+ for (const windMps of [[1000000, 1000000, 0], [0, 0, 1000000]] as [number, number, number][]) {
	123	+ const initial = { positionM: [0, 0, -20] as [number, number, number], velocityMps: [10, 0, 0] as [number, number, number], rotorsN: [1, 2, 3, 4] as [number, number, number, number], pusherThrustN: 7 }
	124	+ const plant = createVtolPlant(initial), reference = createVtolPlant(initial)
	125	+ const before = plant.snapshot()
	126	+ const command = [12, 12, 12, 12, 1, 0, 0, 0]
	127	+ expect(() => plant.step(command, 0.004, { windMps, currentMps: [0, 0, 0] })).toThrow()
	128	+ expect(plant.snapshot()).toEqual(before)
	129	+ const environment = { windMps: [0, 0, 0] as [number, number, number], currentMps: [0, 0, 0] as [number, number, number] }
	130	+ expect(plant.step(command, 0.004, environment)).toEqual(reference.step(command, 0.004, environment))
	131	+ }
	132	+})