ReviewOS

also looking at this

stacks/drone

feat: add shared-clock scoped vehicle simulation sessions

#98
Merged chris wants to merge codex/cargo-fleet-runtime into main
42 files +2764 -127
resources/functions/drone/vehicles/multirotor/profile.tsadded+74-0
Changes to resources/functions/drone/vehicles/multirotor/profile.ts
@@ -0,0 +1,74 @@
1import type { SensorVector } from '../../sensors/types'
2import type { VehicleMissionPlan, VehiclePlant, VehiclePlantState, VehicleProfile, VehicleSession } from '../types'
3import { createRigidBody } from '../../controls/physics'
4import { createSimulatedMultirotorController } from '../../controls/controller'
5import { multirotorModel } from '../../controls/model'
6import { vehicleReference } from '../measurements'
7
8export const multirotorScenarios = ['nominal', 'imu-loss', 'position-loss', 'vehicle-sensor-loss', 'rc-override', 'kill', 'host-loss', 'link-loss'] as const
9export type MultirotorScenario = typeof multirotorScenarios[number]
10
11/** The original fictional quad-X body, now using the shared acknowledged mission contract. */
12export function multirotorMission(): VehicleMissionPlan {
13 const spatial = { acceptanceM: .2, maxArrivalSpeedMps: .18, settleMs: 200, timeoutMs: 20000 }
14 return {
15 version: 2, vehicle: 'quadcopter', id: 'quad-shared-mission', frame: 'LOCAL_NED', altitude: 'ORIGIN_RELATIVE_DOWN', terrain: 'DECLARED_MODEL_SURFACE',
16 origin: { latitudeDeg: 0, longitudeDeg: 0, ellipsoidHeightM: 100 }, fence: { radiusM: 12, minDownM: -5, maxDownM: 0 }, rallyM: [0, 0, -1],
17 power: { availableWh: 40, maxW: 600, reservePercent: 30 }, start: 'start', tasks: [
18 { id: 'start', kind: 'start', next: 'depart' },
19 { id: 'depart', kind: 'takeoff', positionM: [0, 0, -1], ...spatial, next: 'visit' },
20 { id: 'visit', kind: 'waypoint', positionM: [2, 1, -1], ...spatial, next: 'home' },
21 { id: 'home', kind: 'return', ...spatial, next: 'land' },
22 { id: 'land', kind: 'land', positionM: [0, 0, 0], ...spatial },
23 ],
24 }
25}
26
27/** Both bare and carried-load bodies use the same quad plant interface. */
28export function createMultirotorPlant(body: ReturnType<typeof createRigidBody>): VehiclePlant {
29 function snapshot(): VehiclePlantState {
30 const state = body.snapshot()
31 return { positionM: [...state.positionM], velocityMps: [...state.velocityMps], accelerationMps2: [...state.accelerationMps2], quaternionWxyz: [...state.quaternionWxyz], angularRateRadS: [...state.angularRateRadS], onSurface: state.groundContact || state.positionM[2] >= -.1 && state.velocityMps[2] >= 0, details: Object.fromEntries(state.motorsN.map((thrust, i) => ['motor' + i + 'N', thrust])) }
32 }
33 return { snapshot, step(actuators, dtS, environment) { body.step(actuators as [number, number, number, number], dtS, { windMps: environment.windMps }); return snapshot() } }
34}
35
36function createSession(): VehicleSession {
37 let sequence = 0, plant: VehiclePlant | null = null
38 return {
39 createPlant: () => plant ??= createMultirotorPlant(createRigidBody({ positionM: [0, 0, -.1], groundDownM: -.1 })),
40 createController() {
41 const controller = createSimulatedMultirotorController({ headingAlignment: 'initial-simulation-north' })
42 let fault: string | null = null
43 return { step(context) {
44 const rate = context.controlRate
45 if (context.enabled && (!rate || rate.evidence !== 'modeled-qualified-imu' || rate.reference !== vehicleReference(context.estimate)
46 || !Array.isArray(rate.angularRateRadS) || rate.angularRateRadS.length !== 3 || ![0, 1, 2].every(i => Object.hasOwn(rate.angularRateRadS, i) && Number.isFinite(rate.angularRateRadS[i])))) fault ??= 'quad-rate-unqualified'
47 const output = controller.step({ kind: 'simulation', nowMs: context.nowMs, enabled: context.enabled && fault === null, estimate: context.estimate,
48 angularRateRadS: rate?.angularRateRadS ?? [0, 0, 0], rateCapturedAtMs: rate?.capturedAtMs ?? -Infinity,
49 target: { positionM: context.mission.targetM ?? [0, 0, 0], yawRad: 0, issuedAtMs: context.nowMs } })
50 const measurement = context.measurement
51 const contact = !output.inhibited && context.mission.taskKind === 'land' && measurement?.vehicle === 'quadcopter' && measurement.evidence === 'modeled-vehicle-sensors'
52 && measurement.reference === rate?.reference && measurement.capturedAtMs <= context.nowMs && context.nowMs - measurement.capturedAtMs <= 120 && measurement.receivedAtMs <= context.nowMs && measurement.values.onSurface === true
53 return { kind: 'simulated-vehicle-control', vehicle: 'quadcopter', atMs: context.nowMs, actuators: contact ? [0, 0, 0, 0] : [...output.motorsN], inhibited: fault !== null || output.inhibited, reason: fault ?? (contact ? 'quad-qualified-contact' : output.reason), physicalFlightEnabled: false, boardOutputsInhibited: true }
54 } }
55 },
56 renderMeasurement(state, atMs, reference) {
57 return { version: 1, evidence: 'modeled-vehicle-sensors', vehicle: 'quadcopter', sequence: ++sequence, reference, capturedAtMs: atMs, receivedAtMs: atMs + 40,
58 values: { onSurface: state.onSurface, stopped: [0, 1, 2, 3].every(i => Number(state.details['motor' + i + 'N']) < .01), calibration: 'fictional-quad-x-v1', clockUncertaintyMs: 1 } }
59 },
60 }
61}
62
63export function createMultirotorProfile(scenario: MultirotorScenario = 'nominal'): VehicleProfile {
64 if (!multirotorScenarios.includes(scenario)) throw new RangeError('Unknown multirotor scenario')
65 return { scenario, vehicle: 'quadcopter', name: 'Owned quad-X shared mission', scenarios: [...multirotorScenarios], evidence: 'modeled-vehicle-profile', physicalFlightEnabled: false,
66 actuators: [0, 1, 2, 3].map(i => ({ name: 'motor-' + i, unit: 'newton', min: 0, max: multirotorModel.maxMotorThrustN, neutral: 0 })),
67 mission: multirotorMission(), authorityFence: { radiusM: 12, minDownM: -5, maxDownM: 0 }, createSession,
68 faults(selectedScenario, atMs) {
69 if (selectedScenario !== scenario) throw new TypeError('Multirotor scenario changed after capture')
70 const active = atMs >= 5000
71 return { dropImu: active && scenario === 'imu-loss', dropPosition: active && scenario === 'position-loss', dropVehicleSensor: active && scenario === 'vehicle-sensor-loss', rcOverride: active && scenario === 'rc-override', kill: active && scenario === 'kill', hostLost: active && scenario === 'host-loss', linkLost: active && scenario === 'link-loss', windMps: [0, 0, 0] as SensorVector }
72 },
73 }
74}