also looking at this
feat: model VTOL electrical consumption and battery authority
#81
15 files
+605
-28
| @@ -6,12 +6,18 @@ import { multiplyQuaternion, normalizeQuaternion } from '../../estimation/frames | ||
| 6 | 6 | import { rotateBodyToNed, rotateNedToBody } from '../../controls/physics' |
| 7 | 7 | import { airAerodynamics, airModel } from '../air/plant' |
| 8 | 8 | import { numericPacket } from './packets' |
| 9 | import { vtolElectricalLoad, vtolElectricalMaximumW, vtolElectricalModel, vtolElectricalSupply } from './power' | |
| 9 | 10 | |
| 10 | 11 | /** Fictional lift-and-cruise model. No measured hardware coefficients or flight qualification. */ |
| 11 | 12 | export const vtolModel = Object.freeze({ massKg: 2.5, surfaceFrictionCoefficient: 0.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 }) |
| 12 | 13 | |
| 13 | 14 | /** Independent lift rotors and the owned trainer wing/pusher share one rigid body. */ |
| 14 | export function createVtolPlant(initial: { positionM?: SensorVector, velocityMps?: SensorVector, quaternionWxyz?: Quaternion, rotorsN?: [number, number, number, number], pusherThrustN?: number } = {}): VehiclePlant { | |
| 15 | export function createVtolPlant(initial: { positionM?: SensorVector, velocityMps?: SensorVector, quaternionWxyz?: Quaternion, rotorsN?: [number, number, number, number], pusherThrustN?: number, electricalCapacityWh?: number, energyUsedWh?: number } = {}): VehiclePlant { | |
| 16 | const capacityWh = initial.electricalCapacityWh | |
| 17 | let energyUsedWh = initial.energyUsedWh === undefined ? 0 : initial.energyUsedWh | |
| 18 | if (capacityWh === undefined && initial.energyUsedWh !== undefined || capacityWh !== undefined && (!Number.isFinite(capacityWh) || capacityWh <= 0 || capacityWh > 10000 || !Number.isFinite(energyUsedWh) || energyUsedWh < 0 || energyUsedWh > capacityWh)) throw new RangeError('Invalid initial VTOL electrical state') | |
| 19 | const maximumSourceW = capacityWh === undefined ? 0 : vtolElectricalMaximumW(vtolModel.maxRotorThrustN, vtolModel.maxPusherThrustN) | |
| 20 | let batteryCutoff = capacityWh !== undefined && energyUsedWh === capacityWh, surfacesRad = [0, 0, 0] | |
| 15 | 21 | let positionM = numericPacket(initial.positionM === undefined ? [0, 0, 0] : initial.positionM, 3) as SensorVector |
| 16 | 22 | let rotorsN = numericPacket(initial.rotorsN === undefined ? [0, 0, 0, 0] : initial.rotorsN, 4) |
| 17 | 23 | let pusherThrustN = initial.pusherThrustN === undefined ? 0 : initial.pusherThrustN |
| @@ -20,18 +26,28 @@ export function createVtolPlant(initial: { positionM?: SensorVector, velocityMps | ||
| 20 | 26 | let quaternionWxyz = normalizeQuaternion(numericPacket(initial.quaternionWxyz === undefined ? [1, 0, 0, 0] : initial.quaternionWxyz, 4) as Quaternion), angularRateRadS: SensorVector = [0, 0, 0] |
| 21 | 27 | 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') |
| 22 | 28 | let onSurface = positionM[2] === 0 |
| 29 | function electricalDetails(): VehiclePlantState['details'] { | |
| 30 | if (capacityWh === undefined) return {} | |
| 31 | const remainingWh = capacityWh - energyUsedWh | |
| 32 | const loadW = remainingWh === 0 ? 0 : batteryCutoff ? vtolElectricalModel.auxiliaryW : vtolElectricalLoad(rotorsN, pusherThrustN, surfacesRad) | |
| 33 | const supply = vtolElectricalSupply(loadW, remainingWh / capacityWh) | |
| 34 | return { batteryCapacityWh: capacityWh, batteryEnergyUsedWh: energyUsedWh, batteryRemainingWh: remainingWh, batteryLoadPowerW: loadW, batterySourcePowerW: supply.sourceW, batteryCurrentA: supply.currentA, batteryTerminalV: supply.terminalV, batteryCutoff } | |
| 35 | } | |
| 23 | 36 | function snapshot(): VehiclePlantState { |
| 24 | 37 | const relative = rotateNedToBody(quaternionWxyz, velocityMps.map((n, i) => n - windMps[i]!) as SensorVector) |
| 25 | 38 | const airspeedMps = Math.hypot(...relative) |
| 26 | 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 } } | |
| 39 | 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, ...electricalDetails() } } | |
| 27 | 40 | } |
| 28 | 41 | return { |
| 29 | 42 | snapshot, |
| 30 | 43 | step(inputActuators, dtS, environment) { |
| 31 | const actuators = numericPacket(inputActuators, 8) | |
| 44 | let actuators = numericPacket(inputActuators, 8) | |
| 32 | 45 | const nextWindMps = numericPacket(environment.windMps, 3) as SensorVector, currentMps = numericPacket(environment.currentMps, 3) as SensorVector |
| 33 | 46 | 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') |
| 34 | 47 | if (!Number.isFinite(dtS) || dtS <= 0 || dtS > 0.01 || !validVector(nextWindMps) || !validVector(currentMps) || currentMps.some(n => n !== 0)) throw new RangeError('Invalid VTOL environment') |
| 48 | // Fund a whole worst-case step before allowing propulsion. A latched cutoff cannot rebound/rearm. | |
| 49 | const nextBatteryCutoff = batteryCutoff || capacityWh !== undefined && capacityWh - energyUsedWh < maximumSourceW * dtS / 3600 | |
| 50 | if (nextBatteryCutoff) actuators = [0, 0, 0, 0, 0, 0, 0, 0] | |
| 35 | 51 | const relative = rotateNedToBody(quaternionWxyz, velocityMps.map((n, i) => n - nextWindMps[i]!) as SensorVector) |
| 36 | 52 | const response = 1 - Math.exp(-dtS / vtolModel.rotorTimeConstantS), maximumChangeN = vtolModel.rotorSlewNPerS * dtS |
| 37 | 53 | const nextRotorsN = rotorsN.map((n, i) => n + Math.max(-maximumChangeN, Math.min(maximumChangeN, (actuators[i]! - n) * response))) |
| @@ -69,7 +85,15 @@ export function createVtolPlant(initial: { positionM?: SensorVector, velocityMps | ||
| 69 | 85 | const nextPositionM = positionM.map((n, i) => n + nextVelocityMps[i]! * dtS) as SensorVector |
| 70 | 86 | if (nextOnSurface) nextPositionM[2] = 0 |
| 71 | 87 | if (![...nextAccelerationMps2, ...nextVelocityMps, ...nextPositionM].every(Number.isFinite)) throw new RangeError('VTOL rotor model diverged') |
| 88 | let nextEnergyUsedWh = energyUsedWh | |
| 89 | if (capacityWh !== undefined) { | |
| 90 | const remainingWh = capacityWh - energyUsedWh | |
| 91 | const loadW = remainingWh === 0 ? 0 : nextBatteryCutoff ? vtolElectricalModel.auxiliaryW : vtolElectricalLoad(nextRotorsN, nextPusherThrustN, actuators.slice(5)) | |
| 92 | const sourceW = vtolElectricalSupply(loadW, remainingWh / capacityWh).sourceW | |
| 93 | nextEnergyUsedWh += Math.min(remainingWh, sourceW * dtS / 3600) | |
| 94 | } | |
| 72 | 95 | // Reject the entire integration before committing any retained state. |
| 96 | energyUsedWh = nextEnergyUsedWh; batteryCutoff = nextBatteryCutoff; surfacesRad = actuators.slice(5) | |
| 73 | 97 | windMps = nextWindMps |
| 74 | 98 | rotorsN = nextRotorsN |
| 75 | 99 | pusherThrustN = nextPusherThrustN |