also looking at this
feat: add bounded VTOL wind braking
#88
13 files
+447
-25
| @@ -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 }) |