also looking at this
feat: add shared-clock scoped vehicle simulation sessions
#98
42 files
+2764
-127
| @@ -0,0 +1,74 @@ | ||
| 1 | import type { SensorVector } from '../../sensors/types' | |
| 2 | import type { VehicleMissionPlan, VehiclePlant, VehiclePlantState, VehicleProfile, VehicleSession } from '../types' | |
| 3 | import { createRigidBody } from '../../controls/physics' | |
| 4 | import { createSimulatedMultirotorController } from '../../controls/controller' | |
| 5 | import { multirotorModel } from '../../controls/model' | |
| 6 | import { vehicleReference } from '../measurements' | |
| 7 | ||
| 8 | export const multirotorScenarios = ['nominal', 'imu-loss', 'position-loss', 'vehicle-sensor-loss', 'rc-override', 'kill', 'host-loss', 'link-loss'] as const | |
| 9 | export type MultirotorScenario = typeof multirotorScenarios[number] | |
| 10 | ||
| 11 | /** The original fictional quad-X body, now using the shared acknowledged mission contract. */ | |
| 12 | export 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. */ | |
| 28 | export 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 | ||
| 36 | function 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 | ||
| 63 | export 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 | } | |