also looking at this
feat: model VTOL electrical consumption and battery authority
#81
15 files
+605
-28
| @@ -1,12 +1,12 @@ | ||
| 1 | 1 | import type { EstimatorSnapshot } from '../estimation/estimator' |
| 2 | 2 | import type { ImuMeasurement, SensorVector } from '../sensors/types' |
| 3 | import type { VehicleControlOutput, VehicleControllerContext, VehicleMeasurement, VehicleMeasurementPose, VehicleMissionAction, VehicleProfile, VehicleVtolTransitionObservation } from './types' | |
| 3 | import type { VehicleBatteryObservation, VehicleControlOutput, VehicleControllerContext, VehicleMeasurement, VehicleMeasurementPose, VehicleMissionAction, VehicleProfile, VehicleVtolTransitionObservation } from './types' | |
| 4 | 4 | import { createPhysicsSensorFeed } from '../controls/sensors' |
| 5 | 5 | import { createEstimator } from '../estimation/estimator' |
| 6 | 6 | import { observeMission } from '../missions/engine' |
| 7 | 7 | import { createVehicleMissionEngine, validateVehicleMission } from '../missions/vehicle' |
| 8 | 8 | import { createVehicleAuthoritySupervisor, createVehicleOutputWatchdog } from '../authority/vehicle' |
| 9 | import { createVehicleMeasurementQualifier, predictVehicleMeasurementPose, vehicleReference } from './measurements' | |
| 9 | import { captureVehicleBatteryModel, createVehicleMeasurementQualifier, observeVehicleBattery, predictVehicleMeasurementPose, vehicleReference } from './measurements' | |
| 10 | 10 | type ScheduledMissionAction = { atMs: number, action: Exclude<VehicleMissionAction, 'start'> } |
| 11 | 11 | function captureMissionActions(input: unknown, maximumMs: number): ScheduledMissionAction[] { |
| 12 | 12 | function invalid(): never { throw new TypeError('Invalid vehicle mission action schedule') } |
| @@ -33,6 +33,7 @@ export function runVehicleSimulation(profileInput: VehicleProfile, options: { sc | ||
| 33 | 33 | 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') |
| 34 | 34 | const missionActions = captureMissionActions(options.missionActions, seconds * 1000) |
| 35 | 35 | const { plan, worstCaseMs, requiredWh } = validateVehicleMission(profileInput.mission) |
| 36 | const batteryModel = captureVehicleBatteryModel(profileInput.batteryModel, plan.power) | |
| 36 | 37 | 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') |
| 37 | 38 | const hasTransitions = plan.tasks.some(task => task.kind === 'vtol-mode') |
| 38 | 39 | let transitionObservation: VehicleVtolTransitionObservation | null = null |
| @@ -43,11 +44,12 @@ export function runVehicleSimulation(profileInput: VehicleProfile, options: { sc | ||
| 43 | 44 | const supervisor = createVehicleAuthoritySupervisor({ bootId: 55n, sessionId: 123n, vehicle: profileInput.vehicle, actuators, fence }) |
| 44 | 45 | const board = createVehicleOutputWatchdog({ bootId: 55n, sessionId: 123n, vehicle: profileInput.vehicle, actuators }) |
| 45 | 46 | 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) | |
| 47 | 48 | const observedSensors = { imu: 0, gnss: 0 }, sensorSequences = { imu: 0, gnss: 0 } |
| 48 | 49 | let truth = plant.snapshot(), mission = engine.snapshot() |
| 49 | 50 | let estimate: EstimatorSnapshot = estimator.snapshot(0) |
| 50 | 51 | let measurement: VehicleMeasurement | null = null, measurementPose: VehicleMeasurementPose | null = null |
| 52 | let batteryObservation: VehicleBatteryObservation | null = null | |
| 51 | 53 | const queue: { measurement: VehicleMeasurement, pose: VehicleMeasurementPose | null }[] = [] |
| 52 | 54 | const requested = new Set<string>() |
| 53 | 55 | 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 | ||
| 91 | 93 | try { qualified = qualifier.qualify(measurement, nowMs, reference) } |
| 92 | 94 | catch (error) { fault ??= error instanceof Error ? error.message : 'vehicle-sensor-envelope'; measurementPose = null } |
| 93 | 95 | measurement = qualified |
| 96 | if (batteryModel && qualified) { | |
| 97 | batteryObservation = observeVehicleBattery(qualified, batteryModel, nowMs) | |
| 98 | if (batteryObservation.cutoff) fault ??= 'modeled-battery-supply-cutoff' | |
| 99 | } | |
| 94 | 100 | if (navigation) estimate = navigation.update(baseEstimate, qualified, nowMs) |
| 95 | 101 | 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 | |
| 97 | 104 | const observation = { kind: 'simulation' as const, nowMs, estimate, |
| 98 | 105 | runtime: { bootId: b.bootId, capturedAtMs: nowMs, watchdogHealthy: b.fault === null }, |
| 99 | 106 | rc: { capturedAtMs: nowMs, connected: true, armSwitch: true, override: faults.rcOverride ?? false }, |
| 100 | 107 | 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 } } | |
| 102 | 109 | const send = (action: 'arm' | 'engage' | 'renew' | 'disarm', targetM = mission.targetM) => { |
| 103 | 110 | const s = supervisor.snapshot() |
| 104 | 111 | 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 | ||
| 143 | 150 | session.observeIndependent?.(structuredClone(truth), nowMs, [...boardState.actuators], false) |
| 144 | 151 | finalAtMs = nowMs |
| 145 | 152 | } |
| 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 } } : {}) } | |
| 147 | 154 | } |