ReviewOS

also looking at this

stacks/drone

feat: model VTOL electrical consumption and battery authority

#81
Merged chris wants to merge codex/owned-vtol-electrical-energy into main
15 files +605 -28
resources/functions/drone/vehicles/vtol/plant.tsmodified+27-3
Changes to resources/functions/drone/vehicles/vtol/plant.ts
@@ -6,12 +6,18 @@ import { multiplyQuaternion, normalizeQuaternion } from '../../estimation/frames
66import { rotateBodyToNed, rotateNedToBody } from '../../controls/physics'
77import { airAerodynamics, airModel } from '../air/plant'
88import { numericPacket } from './packets'
9import { vtolElectricalLoad, vtolElectricalMaximumW, vtolElectricalModel, vtolElectricalSupply } from './power'
910
1011/** Fictional lift-and-cruise model. No measured hardware coefficients or flight qualification. */
1112export 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 })
1213
1314/** Independent lift rotors and the owned trainer wing/pusher share one rigid body. */
14export function createVtolPlant(initial: { positionM?: SensorVector, velocityMps?: SensorVector, quaternionWxyz?: Quaternion, rotorsN?: [number, number, number, number], pusherThrustN?: number } = {}): VehiclePlant {
15export 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]
1521 let positionM = numericPacket(initial.positionM === undefined ? [0, 0, 0] : initial.positionM, 3) as SensorVector
1622 let rotorsN = numericPacket(initial.rotorsN === undefined ? [0, 0, 0, 0] : initial.rotorsN, 4)
1723 let pusherThrustN = initial.pusherThrustN === undefined ? 0 : initial.pusherThrustN
@@ -20,18 +26,28 @@ export function createVtolPlant(initial: { positionM?: SensorVector, velocityMps
2026 let quaternionWxyz = normalizeQuaternion(numericPacket(initial.quaternionWxyz === undefined ? [1, 0, 0, 0] : initial.quaternionWxyz, 4) as Quaternion), angularRateRadS: SensorVector = [0, 0, 0]
2127 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')
2228 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 }
2336 function snapshot(): VehiclePlantState {
2437 const relative = rotateNedToBody(quaternionWxyz, velocityMps.map((n, i) => n - windMps[i]!) as SensorVector)
2538 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() } }
2740 }
2841 return {
2942 snapshot,
3043 step(inputActuators, dtS, environment) {
31 const actuators = numericPacket(inputActuators, 8)
44 let actuators = numericPacket(inputActuators, 8)
3245 const nextWindMps = numericPacket(environment.windMps, 3) as SensorVector, currentMps = numericPacket(environment.currentMps, 3) as SensorVector
3346 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')
3447 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]
3551 const relative = rotateNedToBody(quaternionWxyz, velocityMps.map((n, i) => n - nextWindMps[i]!) as SensorVector)
3652 const response = 1 - Math.exp(-dtS / vtolModel.rotorTimeConstantS), maximumChangeN = vtolModel.rotorSlewNPerS * dtS
3753 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
6985 const nextPositionM = positionM.map((n, i) => n + nextVelocityMps[i]! * dtS) as SensorVector
7086 if (nextOnSurface) nextPositionM[2] = 0
7187 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 }
7295 // Reject the entire integration before committing any retained state.
96 energyUsedWh = nextEnergyUsedWh; batteryCutoff = nextBatteryCutoff; surfacesRad = actuators.slice(5)
7397 windMps = nextWindMps
7498 rotorsN = nextRotorsN
7599 pusherThrustN = nextPusherThrustN