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/runtime.tsmodified+13-6
Changes to resources/functions/drone/vehicles/runtime.ts
@@ -1,12 +1,12 @@
11import type { EstimatorSnapshot } from '../estimation/estimator'
22import type { ImuMeasurement, SensorVector } from '../sensors/types'
3import type { VehicleControlOutput, VehicleControllerContext, VehicleMeasurement, VehicleMeasurementPose, VehicleMissionAction, VehicleProfile, VehicleVtolTransitionObservation } from './types'
3import type { VehicleBatteryObservation, VehicleControlOutput, VehicleControllerContext, VehicleMeasurement, VehicleMeasurementPose, VehicleMissionAction, VehicleProfile, VehicleVtolTransitionObservation } from './types'
44import { createPhysicsSensorFeed } from '../controls/sensors'
55import { createEstimator } from '../estimation/estimator'
66import { observeMission } from '../missions/engine'
77import { createVehicleMissionEngine, validateVehicleMission } from '../missions/vehicle'
88import { createVehicleAuthoritySupervisor, createVehicleOutputWatchdog } from '../authority/vehicle'
9import { createVehicleMeasurementQualifier, predictVehicleMeasurementPose, vehicleReference } from './measurements'
9import { captureVehicleBatteryModel, createVehicleMeasurementQualifier, observeVehicleBattery, predictVehicleMeasurementPose, vehicleReference } from './measurements'
1010type ScheduledMissionAction = { atMs: number, action: Exclude<VehicleMissionAction, 'start'> }
1111function captureMissionActions(input: unknown, maximumMs: number): ScheduledMissionAction[] {
1212 function invalid(): never { throw new TypeError('Invalid vehicle mission action schedule') }
@@ -33,6 +33,7 @@ export function runVehicleSimulation(profileInput: VehicleProfile, options: { sc
3333 if (!Number.isInteger(seconds) || seconds < 2 || seconds > 180 || scenario !== profileInput.scenario || !profileInput.scenarios.includes(scenario) || profileInput.evidence !== 'modeled-vehicle-profile' || profileInput.physicalFlightEnabled !== false) throw new RangeError('Invalid vehicle simulation profile, scenario or duration')
3434 const missionActions = captureMissionActions(options.missionActions, seconds * 1000)
3535 const { plan, worstCaseMs, requiredWh } = validateVehicleMission(profileInput.mission)
36 const batteryModel = captureVehicleBatteryModel(profileInput.batteryModel, plan.power)
3637 if (profileInput.vehicle !== plan.vehicle || plan.origin.latitudeDeg !== 0 || plan.origin.longitudeDeg !== 0 || plan.origin.ellipsoidHeightM !== 100) throw new TypeError('Vehicle fixture requires its declared 0,0,100 WGS84 origin')
3738 const hasTransitions = plan.tasks.some(task => task.kind === 'vtol-mode')
3839 let transitionObservation: VehicleVtolTransitionObservation | null = null
@@ -43,11 +44,12 @@ export function runVehicleSimulation(profileInput: VehicleProfile, options: { sc
4344 const supervisor = createVehicleAuthoritySupervisor({ bootId: 55n, sessionId: 123n, vehicle: profileInput.vehicle, actuators, fence })
4445 const board = createVehicleOutputWatchdog({ bootId: 55n, sessionId: 123n, vehicle: profileInput.vehicle, actuators })
4546 const feed = createPhysicsSensorFeed(0), estimator = createEstimator({ origin: plan.origin, gravityCorrection: 'initialization-only' })
46 const qualifier = createVehicleMeasurementQualifier(profileInput.vehicle)
47 const qualifier = createVehicleMeasurementQualifier(profileInput.vehicle, batteryModel ?? undefined)
4748 const observedSensors = { imu: 0, gnss: 0 }, sensorSequences = { imu: 0, gnss: 0 }
4849 let truth = plant.snapshot(), mission = engine.snapshot()
4950 let estimate: EstimatorSnapshot = estimator.snapshot(0)
5051 let measurement: VehicleMeasurement | null = null, measurementPose: VehicleMeasurementPose | null = null
52 let batteryObservation: VehicleBatteryObservation | null = null
5153 const queue: { measurement: VehicleMeasurement, pose: VehicleMeasurementPose | null }[] = []
5254 const requested = new Set<string>()
5355 let status: 'completed' | 'blocked' | 'host-lost' | 'duration-exhausted' = 'duration-exhausted', fault: string | null = null, hostLostAt: number | null = null
@@ -91,14 +93,19 @@ export function runVehicleSimulation(profileInput: VehicleProfile, options: { sc
9193 try { qualified = qualifier.qualify(measurement, nowMs, reference) }
9294 catch (error) { fault ??= error instanceof Error ? error.message : 'vehicle-sensor-envelope'; measurementPose = null }
9395 measurement = qualified
96 if (batteryModel && qualified) {
97 batteryObservation = observeVehicleBattery(qualified, batteryModel, nowMs)
98 if (batteryObservation.cutoff) fault ??= 'modeled-battery-supply-cutoff'
99 }
94100 if (navigation) estimate = navigation.update(baseEstimate, qualified, nowMs)
95101 if (nowMs < 600) continue
96 const b = board.snapshot(), batteryPercent = 100 - plan.power.maxW * Math.max(0, nowMs - 600) / 3600000 / plan.power.availableWh * 100
102 const b = board.snapshot()
103 const batteryPercent = batteryModel ? qualified && batteryObservation && !batteryObservation.cutoff ? batteryObservation.remainingPercentLowerBound : 0 : 100 - plan.power.maxW * Math.max(0, nowMs - 600) / 3600000 / plan.power.availableWh * 100
97104 const observation = { kind: 'simulation' as const, nowMs, estimate,
98105 runtime: { bootId: b.bootId, capturedAtMs: nowMs, watchdogHealthy: b.fault === null },
99106 rc: { capturedAtMs: nowMs, connected: true, armSwitch: true, override: faults.rcOverride ?? false },
100107 interlocks: { capturedAtMs: qualified?.capturedAtMs ?? nowMs - 1000, killActive: faults.kill ?? false, onGround: qualified?.values.onSurface === true, motorsStopped: qualified?.values.stopped === true },
101 battery: { capturedAtMs: nowMs, remainingPercent: batteryPercent }, link: { capturedAtMs: nowMs, connected: !faults.linkLost } }
108 battery: { capturedAtMs: batteryModel ? qualified?.capturedAtMs ?? nowMs - 1000 : nowMs, remainingPercent: batteryPercent }, link: { capturedAtMs: nowMs, connected: !faults.linkLost } }
102109 const send = (action: 'arm' | 'engage' | 'renew' | 'disarm', targetM = mission.targetM) => {
103110 const s = supervisor.snapshot()
104111 return supervisor.update(observation, { version: 1, sessionId: s.sessionId, bootId: s.bootId, epoch: s.epoch, sequence: s.sequence + 1, issuedAtMs: nowMs, action, mode: 'position', leaseMs: 100, target: { positionM: targetM ?? [0, 0, 0], yawRad: 0 } })
@@ -143,5 +150,5 @@ export function runVehicleSimulation(profileInput: VehicleProfile, options: { sc
143150 session.observeIndependent?.(structuredClone(truth), nowMs, [...boardState.actuators], false)
144151 finalAtMs = nowMs
145152 }
146 return { version: 1, evidence: 'owned-vehicle-physics-simulation' as const, physicalFlightEnabled: false as const, boardOutputsInhibited: true as const, vehicle: profileInput.vehicle, scenario, status, fault, navigationEvidence: { observedSensors, datum: estimate.localPosition?.altitudeDatum ?? null, quality: [...estimate.quality], localPositionHealthy: estimate.health.localPosition }, mission, trace: engine.trace(), path, transitions, budget: { worstCaseMs, requiredWh }, authority: supervisor.snapshot(), board: board.snapshot(), finalTruthPositionM: [...truth.positionM] as SensorVector }
153 return { version: 1, evidence: 'owned-vehicle-physics-simulation' as const, physicalFlightEnabled: false as const, boardOutputsInhibited: true as const, vehicle: profileInput.vehicle, scenario, status, fault, navigationEvidence: { observedSensors, datum: estimate.localPosition?.altitudeDatum ?? null, quality: [...estimate.quality], localPositionHealthy: estimate.health.localPosition }, mission, trace: engine.trace(), path, transitions, budget: { worstCaseMs, requiredWh }, authority: supervisor.snapshot(), board: board.snapshot(), finalTruthPositionM: [...truth.positionM] as SensorVector, ...(batteryModel ? { energy: { evidence: 'modeled-vehicle-battery-v1' as const, model: batteryModel, lastObservation: batteryObservation } } : {}) }
147154}