ReviewOS
Reload

also looking at this

stacks/drone

feat: add owned VTOL hover development

#76
Merged chris wants to merge codex/owned-vtol-lift-and-cruise into main
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.

resources/functions/drone/controls/controller.tsmodified+7-75
@@ -1,78 +1,10 @@
1import type { EstimatorSnapshot } from '../estimation/estimator'
2import type { SensorVector } from '../sensors/types'
3import type { Motors } from './model'
4import { multiplyQuaternion, quaternionFromTilt } from '../estimation/frames'
1import { createSimulatedLiftController } from './lift-controller'
52import { allocateQuadX } from './allocation'
6import { createBoundedPid } from './pid'
7import { clampControl, multirotorModel as model, validVector } from './model'
8export interface SimulatedControlInput {
9 kind: 'simulation'
10 enabled: boolean
11 nowMs: number
12 estimate: EstimatorSnapshot
13 angularRateRadS: SensorVector
14 rateCapturedAtMs: number
15 target: { positionM: SensorVector, yawRad: number, issuedAtMs: number }
16}
17/** No vehicle adapter or IO. The initial heading relationship is a declared simulation assumption. */
3import { multirotorModel as model } from './model'
4
5export type { SimulatedControlInput, SimulatedControlOutput } from './lift-controller'
6
7/** Original quad-X model and tuning, on the shared simulation-only lift cascade. */
188export function createSimulatedMultirotorController(options: { headingAlignment: 'initial-simulation-north' }) {
19 if (options.headingAlignment !== 'initial-simulation-north') throw new TypeError('An explicit simulation heading alignment is required')
20 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 }))
21 const ratePid = [0, 1, 2].map(i => createBoundedPid({ kp: i === 2 ? 0.08 : 0.12, ki: i === 2 ? 0.04 : 0.08, kd: i === 2 ? 0.001 : 0.002, integralLimit: 0.12, outputLimit: i === 2 ? 0.25 : 0.6, derivativeTauS: 0.03, trackingGain: 4 }))
22 let previous: Motors = [0, 0, 0, 0], lastNow: number | null = null, reference: string | null = null, fault: string | null = null
23 function clear() { for (const pid of [...velocityPid, ...ratePid]) pid.reset(); previous = [0, 0, 0, 0] }
24 function inhibit(reason: string, latch = reference !== null) {
25 if (latch) fault = reason
26 clear()
27 return { kind: 'simulated-control' as const, atMs: lastNow, inhibited: true, reason, physicalFlightEnabled: false as const, boardOutputsInhibited: true as const, motorsN: [...previous] as Motors, limited: false, desiredVelocityMps: null, desiredAccelerationMps2: null, desiredRatesRadS: null, desiredWrench: null, achievedWrench: null, integrals: { velocity: velocityPid.map(p => p.state().integral), rate: ratePid.map(p => p.state().integral) } }
28 }
29 return {
30 reset() { clear(); lastNow = null; reference = null; fault = null },
31 step(input: SimulatedControlInput) {
32 if (input.kind !== 'simulation') return inhibit('simulation-only', true)
33 const now = input.nowMs
34 if (!Number.isFinite(now) || (lastNow !== null && now <= lastNow)) return inhibit('control-clock-discontinuity', true)
35 const dt = lastNow === null ? 0.02 : (now - lastNow) / 1000
36 lastNow = now
37 if (!input.enabled) { reference = null; fault = null; return inhibit('disabled', false) }
38 if (fault) return inhibit(fault, true)
39 if (dt < 0.005 || dt > 0.04) return inhibit('control-schedule-gap', true)
40 const e = input.estimate, attitude = e?.attitude, position = e?.localPosition, target = input.target
41 if (!e || typeof e.bootId !== 'bigint' || e.bootId <= 0n || !Object.values(e.resets).every(n => Number.isInteger(n) && n >= 0) || e.evidence !== 'fixture' || e.physicalFlightEnabled !== false || !e.health.tilt || !e.health.localPosition || !attitude || !position) return inhibit('estimator-unhealthy')
42 if (!Number.isFinite(e.nowMs) || Math.abs(e.nowMs - now) > 1e-6 || attitude.capturedAtMs === null || !Number.isFinite(attitude.capturedAtMs) || attitude.capturedAtMs > now || now - attitude.capturedAtMs > 60
43 || position.capturedAtMs === null || !Number.isFinite(position.capturedAtMs) || !Number.isFinite(position.predictedToMs) || position.predictedToMs > now || now - position.predictedToMs > 40 || position.capturedAtMs > now || now - position.capturedAtMs > 200 || !Number.isFinite(input.rateCapturedAtMs) || input.rateCapturedAtMs > now || now - input.rateCapturedAtMs > 60) return inhibit('stale-control-observation')
44 if (attitude.reference !== 'GRAVITY_ALIGNED_RELATIVE_YAW' || position.frame !== 'LOCAL_NED' || position.altitudeDatum !== 'WGS84_ELLIPSOID' || !validVector(position.positionM) || !validVector(position.velocityMps) || !validVector(input.angularRateRadS)
45 || !position.origin || ![position.origin.latitudeDeg, position.origin.longitudeDeg, position.origin.ellipsoidHeightM].every(Number.isFinite) || Math.hypot(...input.angularRateRadS) > 35 || attitude.quaternionWxyz.length !== 4 || !attitude.quaternionWxyz.every(Number.isFinite) || Math.abs(Math.hypot(...attitude.quaternionWxyz) - 1) > 1e-6) return inhibit('invalid-control-frame-or-values')
46 if (Math.abs(input.rateCapturedAtMs - attitude.capturedAtMs) > 1e-6) return inhibit('unmatched-rate-epoch')
47 if (!target || !validVector(target.positionM) || Math.hypot(...target.positionM) > 100 || !Number.isFinite(target.yawRad) || Math.abs(target.yawRad) > Math.PI || !Number.isFinite(target.issuedAtMs) || target.issuedAtMs > now || now - target.issuedAtMs > 200) return inhibit('invalid-or-stale-target')
48 const identity = JSON.stringify([String(e.bootId), e.resets, position.origin])
49 if (reference !== null && identity !== reference) return inhibit('estimator-reference-reset', true)
50 reference = identity
51 const velocity = target.positionM.map((n, i) => n - position.positionM[i]!) as SensorVector
52 const horizontal = Math.hypot(velocity[0], velocity[1])
53 if (horizontal > 3) { velocity[0] *= 3 / horizontal; velocity[1] *= 3 / horizontal }
54 velocity[2] = clampControl(velocity[2], 2)
55 const acceleration = velocity.map((n, i) => velocityPid[i]!.step(n - position.velocityMps[i]!, position.velocityMps[i]!, dt)) as SensorVector
56 const beforeLimit = [...acceleration]
57 const horizontalAcceleration = Math.hypot(acceleration[0], acceleration[1]), tiltBound = Math.tan(25 * Math.PI / 180) * (model.gravityMps2 - acceleration[2])
58 if (horizontalAcceleration > Math.min(3, tiltBound)) {
59 const scale = Math.min(3, tiltBound) / horizontalAcceleration
60 acceleration[0] *= scale; acceleration[1] *= scale
61 }
62 for (let i = 0; i < 3; i++) velocityPid[i]!.track(acceleration[i]! - beforeLimit[i]!, dt)
63 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]
64 const vertical = model.gravityMps2 - acceleration[2]
65 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)))
66 const q = attitude.quaternionWxyz
67 const error = multiplyQuaternion([q[0], -q[1], -q[2], -q[3]], desired)
68 const rates = error.slice(1).map((n, i) => clampControl(8 * n * (error[0] < 0 ? -1 : 1), i === 2 ? 1.5 : 3)) as SensorVector
69 const torque = rates.map((n, i) => ratePid[i]!.step(n - input.angularRateRadS[i]!, input.angularRateRadS[i]!, dt)) as SensorVector
70 const desiredWrench = { thrustN: model.massKg * Math.hypot(...[acceleration[0], acceleration[1], vertical]), torqueNm: torque }
71 const allocation = allocateQuadX(desiredWrench, previous, dt)
72 for (let i = 0; i < 3; i++) ratePid[i]!.track(allocation.achieved.torqueNm[i]! - torque[i]!, dt)
73 previous = allocation.motorsN
74 return { kind: 'simulated-control' as const, atMs: lastNow, inhibited: false, reason: 'simulation-feedback', physicalFlightEnabled: false as const, boardOutputsInhibited: true as const, motorsN: [...previous] as Motors, limited: allocation.limited, desiredVelocityMps: velocity, desiredAccelerationMps2: acceleration, desiredRatesRadS: rates, desiredWrench, achievedWrench: allocation.achieved, integrals: { velocity: velocityPid.map(p => p.state().integral), rate: ratePid.map(p => p.state().integral) } }
75 },
76 }
9 return createSimulatedLiftController(options, { massKg: model.massKg, gravityMps2: model.gravityMps2, rateScale: [1, 1, 1], allocate: allocateQuadX })
7710}
78export type SimulatedControlOutput = ReturnType<ReturnType<typeof createSimulatedMultirotorController>['step']>
resources/functions/drone/controls/controller.ts resources/functions/drone/controls/lift-controller.tscopied+11-8
@@ -1,10 +1,9 @@
11import type { EstimatorSnapshot } from '../estimation/estimator'
22import type { SensorVector } from '../sensors/types'
3import type { Motors } from './model'
3import type { Motors, Wrench } from './model'
44import { multiplyQuaternion, quaternionFromTilt } from '../estimation/frames'
5import { allocateQuadX } from './allocation'
65import { createBoundedPid } from './pid'
7import { clampControl, multirotorModel as model, validVector } from './model'
6import { clampControl, validVector } from './model'
87export interface SimulatedControlInput {
98 kind: 'simulation'
109 enabled: boolean
@@ -14,11 +13,15 @@ export interface SimulatedControlInput {
1413 rateCapturedAtMs: number
1514 target: { positionM: SensorVector, yawRad: number, issuedAtMs: number }
1615}
17/** No vehicle adapter or IO. The initial heading relationship is a declared simulation assumption. */
18export function createSimulatedMultirotorController(options: { headingAlignment: 'initial-simulation-north' }) {
16interface LiftProfile { massKg: number, gravityMps2: number, rateScale: SensorVector, allocate(wrench: Wrench, previous: Motors, dtS: number): { motorsN: Motors, achieved: Wrench, limited: boolean } }
17/** Shared simulated lift cascade. Vehicle wrappers own mass, tuning and allocation; no vehicle IO. */
18export function createSimulatedLiftController(options: { headingAlignment: 'initial-simulation-north' }, profile: LiftProfile) {
1919 if (options.headingAlignment !== 'initial-simulation-north') throw new TypeError('An explicit simulation heading alignment is required')
20 const model = { massKg: profile.massKg, gravityMps2: profile.gravityMps2 }, allocate = profile.allocate
21 const rateScale = Array.isArray(profile.rateScale) ? [...profile.rateScale] : []
22 if (!Number.isFinite(model.massKg) || model.massKg <= 0 || !Number.isFinite(model.gravityMps2) || model.gravityMps2 <= 0 || !validVector(rateScale) || rateScale.some(n => n <= 0) || typeof allocate !== 'function') throw new RangeError('Invalid simulated lift profile')
2023 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 }))
21 const ratePid = [0, 1, 2].map(i => createBoundedPid({ kp: i === 2 ? 0.08 : 0.12, ki: i === 2 ? 0.04 : 0.08, kd: i === 2 ? 0.001 : 0.002, integralLimit: 0.12, outputLimit: i === 2 ? 0.25 : 0.6, derivativeTauS: 0.03, trackingGain: 4 }))
24 const ratePid = [0, 1, 2].map(i => createBoundedPid({ kp: (i === 2 ? 0.08 : 0.12) * rateScale[i]!, ki: (i === 2 ? 0.04 : 0.08) * rateScale[i]!, kd: (i === 2 ? 0.001 : 0.002) * rateScale[i]!, integralLimit: 0.12 * rateScale[i]!, outputLimit: (i === 2 ? 0.25 : 0.6) * rateScale[i]!, derivativeTauS: 0.03, trackingGain: 4 }))
2225 let previous: Motors = [0, 0, 0, 0], lastNow: number | null = null, reference: string | null = null, fault: string | null = null
2326 function clear() { for (const pid of [...velocityPid, ...ratePid]) pid.reset(); previous = [0, 0, 0, 0] }
2427 function inhibit(reason: string, latch = reference !== null) {
@@ -68,11 +71,11 @@ export function createSimulatedMultirotorController(options: { headingAlignment:
6871 const rates = error.slice(1).map((n, i) => clampControl(8 * n * (error[0] < 0 ? -1 : 1), i === 2 ? 1.5 : 3)) as SensorVector
6972 const torque = rates.map((n, i) => ratePid[i]!.step(n - input.angularRateRadS[i]!, input.angularRateRadS[i]!, dt)) as SensorVector
7073 const desiredWrench = { thrustN: model.massKg * Math.hypot(...[acceleration[0], acceleration[1], vertical]), torqueNm: torque }
71 const allocation = allocateQuadX(desiredWrench, previous, dt)
74 const allocation = allocate(desiredWrench, previous, dt)
7275 for (let i = 0; i < 3; i++) ratePid[i]!.track(allocation.achieved.torqueNm[i]! - torque[i]!, dt)
7376 previous = allocation.motorsN
7477 return { kind: 'simulated-control' as const, atMs: lastNow, inhibited: false, reason: 'simulation-feedback', physicalFlightEnabled: false as const, boardOutputsInhibited: true as const, motorsN: [...previous] as Motors, limited: allocation.limited, desiredVelocityMps: velocity, desiredAccelerationMps2: acceleration, desiredRatesRadS: rates, desiredWrench, achievedWrench: allocation.achieved, integrals: { velocity: velocityPid.map(p => p.state().integral), rate: ratePid.map(p => p.state().integral) } }
7578 },
7679 }
7780}
78export type SimulatedControlOutput = ReturnType<ReturnType<typeof createSimulatedMultirotorController>['step']>
81export type SimulatedControlOutput = ReturnType<ReturnType<typeof createSimulatedLiftController>['step']>
resources/functions/drone/vehicles/vtol/hover-controller.tsadded+16-0
@@ -0,0 +1,16 @@
1import { createSimulatedLiftController } from '../../controls/lift-controller'
2import { allocateVtolLift } from './allocation'
3import { vtolModel as model } from './plant'
4
5/** Fictional hover tuning scaled for the declared diagonal inertia. No transition or physical control. */
6export function createVtolHoverController(options: { headingAlignment: 'initial-simulation-north' }) {
7 return createSimulatedLiftController(options, {
8 massKg: model.massKg,
9 gravityMps2: model.gravityMps2,
10 rateScale: [10, 40 / 3, 12],
11 allocate(wrench) {
12 const result = allocateVtolLift(wrench)
13 return { motorsN: result.rotorsN, achieved: result.achieved, limited: result.limited }
14 },
15 })
16}
tests/tsconfig.runtime.jsonmodified+1-0
@@ -73,6 +73,7 @@
7373 "./unit/vehicle-runtime.test.ts",
7474 "./unit/air.test.ts",
7575 "./unit/vtol.test.ts",
76 "./unit/vtol-control.test.ts",
7677 "./unit/ground.test.ts",
7778 "./unit/surface.test.ts",
7879 "./unit/mission-workspace.test.ts",
tests/unit/vtol-control.test.tsadded+66-0
@@ -0,0 +1,66 @@
1import { expect, test } from 'bun:test'
2import type { SimulatedControlInput } from '../../resources/functions/drone/controls/controller'
3import { createEstimatorReplay } from '../../resources/functions/drone/estimation/replay'
4import { createVtolHoverController } from '../../resources/functions/drone/vehicles/vtol/hover-controller'
5import type { ImuMeasurement, SensorVector } from '../../resources/functions/drone/sensors/types'
6import { createVtolPlant } from '../../resources/functions/drone/vehicles/vtol/plant'
7import { createPhysicsSensorFeed } from '../../resources/functions/drone/controls/sensors'
8import { createEstimator } from '../../resources/functions/drone/estimation/estimator'
9import motion from '../fixtures/estimation/motion-v1.json'
10
11function hoverInput(nowMs = 1000): SimulatedControlInput {
12 const estimate = createEstimatorReplay(motion).advanceTo(1040).estimate
13 estimate.nowMs = nowMs
14 estimate.attitude!.quaternionWxyz = [1, 0, 0, 0]
15 estimate.attitude!.capturedAtMs = nowMs
16 estimate.localPosition!.positionM = [0, 0, -10]
17 estimate.localPosition!.velocityMps = [0, 0, 0]
18 estimate.localPosition!.capturedAtMs = nowMs
19 estimate.localPosition!.predictedToMs = nowMs
20 return { kind: 'simulation', enabled: true, nowMs, estimate, angularRateRadS: [0, 0, 0], rateCapturedAtMs: nowMs, target: { positionM: [0, 0, -10], yawRad: 0, issuedAtMs: nowMs } }
21}
22
23// Public estimator-to-control seam catches model mass and allocation, not closed-loop flight or transition qualification.
24test('owned VTOL hover feedback requests the 2.5 kg weight using its own rotor order and limits', () => {
25 const output = createVtolHoverController({ headingAlignment: 'initial-simulation-north' }).step(hoverInput())
26 expect(output.inhibited).toBe(false)
27 expect(output.desiredWrench!.thrustN).toBeCloseTo(24.516625, 10)
28 expect(output.desiredWrench!.torqueNm).toEqual([0, 0, 0])
29 for (const thrustN of output.motorsN) expect(thrustN).toBeCloseTo(6.12915625, 10)
30 expect(output.physicalFlightEnabled).toBe(false)
31 expect(output.boardOutputsInhibited).toBe(true)
32})
33
34// Public owned sensor/estimator/control/plant seams catch closed-loop response, not mission, authority or transition qualification.
35test('the VTOL body follows a hover position step using raw IMU and GNSS rather than plant pose feedback', () => {
36 const plant = createVtolPlant(), feed = createPhysicsSensorFeed(0)
37 const estimator = createEstimator({ origin: { latitudeDeg: 0, longitudeDeg: 0, ellipsoidHeightM: 100 }, gravityCorrection: 'initialization-only' })
38 const controller = createVtolHoverController({ headingAlignment: 'initial-simulation-north' })
39 let state = plant.snapshot(), command = [0, 0, 0, 0], activeTicks = 0, inhibitedTicks = 0, invalidCommands = 0, maxTiltRad = 0
40 let estimate = estimator.snapshot(0)
41 const targetAt = (nowMs: number): SensorVector => nowMs >= 2000 ? [2, 1, -1.5] : [0, 0, -1]
42 for (let nowMs = 0; nowMs <= 20000; nowMs += 4) {
43 if (nowMs > 600) state = plant.step([...command, 0, 0, 0, 0], 0.004, { windMps: [0, 0, 0], currentMps: [0, 0, 0] })
44 feed.capture(state, nowMs)
45 const sensors = feed.advanceTo(nowMs)
46 estimate = estimator.update(sensors)
47 if (nowMs >= 600 && nowMs % 20 === 0) {
48 const observation = sensors.sensors.find(s => s.kind === 'imu')?.observation
49 const imu = observation?.value as ImuMeasurement | undefined
50 const output = controller.step({ kind: 'simulation', enabled: true, nowMs, estimate, angularRateRadS: imu?.angularRateRadS ?? [0, 0, 0], rateCapturedAtMs: observation?.estimatedCaptureMs ?? -Infinity, target: { positionM: targetAt(nowMs), yawRad: 0, issuedAtMs: nowMs } })
51 command = [...output.motorsN]
52 if (output.inhibited) inhibitedTicks++
53 else activeTicks++
54 if (command.some(n => !Number.isFinite(n) || n < 0 || n > 12)) invalidCommands++
55 }
56 maxTiltRad = Math.max(maxTiltRad, Math.acos(Math.max(-1, Math.min(1, 1 - 2 * (state.quaternionWxyz[1] ** 2 + state.quaternionWxyz[2] ** 2)))))
57 }
58 expect(activeTicks).toBe(971)
59 expect(inhibitedTicks).toBe(0)
60 expect(invalidCommands).toBe(0)
61 expect(Math.hypot(...state.positionM.map((n, i) => n - targetAt(20000)[i]!))).toBeLessThan(0.2)
62 expect(maxTiltRad).toBeLessThan(0.44)
63 expect(state.onSurface).toBe(false)
64 expect(estimate.health.localPosition).toBe(true)
65 expect(estimate.health.absoluteHeading).toBe(false)
66})