also looking at this
feat: add bounded VTOL wind braking
#88
13 files
+447
-25
| @@ -12,15 +12,22 @@ export function validateAirActuators(a: number[]) { | ||
| 12 | 12 | if (!Array.isArray(a) || a.length !== 5 || !a.every(Number.isFinite) || a[0]! < 0 || a[0]! > 1 || a.slice(1, 4).some(n => Math.abs(n) > 0.4) || a[4]! < 0 || a[4]! > 1) throw new RangeError('Invalid fixed-wing throttle, aileron, elevator, rudder or wheel brake') |
| 13 | 13 | } |
| 14 | 14 | /** Quasi-steady lift/drag and rate-damped surface moments. Not firmware coefficients. */ |
| 15 | export function airAerodynamics(relativeBodyMps: SensorVector, rateRadS: SensorVector, actuators: number[]) { | |
| 15 | export function airAerodynamics(relativeBodyMps: SensorVector, rateRadS: SensorVector, actuators: number[], foldReverseFlow = false) { | |
| 16 | if (typeof foldReverseFlow !== 'boolean') throw new TypeError('Invalid aerodynamic incidence model') | |
| 16 | 17 | if (!validVector(relativeBodyMps) || !validVector(rateRadS)) throw new RangeError('Invalid aerodynamic vector') |
| 17 | 18 | validateAirActuators(actuators) |
| 18 | 19 | const [u, v, w] = relativeBodyMps |
| 19 | 20 | const speedMps = Math.hypot(u, v, w), alphaRad = Math.atan2(w, u), betaRad = Math.atan2(v, Math.hypot(u, w)) |
| 20 | 21 | const dynamicPressurePa = 0.5 * airModel.densityKgM3 * speedMps ** 2 |
| 21 | const linearLift = 0.25 + 4.8 * alphaRad | |
| 22 | // Explicit VTOL extension: fold the incidence and project stalled flow along | |
| 23 | // the chord. Preserve the forward pre-stall curve; fade to zero at broadside | |
| 24 | // and reverse the coefficient sign for flow over the trailing edge. | |
| 25 | // Raw airflow angles remain unchanged for capture/envelope checks. | |
| 26 | const coefficientAlphaRad = foldReverseFlow ? Math.atan2(w, Math.abs(u)) : alphaRad | |
| 27 | const chordDirection = foldReverseFlow ? Math.sign(u) * Math.min(1, Math.abs(Math.cos(alphaRad)) / Math.cos(airModel.stallAlphaRad)) : 1 | |
| 28 | const linearLift = 0.25 + 4.8 * coefficientAlphaRad | |
| 22 | 29 | // Bounded post-stall loss, retaining drag. No claim of dynamic stall or spin modeling. |
| 23 | const liftCoefficient = clamp(linearLift, -1.1, 1.4) * Math.max(0.15, 1 - Math.max(0, Math.abs(alphaRad) - airModel.stallAlphaRad) * 2.5) | |
| 30 | const liftCoefficient = chordDirection * clamp(linearLift, -1.1, 1.4) * Math.max(0.15, 1 - Math.max(0, Math.abs(coefficientAlphaRad) - airModel.stallAlphaRad) * 2.5) | |
| 24 | 31 | const dragCoefficient = 0.04 + 0.06 * liftCoefficient ** 2 + 0.8 * Math.sin(alphaRad) ** 2 + 0.2 * Math.sin(betaRad) ** 2 |
| 25 | 32 | const lift = dynamicPressurePa * airModel.areaM2 * liftCoefficient, drag = dynamicPressurePa * airModel.areaM2 * dragCoefficient |
| 26 | 33 | const ca = Math.cos(alphaRad), sa = Math.sin(alphaRad) |
| @@ -28,7 +35,7 @@ export function airAerodynamics(relativeBodyMps: SensorVector, rateRadS: SensorV | ||
| 28 | 35 | const rateScale = Math.max(1, speedMps) |
| 29 | 36 | const torqueNm: SensorVector = [ |
| 30 | 37 | dynamicPressurePa * airModel.areaM2 * airModel.spanM * (0.12 * actuators[1]! - 0.3 * rateRadS[0] * airModel.spanM / (2 * rateScale)), |
| 31 | dynamicPressurePa * airModel.areaM2 * airModel.chordM * (0.8 * actuators[2]! - 0.15 * alphaRad - 2 * rateRadS[1] * airModel.chordM / (2 * rateScale)), | |
| 38 | dynamicPressurePa * airModel.areaM2 * airModel.chordM * (0.8 * actuators[2]! - 0.15 * coefficientAlphaRad * chordDirection - 2 * rateRadS[1] * airModel.chordM / (2 * rateScale)), | |
| 32 | 39 | dynamicPressurePa * airModel.areaM2 * airModel.spanM * (0.08 * actuators[3]! + 0.12 * betaRad - 0.25 * rateRadS[2] * airModel.spanM / (2 * rateScale)), |
| 33 | 40 | ] |
| 34 | 41 | return { forceN, torqueNm, speedMps, alphaRad, betaRad, liftCoefficient, dragCoefficient, dynamicPressurePa, stalled: Math.abs(alphaRad) > airModel.stallAlphaRad } |