ReviewOS

also looking at this

stacks/drone

feat: add bounded VTOL wind braking

#88
Merged chris wants to merge codex/owned-vtol-wind into main
13 files +447 -25
tests/unit/vtol-transition.test.tsmodified+60-1
Changes to tests/unit/vtol-transition.test.ts
@@ -2,7 +2,7 @@ import { expect, test } from 'bun:test'
22import { createVtolTransitionProfile } from '../../resources/functions/drone/vehicles/vtol/transition-profile'
33import { runVehicleSimulation } from '../../resources/functions/drone/vehicles/runtime'
44import { createVtolFlightController } from '../../resources/functions/drone/vehicles/vtol/flight-controller'
5import { multiplyQuaternion, quaternionEuler } from '../../resources/functions/drone/estimation/frames'
5import { multiplyQuaternion, quaternionEuler, quaternionFromTilt } from '../../resources/functions/drone/estimation/frames'
66import { replayVehicleMission, validateVehicleMission } from '../../resources/functions/drone/missions/vehicle'
77import type { VehicleControllerContext, VehiclePlantState } from '../../resources/functions/drone/vehicles/types'
88
@@ -333,3 +333,62 @@ test('back hover cannot confirm while a near-stationary body remains away from t
333333 }
334334 expect(qualifiedAt).toEqual([settled.nowMs + 520])
335335})
336
337function unachievableBrakeObservation(context: VehicleControllerContext) {
338 const input = structuredClone(context), pose = quaternionFromTilt(0, -0.4)
339 input.estimate.attitude!.quaternionWxyz = [...pose]
340 input.estimate.attitude!.eulerRad = [0, -0.4, 0]
341 input.measurementPose!.quaternionWxyz = [...pose]
342 input.controlRate!.angularRateRadS = [0, 0, 0]
343 Object.assign(input.measurement!.values, { airspeedMps: 13, differentialPressurePa: 103.5125, alphaRad: 0.1, betaRad: 0 })
344 return input
345}
346
347test('back braking refuses and latches when qualified airflow admits no pitch inside the declared bounds', () => {
348 const inputs = actualTransitionInputs(), index = inputs.findIndex(c => c.mission.transition?.taskId === 'back')
349 expect(index).toBeGreaterThan(0)
350 const controller = createVtolFlightController()
351 let warmed = true
352 for (const input of inputs.slice(0, index)) if (controller.step(input).inhibited) warmed = false
353 expect(warmed).toBe(true)
354 const refused = controller.step(unachievableBrakeObservation(inputs[index]!))
355 expect(refused.inhibited).toBe(true)
356 expect(refused.reason).toBe('vtol-braking-no-feasible-pitch')
357 expect(refused.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0])
358 expect(refused.transition).toBeUndefined()
359 expect(controller.step(inputs[index + 1]!).inhibited).toBe(true)
360})
361
362function advanceControlEpoch(context: VehicleControllerContext) {
363 const input = structuredClone(context)
364 input.nowMs += 20; input.estimate.nowMs += 20; input.mission.elapsedMs += 20
365 input.estimate.attitude!.capturedAtMs! += 20
366 input.estimate.localPosition!.capturedAtMs! += 20
367 input.estimate.localPosition!.predictedToMs += 20
368 input.controlRate!.capturedAtMs += 20
369 input.measurement!.capturedAtMs += 20; input.measurement!.receivedAtMs += 20; input.measurement!.sequence++
370 for (const key of ['capturedAtMs', 'positionCapturedAtMs', 'positionPredictedToMs', 'attitudeCapturedAtMs', 'attitudePredictedToMs', 'gyroCapturedAtMs'] as const) input.measurementPose![key] += 20
371 return input
372}
373
374test('a later front recovery cannot inherit a previous back leg rotary-capture latch', () => {
375 const inputs = actualTransitionInputs(), index = inputs.findIndex(c => c.mission.taskId === 'land')
376 expect(index).toBeGreaterThan(0)
377 const controller = createVtolFlightController()
378 let warmed = true
379 for (const input of inputs.slice(0, index)) if (controller.step(input).inhibited) warmed = false
380 expect(warmed).toBe(true)
381 const front = advanceControlEpoch(inputs[index - 1]!), anchor = [...front.estimate.localPosition!.positionM] as [number, number, number]
382 front.mission.taskId = 'front-second'; front.mission.taskKind = 'vtol-mode'; front.mission.targetM = anchor
383 front.mission.transition = { eventId: 'front-second', taskId: 'front-second', mode: 'cruise', headingRad: 0, issuedAtMs: front.nowMs, expiresAtMs: front.nowMs + 15000, positionM: anchor, reference: front.measurement!.reference }
384 expect(controller.step(front).inhibited).toBe(false)
385 const recovery = unachievableBrakeObservation(advanceControlEpoch(front))
386 recovery.mission.taskId = 'recover-second'
387 recovery.mission.recovery = { failedEventId: 'front-second', failedTaskId: 'front-second', hoverTaskId: 'recover-second', triggeredAtMs: recovery.nowMs, cause: 'operator-recover' }
388 recovery.mission.transition = { eventId: 'recover-second', taskId: 'recover-second', mode: 'hover', headingRad: 0, issuedAtMs: recovery.nowMs, expiresAtMs: recovery.nowMs + 45000, positionM: anchor, reference: recovery.measurement!.reference, supersedesEventId: 'front-second' }
389 const refused = controller.step(recovery)
390 expect(refused.inhibited).toBe(true)
391 expect(refused.reason).toBe('vtol-braking-no-feasible-pitch')
392 expect(refused.actuators).toEqual([0, 0, 0, 0, 0, 0, 0, 0])
393 expect(refused.transition).toBeUndefined()
394})