ReviewOS

also looking at this

stacks/drone

feat: add bounded VTOL wind braking

#88
Merged chris wants to merge codex/owned-vtol-wind into main
13 files +447 -25
resources/functions/drone/vehicles/vtol/flight-controller.tsmodified+25-7
Changes to resources/functions/drone/vehicles/vtol/flight-controller.ts
@@ -5,11 +5,11 @@ import { createVehicleMeasurementQualifier, vehicleReference } from '../measurem
55import { quaternionEuler, quaternionFromTilt, multiplyQuaternion, normalizeQuaternion } from '../../estimation/frames'
66import { rotateBodyToNed, rotateNedToBody } from '../../controls/physics'
77import { createBoundedPid } from '../../controls/pid'
8import { airAerodynamics, airModel } from '../air/plant'
8import { airModel } from '../air/plant'
99import { createVtolVehicleHoverController } from './controller'
1010import { numericPacket } from './packets'
1111import { allocateVtolLift } from './allocation'
12import { vtolModel } from './plant'
12import { vtolAerodynamics, vtolModel } from './plant'
1313
1414const clamp = (n: number, low: number, high: number) => Math.max(low, Math.min(high, n))
1515const wrap = (n: number) => Math.atan2(Math.sin(n), Math.cos(n))
@@ -89,7 +89,10 @@ export function createVtolFlightController(): VehicleController {
8989 }
9090 if (request?.eventId !== incoming.eventId && incoming.mode === 'hover' && hasWingControl) {
9191 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
9396 for (const pid of velocityPid) pid.reset()
9497 }
9598 }
@@ -131,15 +134,30 @@ export function createVtolFlightController(): VehicleController {
131134 let requiredUpN = vtolModel.massKg * (vtolModel.gravityMps2 - downAcceleration)
132135 const alphaTarget = clamp((requiredUpN / Math.max(1, pressureArea * Math.cos(bankTarget)) - 0.25) / 4.8, -0.08, 0.18)
133136 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])
135138 if (rotary) {
136139 backSlow ||= speed < 3
137140 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.
140143 const optimalAlpha = Math.max(0, (requiredUpN / Math.max(1, pressureArea) - 0.25) / (2 * 4.8))
141144 pitchTarget = clamp(gamma + optimalAlpha, -0.08, 0.3)
142145 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')
143161 }
144162 else {
145163 const goal = [...target!] as SensorVector
@@ -176,7 +194,7 @@ export function createVtolFlightController(): VehicleController {
176194 const pusher = rotary ? 0 : clamp((-zero.forceN[0] + vtolModel.massKg * clamp((13 - speed) * 1.2, -2.5, 2.5)) / vtolModel.maxPusherThrustN, 0, 1)
177195 const coefficients = [pressureArea * airModel.spanM * 0.12, pressureArea * airModel.chordM * 0.8, pressureArea * airModel.spanM * 0.08]
178196 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])
180198 const aeroNed = rotateBodyToNed(q!, aero.forceN)
181199 const collective = clamp((requiredUpN + aeroNed[2]) / Math.max(0.7, Math.cos(roll) * Math.cos(pitch)), 0, 48)
182200 const allocation = allocateVtolLift({ thrustN: collective, torqueNm: torque.map((n, i) => n - aero.torqueNm[i]!) as SensorVector })