also looking at this
feat: add bounded VTOL wind braking
#88
13 files
+447
-25
| @@ -2,7 +2,7 @@ import { expect, test } from 'bun:test' | ||
| 2 | 2 | import { createVtolTransitionProfile } from '../../resources/functions/drone/vehicles/vtol/transition-profile' |
| 3 | 3 | import { runVehicleSimulation } from '../../resources/functions/drone/vehicles/runtime' |
| 4 | 4 | import { createVtolFlightController } from '../../resources/functions/drone/vehicles/vtol/flight-controller' |
| 5 | import { multiplyQuaternion, quaternionEuler } from '../../resources/functions/drone/estimation/frames' | |
| 5 | import { multiplyQuaternion, quaternionEuler, quaternionFromTilt } from '../../resources/functions/drone/estimation/frames' | |
| 6 | 6 | import { replayVehicleMission, validateVehicleMission } from '../../resources/functions/drone/missions/vehicle' |
| 7 | 7 | import type { VehicleControllerContext, VehiclePlantState } from '../../resources/functions/drone/vehicles/types' |
| 8 | 8 | |
| @@ -333,3 +333,62 @@ test('back hover cannot confirm while a near-stationary body remains away from t | ||
| 333 | 333 | } |
| 334 | 334 | expect(qualifiedAt).toEqual([settled.nowMs + 520]) |
| 335 | 335 | }) |
| 336 | ||
| 337 | function 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 | ||
| 347 | test('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 | ||
| 362 | function 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 | ||
| 374 | test('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 | }) | |