From f9a61a086f0370b6aa8cecbd99fc8eef4e555fe4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Onur=20=C3=96zkan?= Date: Sat, 7 Nov 2026 18:12:11 +0300 Subject: [PATCH] feat(firmware): validate configuration and migrate sensor-driven flight APIs Validate configuration through its owning schemas and keep board declarations separate from shared F722 resource ownership. Bound USB transitions, isolate transport failures and retain restart-only activation. Migrate navigation and flight consumers with the sensor and schema contracts. Require explicit installation data and synchronous output authority; board motor outputs remain inhibited. Validation: assembled-prefix host tests, strict Clippy, platform/USB tests and both ARM release builds. Final-source coverage and scoped Miri/Kani evidence are retained separately from hardware qualification. --- docs/src/architecture.md | 208 +- docs/src/boards/geprc-f722-v2.md | 37 +- docs/src/boards/index.md | 16 +- docs/src/boards/kakute-f7-mini-v3.md | 35 +- docs/src/configuration.md | 327 +- docs/src/errors.md | 15 +- docs/src/getting-started.md | 4 +- docs/src/index.md | 6 +- docs/src/logs.md | 20 +- docs/src/mavlink.md | 91 +- docs/src/safety-boundary.md | 94 +- firmware/Cargo.lock | 60 +- firmware/Cargo.toml | 6 +- firmware/README.md | 12 +- firmware/boards/geprc-f722-v2/Cargo.toml | 4 +- firmware/boards/geprc-f722-v2/src/board.rs | 92 +- firmware/boards/kakute-f7-mini-v3/Cargo.toml | 4 +- .../boards/kakute-f7-mini-v3/src/board.rs | 128 +- .../libs/kaptan-config-derive/src/expand.rs | 54 +- .../libs/kaptan-config-derive/src/parse.rs | 51 +- .../kaptan-config-derive/tests/section.rs | 88 + .../tests/ui/f32_double_rounded_bounds.rs | 9 + .../tests/ui/f32_double_rounded_bounds.stderr | 5 + .../tests/ui/numeric_rules_on_bool.rs | 24 + .../tests/ui/numeric_rules_on_bool.stderr | 17 + .../tests/ui/numeric_rules_on_enum.rs | 31 + .../tests/ui/numeric_rules_on_enum.stderr | 17 + .../tests/ui/unsupported_scalar.rs | 15 + .../tests/ui/unsupported_scalar.stderr | 11 + firmware/libs/kaptan-config/src/artifact.rs | 366 +- firmware/libs/kaptan-config/src/error.rs | 3 + firmware/libs/kaptan-config/src/field.rs | 32 +- firmware/libs/kaptan-config/src/journal.rs | 209 +- firmware/libs/kaptan-config/src/lib.rs | 24 +- firmware/libs/kaptan-config/src/lifecycle.rs | 360 +- firmware/libs/kaptan-config/src/snapshot.rs | 40 +- firmware/libs/kaptan-config/src/storage.rs | 35 +- firmware/libs/kaptan-config/src/validation.rs | 16 +- .../libs/kaptan-config/tests/transactions.rs | 318 ++ .../libs/kaptan-control/src/kani_proofs.rs | 95 + firmware/libs/kaptan-control/src/lib.rs | 1122 ++++- firmware/libs/kaptan-data-channel/Cargo.toml | 4 + firmware/libs/kaptan-data-channel/src/lib.rs | 229 +- .../libs/kaptan-data-channel/src/mailbox.rs | 377 +- .../libs/kaptan-data-channel/src/tests.rs | 442 ++ .../kaptan-diagnostics-mavlink/src/lib.rs | 8 +- firmware/libs/kaptan-dma/src/lib.rs | 73 - .../libs/kaptan-drivers/src/icm42688/error.rs | 12 + .../libs/kaptan-drivers/src/icm42688/mod.rs | 551 ++- .../kaptan-drivers/src/icm42688/sample.rs | 70 +- .../kaptan-drivers/src/icm42688/service.rs | 130 +- firmware/libs/kaptan-drivers/src/lib.rs | 4 - .../kaptan-drivers/src/lidar_lite_v3/error.rs | 3 + .../kaptan-drivers/src/lidar_lite_v3/mod.rs | 335 +- .../src/lidar_lite_v3/service.rs | 159 +- .../libs/kaptan-drivers/src/mpu6000/error.rs | 12 + .../libs/kaptan-drivers/src/mpu6000/mod.rs | 652 ++- .../libs/kaptan-drivers/src/mpu6000/sample.rs | 98 +- .../kaptan-drivers/src/mpu6000/service.rs | 113 +- .../kaptan-drivers/src/mpu6000/test_bus.rs | 97 + firmware/libs/kaptan-drivers/src/mtf01/mod.rs | 186 +- firmware/libs/kaptan-file-mavlink/src/lib.rs | 2 +- .../libs/kaptan-file-mavlink/src/protocol.rs | 6 +- .../libs/kaptan-file-mavlink/src/service.rs | 452 +- .../kaptan-file-mavlink/tests/peer_lease.rs | 632 +++ firmware/libs/kaptan-flight-config/Cargo.toml | 8 +- .../libs/kaptan-flight-config/src/cbor.rs | 382 -- firmware/libs/kaptan-flight-config/src/lib.rs | 14 +- .../src/profile/active.rs | 278 +- .../src/profile/candidate.rs | 435 +- .../src/profile/control.rs | 133 +- .../src/profile/flight.rs | 88 + .../kaptan-flight-config/src/profile/ids.rs | 32 +- .../src/profile/mavlink_logging.rs | 16 - .../kaptan-flight-config/src/profile/mod.rs | 13 +- .../src/profile/safety.rs | 7 +- .../src/profile/timing.rs | 27 +- .../tests/control_domain.rs | 275 ++ .../kaptan-flight-config/tests/support/mod.rs | 96 + .../libs/kaptan-flight-runtime/Cargo.toml | 10 +- .../kaptan-flight-runtime/src/controller.rs | 214 + .../kaptan-flight-runtime/src/execution.rs | 2147 +++++++++ .../libs/kaptan-flight-runtime/src/lib.rs | 473 +- .../kaptan-flight-runtime/tests/lifecycle.rs | 586 +++ .../kaptan-flight-runtime/tests/navigation.rs | 164 + .../kaptan-flight-runtime/tests/replay.rs | 413 ++ .../tests/support/mod.rs | 173 + firmware/libs/kaptan-i2c/Cargo.toml | 3 + firmware/libs/kaptan-i2c/src/lib.rs | 38 +- firmware/libs/kaptan-math/src/kani_proofs.rs | 26 + firmware/libs/kaptan-math/src/lib.rs | 257 +- .../libs/kaptan-math/tests/so3_reference.rs | 122 + firmware/libs/kaptan-mavlink/Cargo.toml | 4 - firmware/libs/kaptan-mavlink/build.rs | 8 +- .../kaptan-mavlink/definitions/kaptan.xml | 4 +- .../definitions/publish/error.xml | 2 +- .../definitions/publish/heartbeat.xml | 2 +- .../definitions/publish/imu.xml | 4 +- .../definitions/publish/log.xml | 2 +- .../definitions/publish/range.xml | 2 +- firmware/libs/kaptan-mavlink/src/encoder.rs | 48 +- firmware/libs/kaptan-mavlink/src/lib.rs | 11 +- .../libs/kaptan-mavlink/src/transmitter.rs | 313 -- firmware/libs/kaptan-navigation/Cargo.toml | 7 +- firmware/libs/kaptan-navigation/src/eskf.rs | 4216 ----------------- firmware/libs/kaptan-navigation/src/filter.rs | 1140 +++++ firmware/libs/kaptan-navigation/src/flight.rs | 868 ++++ firmware/libs/kaptan-navigation/src/lib.rs | 48 +- .../libs/kaptan-navigation/src/navigation.rs | 2771 +++++++++++ .../libs/kaptan-navigation/src/observation.rs | 327 +- firmware/libs/kaptan-navigation/src/timing.rs | 335 ++ .../{kaptan-dma => kaptan-ports}/Cargo.toml | 5 +- firmware/libs/kaptan-ports/src/catalog.rs | 227 + firmware/libs/kaptan-ports/src/lib.rs | 334 ++ firmware/libs/kaptan-ports/src/schema.rs | 105 + firmware/libs/kaptan-ports/tests/ports.rs | 642 +++ .../libs/kaptan-safety/src/kani_proofs.rs | 155 + firmware/libs/kaptan-safety/src/lib.rs | 648 +-- firmware/libs/kaptan-safety/tests/commit.rs | 268 ++ firmware/libs/kaptan-sensor/src/imu.rs | 16 +- firmware/libs/kaptan-sensor/src/lib.rs | 4 - .../libs/kaptan-sensor/src/optical_flow.rs | 4 - firmware/libs/kaptan-sensor/src/range.rs | 7 +- firmware/libs/kaptan-spi/src/dma.rs | 329 ++ firmware/libs/kaptan-spi/src/lib.rs | 77 +- firmware/libs/kaptan-tx/src/lib.rs | 160 +- firmware/platforms/stm32f7/Cargo.toml | 14 +- .../stm32f7/examples/icm_dual_uart.rs | 32 + .../stm32f7/examples/mpu_dual_uart.rs | 38 + .../platforms/stm32f7/examples/support/mod.rs | 11 + firmware/platforms/stm32f7/src/f722/board.rs | 1478 +++--- firmware/platforms/stm32f7/src/f722/clock.rs | 34 +- firmware/platforms/stm32f7/src/f722/config.rs | 343 +- .../stm32f7/src/f722/data_channel.rs | 307 +- .../platforms/stm32f7/src/f722/firmware.rs | 226 +- .../platforms/stm32f7/src/f722/interrupts.rs | 191 +- firmware/platforms/stm32f7/src/f722/mod.rs | 85 +- .../stm32f7/src/f722/motor_inhibit.rs | 99 +- .../stm32f7/src/f722/notification.rs | 87 + .../stm32f7/src/f722/peripherals/clock.rs | 7 +- .../src/f722/peripherals/dma_buffer.rs | 35 - .../stm32f7/src/f722/peripherals/gpio.rs | 43 +- .../stm32f7/src/f722/peripherals/i2c1.rs | 663 ++- .../stm32f7/src/f722/peripherals/mod.rs | 22 +- .../stm32f7/src/f722/peripherals/spi1.rs | 175 +- .../stm32f7/src/f722/peripherals/spi3.rs | 176 +- .../stm32f7/src/f722/peripherals/spi_dma.rs | 53 + .../stm32f7/src/f722/peripherals/tim2.rs | 139 +- .../stm32f7/src/f722/peripherals/time.rs | 224 +- .../stm32f7/src/f722/peripherals/usart.rs | 252 +- .../peripherals/usart1_tx_dma2_stream7.rs | 222 - .../peripherals/usart2_tx_dma1_stream6.rs | 211 - .../stm32f7/src/f722/peripherals/usb.rs | 41 +- firmware/platforms/stm32f7/src/f722/ports.rs | 160 + firmware/platforms/stm32f7/src/f722/routes.rs | 107 +- .../platforms/stm32f7/src/f722/sensors.rs | 209 +- firmware/platforms/stm32f7/src/f722/serial.rs | 241 + firmware/platforms/stm32f7/src/f722/usb.rs | 103 +- firmware/platforms/stm32f7/src/lib.rs | 588 +-- .../stm32f7/tests/platform_safety_boundary.rs | 481 +- .../{ => support}/config_active_state.rs | 72 +- .../stm32f7/tests/support/config_files.rs | 775 +++ .../stm32f7/tests/support/io_latency.rs | 671 +++ .../stm32f7/tests/support/route_variants.rs | 239 + .../stm32f7/tests/support/timer_clock.rs | 332 ++ .../stm32f7/tests/support/usb_bus.rs | 407 ++ .../stm32f7/tests/support/usb_irq.rs | 391 ++ .../platforms/stm32f7/tests/usb_mavlink.rs | 345 +- firmware/tools/configurator/src/api.rs | 263 +- .../configurator/src/api_hosted_tests.rs | 416 ++ .../tools/configurator/src/config_client.rs | 1165 ++++- .../configurator/src/config_client_interop.rs | 648 +++ firmware/tools/configurator/src/lib.rs | 2 +- firmware/tools/configurator/src/main.rs | 4 +- .../tools/configurator/src/serial_io_tests.rs | 199 + .../tools/configurator/src/serial_worker.rs | 290 +- firmware/tools/configurator/src/state.rs | 967 +++- firmware/tools/configurator/web/app.js | 128 +- firmware/tools/configurator/web/index.html | 5 + firmware/tools/configurator/web/view.js | 102 + firmware/tools/configurator/web/view.test.cjs | 160 + firmware/vendor/synopsys-usb-otg/Cargo.toml | 26 + firmware/vendor/synopsys-usb-otg/KAPTAN.md | 13 + firmware/vendor/synopsys-usb-otg/LICENSE | 21 + firmware/vendor/synopsys-usb-otg/README.md | 43 + firmware/vendor/synopsys-usb-otg/src/bus.rs | 941 ++++ .../vendor/synopsys-usb-otg/src/endpoint.rs | 242 + .../synopsys-usb-otg/src/endpoint_memory.rs | 185 + firmware/vendor/synopsys-usb-otg/src/lib.rs | 90 + .../synopsys-usb-otg/src/ral/instances/mod.rs | 8 + ...fs_device_f401_f405_f407_f411_f427_f429.rs | 83 + ...fs_global_f401_f405_f407_f411_f427_f429.rs | 60 + .../src/ral/instances/otg_fs_host.rs | 83 + .../src/ral/instances/otg_hs_device.rs | 109 + .../src/ral/instances/otg_hs_global.rs | 64 + .../src/ral/instances/otg_hs_host.rs | 127 + .../src/ral/instances/otg_s_pwrclk.rs | 70 + .../vendor/synopsys-usb-otg/src/ral/mod.rs | 118 + .../src/ral/peripherals/mod.rs | 8 + .../src/ral/peripherals/otg_fs_device_v1.rs | 1950 ++++++++ .../src/ral/peripherals/otg_fs_global_v1.rs | 1774 +++++++ .../src/ral/peripherals/otg_fs_host.rs | 1370 ++++++ .../src/ral/peripherals/otg_fs_pwrclk.rs | 78 + .../src/ral/peripherals/otg_hs_device.rs | 2772 +++++++++++ .../src/ral/peripherals/otg_hs_global.rs | 2046 ++++++++ .../src/ral/peripherals/otg_hs_host.rs | 2035 ++++++++ .../synopsys-usb-otg/src/ral/register.rs | 610 +++ .../synopsys-usb-otg/src/ral/stm32f429/mod.rs | 7 + .../vendor/synopsys-usb-otg/src/target.rs | 92 + firmware/vendor/synopsys-usb-otg/src/tests.rs | 382 ++ .../vendor/synopsys-usb-otg/src/transition.rs | 41 + 211 files changed, 45657 insertions(+), 12740 deletions(-) create mode 100644 firmware/libs/kaptan-config-derive/tests/ui/f32_double_rounded_bounds.rs create mode 100644 firmware/libs/kaptan-config-derive/tests/ui/f32_double_rounded_bounds.stderr create mode 100644 firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_bool.rs create mode 100644 firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_bool.stderr create mode 100644 firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_enum.rs create mode 100644 firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_enum.stderr create mode 100644 firmware/libs/kaptan-config-derive/tests/ui/unsupported_scalar.rs create mode 100644 firmware/libs/kaptan-config-derive/tests/ui/unsupported_scalar.stderr create mode 100644 firmware/libs/kaptan-config/tests/transactions.rs create mode 100644 firmware/libs/kaptan-control/src/kani_proofs.rs create mode 100644 firmware/libs/kaptan-data-channel/src/tests.rs delete mode 100644 firmware/libs/kaptan-dma/src/lib.rs create mode 100644 firmware/libs/kaptan-drivers/src/mpu6000/test_bus.rs create mode 100644 firmware/libs/kaptan-file-mavlink/tests/peer_lease.rs delete mode 100644 firmware/libs/kaptan-flight-config/src/cbor.rs create mode 100644 firmware/libs/kaptan-flight-config/src/profile/flight.rs delete mode 100644 firmware/libs/kaptan-flight-config/src/profile/mavlink_logging.rs create mode 100644 firmware/libs/kaptan-flight-config/tests/control_domain.rs create mode 100644 firmware/libs/kaptan-flight-config/tests/support/mod.rs create mode 100644 firmware/libs/kaptan-flight-runtime/src/controller.rs create mode 100644 firmware/libs/kaptan-flight-runtime/src/execution.rs create mode 100644 firmware/libs/kaptan-flight-runtime/tests/lifecycle.rs create mode 100644 firmware/libs/kaptan-flight-runtime/tests/navigation.rs create mode 100644 firmware/libs/kaptan-flight-runtime/tests/replay.rs create mode 100644 firmware/libs/kaptan-flight-runtime/tests/support/mod.rs create mode 100644 firmware/libs/kaptan-math/src/kani_proofs.rs create mode 100644 firmware/libs/kaptan-math/tests/so3_reference.rs delete mode 100644 firmware/libs/kaptan-mavlink/src/transmitter.rs delete mode 100644 firmware/libs/kaptan-navigation/src/eskf.rs create mode 100644 firmware/libs/kaptan-navigation/src/filter.rs create mode 100644 firmware/libs/kaptan-navigation/src/flight.rs create mode 100644 firmware/libs/kaptan-navigation/src/navigation.rs create mode 100644 firmware/libs/kaptan-navigation/src/timing.rs rename firmware/libs/{kaptan-dma => kaptan-ports}/Cargo.toml (62%) create mode 100644 firmware/libs/kaptan-ports/src/catalog.rs create mode 100644 firmware/libs/kaptan-ports/src/lib.rs create mode 100644 firmware/libs/kaptan-ports/src/schema.rs create mode 100644 firmware/libs/kaptan-ports/tests/ports.rs create mode 100644 firmware/libs/kaptan-safety/src/kani_proofs.rs create mode 100644 firmware/libs/kaptan-safety/tests/commit.rs create mode 100644 firmware/libs/kaptan-spi/src/dma.rs create mode 100644 firmware/platforms/stm32f7/examples/icm_dual_uart.rs create mode 100644 firmware/platforms/stm32f7/examples/mpu_dual_uart.rs create mode 100644 firmware/platforms/stm32f7/examples/support/mod.rs create mode 100644 firmware/platforms/stm32f7/src/f722/notification.rs delete mode 100644 firmware/platforms/stm32f7/src/f722/peripherals/dma_buffer.rs create mode 100644 firmware/platforms/stm32f7/src/f722/peripherals/spi_dma.rs delete mode 100644 firmware/platforms/stm32f7/src/f722/peripherals/usart1_tx_dma2_stream7.rs delete mode 100644 firmware/platforms/stm32f7/src/f722/peripherals/usart2_tx_dma1_stream6.rs create mode 100644 firmware/platforms/stm32f7/src/f722/ports.rs create mode 100644 firmware/platforms/stm32f7/src/f722/serial.rs rename firmware/platforms/stm32f7/tests/{ => support}/config_active_state.rs (72%) create mode 100644 firmware/platforms/stm32f7/tests/support/config_files.rs create mode 100644 firmware/platforms/stm32f7/tests/support/io_latency.rs create mode 100644 firmware/platforms/stm32f7/tests/support/route_variants.rs create mode 100644 firmware/platforms/stm32f7/tests/support/timer_clock.rs create mode 100644 firmware/platforms/stm32f7/tests/support/usb_bus.rs create mode 100644 firmware/platforms/stm32f7/tests/support/usb_irq.rs create mode 100644 firmware/tools/configurator/src/api_hosted_tests.rs create mode 100644 firmware/tools/configurator/src/config_client_interop.rs create mode 100644 firmware/tools/configurator/src/serial_io_tests.rs create mode 100644 firmware/tools/configurator/web/view.js create mode 100644 firmware/tools/configurator/web/view.test.cjs create mode 100644 firmware/vendor/synopsys-usb-otg/Cargo.toml create mode 100644 firmware/vendor/synopsys-usb-otg/KAPTAN.md create mode 100644 firmware/vendor/synopsys-usb-otg/LICENSE create mode 100644 firmware/vendor/synopsys-usb-otg/README.md create mode 100644 firmware/vendor/synopsys-usb-otg/src/bus.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/endpoint.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/endpoint_memory.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/lib.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/ral/instances/mod.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_fs_device_f401_f405_f407_f411_f427_f429.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_fs_global_f401_f405_f407_f411_f427_f429.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_fs_host.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_hs_device.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_hs_global.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_hs_host.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_s_pwrclk.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/ral/mod.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/ral/peripherals/mod.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_fs_device_v1.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_fs_global_v1.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_fs_host.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_fs_pwrclk.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_hs_device.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_hs_global.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_hs_host.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/ral/register.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/ral/stm32f429/mod.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/target.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/tests.rs create mode 100644 firmware/vendor/synopsys-usb-otg/src/transition.rs diff --git a/docs/src/architecture.md b/docs/src/architecture.md index 1168d4f..e3eda9d 100644 --- a/docs/src/architecture.md +++ b/docs/src/architecture.md @@ -12,22 +12,72 @@ F722 platform / RTIC application ──► data channel ──► MAVLink teleme ├── physical sensor drivers └── startup motor-pin inhibition -portable libraries - math ─► navigation ─► control ─► safety +Portable flight libraries (not connected to physical actuators) + FlightEndpoint ──► Navigation / control / phases / safety + │ + └── caller-owned synchronous output sink ``` -Each F722 board contract pairs a board-owned `UserConfig` schema and explicit -`FACTORY_USER_CONFIG` with a board-owned `ImmutableConfig`. The immutable root -is one literal typestate manifest and the sole source of output inhibition, -physical routes, driver setup, sensor mount, bootstrap line rates, status LED, -image role, and service frequency. The user root contains only settings -consumed by that compiled driver graph. +Each F722 board combines `F722UserConfig` with an immutable hardware manifest. +The manifest selects the IMU route, driver settings, mount, serial inventory, +status LED, optional I²C sensor, motor pins and service frequency. User settings +select UART roles, baud and MAVLink settings. Motor pins are inhibited at startup. -The data channel owns rate selection, mailbox consumption, message encoding, -and bounded TX classification. Board code supplies physical routes, target -schema, and factory values; it does not receive generic output authority. +`F722ImmutableConfig` retains the IMU route type through initialization and +interrupt acknowledgement. The sealed `Mpu6000Spi1` and `Icm42688Spi3` routes +select IMU wiring and backend independently of serial inventory and LED wiring. -Configuration and runtime observability are separate planes: +Each board declares one `PortCatalog::DESCRIPTORS` array. Both +`Ports` and `SerialInventory::new(&Catalog::DESCRIPTORS)` use it for +stable UART/field identities, supported roles, baud allowlists and resource +claims. Validation produces a boot-only `PortPlan`. The same const `N` sizes +I/O and protocol storage: five UARTs on each current board. Absent UARTs +have no slots. A declared UART set to Disabled retains its slot but initializes +no peripheral or pins. USB is fixed normal MAVLink outside that inventory, +separate from ROM DFU. + +Each MAVLink `Link` has its own scheduler, encoder and receive framing, with a +separate transport queue. A shared collector snapshots sensor mailboxes for all +links. See [MAVLink](mavlink.md) for scheduling and transport behavior. + +`kaptan-flight-runtime::FlightEndpoint` runs Quad-X control and +`kaptan-navigation::Navigation` without allocation. Callers supply +observations, independently captured receipt/processing time, clock ticks and a +synchronous output sink. Transport and wire encoding stay outside the core. +The board images do not yet construct a flight endpoint. +See [Safety boundary](safety-boundary.md#shared-runtime-commit-contract) for +epochs, request sequencing and the output commit contract. + +`FlightEndpoint::supervise` checks actual time, cached health and phase/stream +deadlines without advancing navigation, integrating control or consuming a +control release. It can inhibit and publish outcomes, but cannot renew enabled +output. Callers must service the endpoint with actual time even when input +stops. The core owns control release and deadline decisions. + +Within `FlightEndpoint`, fusion, current prediction and initialization completion +run only when the 100-Hz control service is due. Early `tick` calls supervise +cached evidence without advancing navigation. Ingress admission and its +diagnostics remain immediate, but receiving or queuing fresh observations alone +does not renew capabilities. The actual control gap is checked before a due +navigation update can refresh evidence. + +`Navigation` initializes from a stationary window of IMU, fixed-reference local +position/velocity, magnetic, pressure and range observations. It derives attitude +and a fixed launch origin from sensors, with explicit calibration/reference +identities. Its shared 16-state error covariance covers position, velocity, +attitude error, gyro bias, accelerometer bias and barometric datum. Bounded +delayed fusion and current prediction preserve observation timing and capability +evidence. + +The navigation `sensor-snapshots` feature enables driver snapshot conversion. +The same flight estimator is available on host and embedded targets. + +Navigation owns the 25-m/s² free-flight acceleration bound checked by +complete-profile validation. See +[Configuration](configuration.md#flight-configuration) for required installation +data and the supported vehicle model. + +Configuration and diagnostics use separate paths: ```text FILE_CONTROL / FILE_DATA @@ -42,50 +92,97 @@ kaptan-file-mavlink ──► virtual FileProvider kaptan-log / kaptan-err-reportable │ ▼ -kaptan-diagnostics-mavlink ──► platform-owned encoder / DMA TX +kaptan-diagnostics-mavlink ──► per-link encoder / bounded TX ``` -The file dialect is intentionally Kaptan-native rather than standard MAVLink -FTP, `PARAM_*`, or a QGC adapter. Its two messages know only bounded paths, -bytes, positions, checksums, transfer ownership, and retries. The configuration -provider maps `/config/user.kcfg` to a canonical self-describing artifact. -KCFG carries generated metadata and a complete committed value set; the target -streams it without retaining a whole-file buffer. Saving imports and validates -one complete replacement candidate. The core distinguishes only immutable -boot-active values from the latest committed values. A save cannot mutate -controller or estimator state in flight. +The Kaptan-native file dialect owns bounded transfers and retries across links. +Its provider maps saved, active and factory-reset endpoints to streaming KCFG v3 +and the generic configuration lifecycle. + +`ConfigManager` owns boot-active values, known committed values or unknown durable +state, and the factory candidate. F722 supplies its UART-disabled setup fallback +through `recover_with`. Readable invalid settings remain inspectable for USB +repair. Saving or factory reset never changes active consumers until reboot. Target-owned stores use private persistence formats. F722 alternates compact -`KCSP` snapshots between two erase sectors, retaining only the old and -replacement records across the commit boundary. That format is not exposed as the public KCFG file. -Motor allocation/mixer topology is immutable compiled vehicle capability data, -never a scalar field or persistence member. F722 recovers operational settings -before constructing consumers. Its local USB CDC-ACM and directly wired UART -accept only exact-target, CRC-valid Kaptan file requests while physical outputs -remain inhibited. CRC is integrity-only; a shared or remote GCS still needs -production authentication, source binding, and persistent anti-replay state. -Disarmed activation and flight-consumer reconstruction remain separate reviewed -work. +`KCSP` snapshots between two erase sectors, retaining only the old and replacement +records across the commit boundary. That format is not exposed as KCFG. Motor +allocation/mixer topology remains compiled vehicle capability data. -The MAVLink dialect root directly includes one-message XML leaves under -`publish/` and `transfer/`. The `exchange/` contract remains empty until a -current consumer justifies a message. There are no category index XML files or -separate enum catalogs. The build watches the definitions tree and generates -one `generated::kaptan` module; leaves are never separately generated Rust -modules. `kaptan-log` itself has -no MAVLink, platform, time, allocation, or filesystem dependency; its sole -configuration coupling is compile-time enum metadata for `LevelFilter`. -`kaptan-err-reportable` independently owns explicitly assigned reportable-error -identity and a bounded nonblocking ingress; it has no logging, MAVLink, -platform, filesystem, or clock dependency. Event creation is independent from -sink filtering, allowing a future blackbox and MAVLink to select different -policies. The platform drains reportable errors before its bounded critical and -best-effort log queues and removes a pending record only after TX admission. -`LOG` carries one conventional level plus one bounded text field; `ERROR` -carries an explicitly assigned 16-bit ID and the bounded `file:line` of the -reporting point. Its message kind already identifies a reportable firmware -error. Kaptan-native heartbeat and sensor publications remain separate from -diagnostics. +## I/O ownership + +Sensor handoff uses owned `LatestMailbox` storage split into exactly one +non-cloneable producer and one non-cloneable consumer. Publishing replaces an +unread value, and taking removes it at most once. Each operation copies or takes +an `Option` inside the platform critical section. No callback, allocation, +retry or wait occurs inside that section. Storage stays borrowed while endpoints +are in use, and moving, dropping or forgetting an endpoint holds no lock. The +backend must exclude every participating context, using Cortex-M single-core exclusion on +F722 and the `critical-section/std` backend for host threads. + +UART TX and RX are interrupt-driven, without UART DMA. Each I/O owner retains +the peripheral, TX queue and a 512-byte RX buffer with a typed `Timestamp` per +byte captured at IRQ receipt. The worker takes at most 16 bytes per UART per +turn under a short lock. Protocol parsing, scheduling and persistence run +outside UART locks. Overflow or a hardware RX fault discards the damaged backlog +and resets the decoder before later bytes. These bounds do not establish a +measured latency guarantee. + +`kaptan-tx::TransmitQueue` owns bounded admission and one logically active frame. +Pending admission cannot replace that active frame. The queue itself is movable +and does not promise DMA address stability. UART byte writes and USB endpoint +writes borrow or copy its bytes synchronously. MAVLink owns encoding, while +platform I/O owners handle transmission. + +USB uses the repository-patched Synopsys driver. Runtime register waits allow +higher-priority interrupts and stop after 5 ms or 1,080,000 polls per wait. These +are fault limits, not measured execution times. A fault holds USB in peripheral +reset until restart and discards its queued data. Brief critical sections protect +USB ownership and endpoint buffers. Boot initialization still runs under RTIC's +startup interrupt mask. + +SPI1 and SPI3 DMA each use a once-claimed static pair of 32-byte TX/RX buffers. +`start_transfer` copies in, so hardware retains no caller slice. Completion +copies out only after checking stream disable, SPI quiescence, transfer counts +and faults. Failure to establish shutdown or drain RX quarantines the storage +and prevents reuse. Moving, dropping or forgetting the owner never releases +that claim. The F722 implementation relies on DMA-accessible SRAM, disabled +D-cache and device barriers. Physical SPI shutdown, timing and fault behavior +still require hardware validation. + +## Monotonic time + +TIM2 is the canonical F722 clock. The configured 108-MHz timer input and PSC=107 +produce a free-running 32-bit counter at 1 MHz with ARR=`u32::MAX`. All readers +and the CC1 service IRQ extend one shared `u64` microsecond epoch inside the same +critical section. A consumer may remain silent across wraps if other readers +or IRQ service keep extending that epoch. Combined observations must be strictly +less than 2^32 microseconds apart, about 71.58 minutes, including interrupt +blackouts and debugger halts. NMI and fault handlers must not access this clock. + +CC1 schedules the current 100-Hz service at epoch-aligned 10-ms deadlines. +Delayed service skips missed periods in constant work and emits at most one +notification per IRQ. Rearming uses two bounded attempts before pending a +recovery invocation. Callbacks and notifications run after clock exclusion ends. +TIM2 is not reset for each service tick, and no update IRQ or timer DMA is used. +Its IRQ is enabled only at the successful end of initialization. + +Ordinary WFI retains the timer clock. Deep sleep, STOP/Standby, clock-tree changes, +counter reset/regression and debugger halts spanning a full wrap are unsupported +and require platform reset. Uptime must leave one service period before `u64` +microsecond overflow. DWT is used only for the CPU-running SRAM flash-poll timeout +at 216 MHz, not sensor timestamps or service deadlines. These contracts do not +establish measured oscillator accuracy, IRQ latency, jitter or worst-case timing. + +## Diagnostics + +`kaptan-log` has no MAVLink, platform, time, allocation or filesystem dependency. +Its configuration coupling is compile-time enum metadata for `LevelFilter`. +`kaptan-err-reportable` independently owns explicit error identities and a bounded +nonblocking ingress, without logging, transport or clock dependencies. +Event creation is independent from sink filtering. The worker drains reportable +errors before logs and offers each diagnostic to all MAVLink links. Heartbeat +and sensor publications use separate scheduling. See [Configuration](configuration.md) for KCFG and the file-transfer contract, [Logs](logs.md) for the receiver contract, and [Errors](errors.md) @@ -96,7 +193,8 @@ for derived-error and failure-reporting scope. - A peripheral owner is local to the platform runtime. - Portable crates use typed units and validated values rather than raw wire or timer representations. -- A safety permit is epoch-bound and must be revalidated at the only nonzero - output boundary. -- A failed observation or numerical check does not partially commit estimator, - controller, allocator, or safety state. +- `SafetySupervisor::evaluate_and_commit` checks current evidence at the + synchronous enabled-output boundary. No transferable permit escapes it. +- Rejected numerical updates do not partially commit computational candidates. + Fault latches, inhibition and ingress replay watermarks follow their explicit + failure contracts rather than rolling back authority or replay history. diff --git a/docs/src/boards/geprc-f722-v2.md b/docs/src/boards/geprc-f722-v2.md index 5df814b..c83b696 100644 --- a/docs/src/boards/geprc-f722-v2.md +++ b/docs/src/boards/geprc-f722-v2.md @@ -1,19 +1,13 @@ # GEPRC GEP-F722-HD V2 -The GEPRC GEP-F722-HD V2 board declaration uses the ICM-42688-P on SPI3 and -MAVLink telemetry on USB CDC-ACM and USART2. It declares no vertical sensor stream. The -manufacturer's [GEP-F722-HD V2 manual](https://geprc.com/wp-content/uploads/2025/05/GEP-F722-HD-v2%E9%A3%9E%E6%8E%A7_%E4%BA%A7%E5%93%81%E8%AF%B4%E6%98%8E%E4%B9%A6-.pdf) -labels the external UART2 pads `T2` and `R2`. +The GEPRC GEP-F722-HD V2 uses the ICM-42688-P on SPI3. +The manufacturer's [GEP-F722-HD V2 manual](https://geprc.com/wp-content/uploads/2025/05/GEP-F722-HD-v2%E9%A3%9E%E6%8E%A7_%E4%BA%A7%E5%93%81%E8%AF%B4%E6%98%8E%E4%B9%A6-.pdf) +labels UART1 as `T1`/`R1` and UART2 as `T2`/`R2`. -Its board-local `UserConfig` factory selects endpoint `1/1`, IMU and -heartbeat publication rates, and an enabled MAVLink diagnostic sink with an -`Info` level filter. The web configurator may replace those -target-advertised values in the journal. Range -and optical-flow fields are absent because this image has no vertical-sensor -route. Its `ImmutableConfig` keeps the ICM-42688-P route, yaw-180° mount, -USART2 pins and baud, active-high PA13 status LED, and PB0/PB1/PB4/PB5 output -inhibition outside the writable schema. These selections appear together in -the board's immutable typestate literal. +The board declares its yaw-180° IMU mount, UART catalog, active-high PA13 LED +and no I²C range sensor. +Before fallible initialization, the platform holds the four documented motor +pins PB0/PB1/PB4/PB5 low. ## Build and package @@ -53,9 +47,10 @@ make firmware/tools/configurator Open `http://127.0.0.1:4861` and connect `/dev/serial/by-id/` or the discovered `/dev/ttyACM*` path from the Connection section. -## UART fallback +## UART connection -Connect a 3.3 V USB-UART adapter with crossed data lines and common ground: +First assign UART2 to `KaptanMavlink` over USB, save and restart. Then connect +a 3.3 V USB-UART adapter with crossed data lines and common ground: | Adapter | Flight controller | | --- | --- | @@ -65,11 +60,7 @@ Connect a 3.3 V USB-UART adapter with crossed data lines and common ground: Enter `/dev/serial/by-id/` in the same configurator's Connection section. -Open `http://127.0.0.1:4861`, Save the discovered fields, and manually -power-cycle the controller after Save completes. The flash journal is outside -the application image, so the exact range-limited DFU command above preserves -it. A mass erase or full-chip erase removes committed settings and restores -board factory defaults at the next boot. The image remains telemetry-only and -applies startup motor-pin inhibition. The unsigned local configuration path -assumes one directly connected client; disconnect every other transmitter from -`R2` while configuring over UART. +Use UART2's boot-active baud. If its role was changed or recovery disabled it, +connect over normal USB. Follow [save/reset/reload](../configuration.md#web-configurator) +before expecting new settings to apply. Disconnect other transmitters from `R2` +while configuring. Physical motor control is not yet implemented. diff --git a/docs/src/boards/index.md b/docs/src/boards/index.md index 116de2d..94b1f6c 100644 --- a/docs/src/boards/index.md +++ b/docs/src/boards/index.md @@ -3,10 +3,20 @@ Board pages describe supported hardware topology, build products, and safe verification. They do not declare a board flight-ready. -| Board | MCU family | Primary IMU | MAVLink links | +| Board | IMU route | Serial inventory | Factory UART roles | | --- | --- | --- | --- | -| [Kakute F7 Mini V3](kakute-f7-mini-v3.md) | STM32F722 | MPU-6000 | USB CDC-ACM, USART1 | -| [GEPRC GEP-F722-HD V2](geprc-f722-v2.md) | STM32F722 | ICM-42688-P | USB CDC-ACM, USART2 | +| [Kakute F7 Mini V3](kakute-f7-mini-v3.md) | `Mpu6000Spi1` | UART1, UART2, UART3, UART4, UART6 | All disabled | +| [GEPRC GEP-F722-HD V2](geprc-f722-v2.md) | `Icm42688Spi3` | UART1, UART2, UART3, UART4, UART6 | All disabled | + +USB CDC-ACM always uses Kaptan MAVLink and has no configurable role. ROM DFU is a separate flashing mode. UART roles and baud are +restart-only user settings. Both factories use 115200 baud, endpoint `1/1`, +20-Hz IMU, 2-Hz heartbeat, 20-Hz range/flow settings and the `Info` log filter. +Range/flow publications require an enabled sensor source. + +See [Configuration](../configuration.md) for role choices, supported baud values, +save/reset/reload and invalid-storage USB repair. The range-limited DFU workflows +on these pages preserve the journal outside the application image. Mass erase +removes saved settings and selects factory values at the next boot. All board images share the motor-pin inhibit boundary described in [Safety boundary](../safety-boundary.md). diff --git a/docs/src/boards/kakute-f7-mini-v3.md b/docs/src/boards/kakute-f7-mini-v3.md index b2056f8..12bdb44 100644 --- a/docs/src/boards/kakute-f7-mini-v3.md +++ b/docs/src/boards/kakute-f7-mini-v3.md @@ -1,20 +1,14 @@ # Kakute F7 Mini V3 -The Holybro Kakute F7 Mini V3 board declaration uses the MPU-6000 on SPI1, -MAVLink telemetry on USB CDC-ACM and USART1, and the MTF01 range/optical-flow -route on UART4. +The Holybro Kakute F7 Mini V3 uses the MPU-6000 on SPI1. The checked-in manufacturer datasheet is `firmware/boards/kakute-f7-mini-v3/datasheet.pdf`. -Its board-local `UserConfig` factory selects endpoint `1/1`, IMU, range, -optical-flow and heartbeat publication rates, and an enabled MAVLink diagnostic -sink with an `Info` level filter. The web configurator may replace those -target-advertised values in the journal. Its `ImmutableConfig` keeps the -MPU-6000 route, body-aligned -mount and concrete driver setup, USART1 pins and baud, MTF01 route and baud, -active-low PD2 status LED, and PB0/PB1/PB4/PB5 output inhibition outside the -writable schema. These selections appear together in the board's immutable -typestate literal. +The board maps MPU axes `(X, Y, Z)` to `(-Y, X, Z)`, matching the manufacturer's +`KAKUTEF7MINIV3` target, and supplies the driver settings. It declares the UART +catalog, active-low PD2 LED and no I²C range sensor. +Before fallible initialization, the platform holds all six documented +motor pins PB0/PB1/PB4/PB5/PB6/PB7 low. ## Build and package @@ -59,9 +53,10 @@ make firmware/tools/configurator Open `http://127.0.0.1:4861` and connect `/dev/serial/by-id/` or the discovered `/dev/ttyACM*` path from the Connection section. -## UART fallback +## UART connection -Use a 3.3 V USB-to-UART adapter with crossed data lines and common ground: +First assign UART1 to `KaptanMavlink` over USB, save and restart. Then connect +a 3.3 V USB-UART adapter with crossed data lines and common ground: | Adapter | Flight controller | | --- | --- | @@ -71,11 +66,7 @@ Use a 3.3 V USB-to-UART adapter with crossed data lines and common ground: Enter `/dev/serial/by-id/` in the same configurator's Connection section. -Open `http://127.0.0.1:4861`, edit the discovered fields, press Save, and -manually power-cycle the controller after Save completes. The flash journal is -outside the application image, so the exact range-limited DFU command above -preserves it. A mass erase or full-chip erase removes committed settings and -restores board factory defaults at the next boot. The image remains -telemetry-only and does not enable motor control. The unsigned local -configuration path assumes one directly connected client; disconnect every -other transmitter from `R1` while configuring over UART. +Use the UART's boot-active baud. If its role was changed or recovery disabled +UARTs, connect over normal USB. Follow [save/reset/reload](../configuration.md#web-configurator) +before expecting new settings to apply. Disconnect other transmitters from the +selected RX pad while configuring. Physical motor control is not yet implemented. diff --git a/docs/src/configuration.md b/docs/src/configuration.md index 40e5e08..2ba6b61 100644 --- a/docs/src/configuration.md +++ b/docs/src/configuration.md @@ -1,16 +1,56 @@ # Configuration -Kaptan has one generic configuration model in `kaptan-config`. Target schemas -declare typed fields and validation once; derive code generates the stable -field registry and schema fingerprint. Boards supply the sole erased-flash -factory values. The browser contains no board, driver, sensor, schema, or -parameter catalog. +`kaptan-config` owns typed fields, validation and the committed-versus-active +lifecycle. The F722 platform supplies the shared user schema and factory values. +Boards declare their UART catalog and immutable hardware context. The +configurator renders target metadata without a board or parameter catalog. -## Current deployment matrix +## Flight configuration -| Deployment | Configuration consumed today | Remote writes | +`kaptan-flight-config::CandidateProfile` owns the flight policy, gains, thrust +model, navigation noise densities and timing constraints. It is consumed by the +portable runtime and its tests. The board firmware uses its own board schema +and **KCFG v3** service. +Field IDs **6, 7 and 25** remain reserved. +The other field IDs retain their meanings and numbering. + +Construct the candidate's raw sections and supply a `NavigationInstallation` +through `VehicleCapabilities::quad_x`. It declares sources, calibration and +reference identities, range mounting and quantization, contact geometry, +magnetic field and local pressure/temperature. Complete validation produces +the immutable `VehicleProfile`. The current runtime supports Quad-X allocation +and four equal contact spheres on a body-frame rectangle. + +The current controller descriptors include fields 41..44: + +| ID | Key | Meaning | | --- | --- | --- | -| STM32F722 image | Board factory settings, or the newest valid flash-journal snapshot. | `/config/user.kcfg` over local USB CDC-ACM or the directly wired UART while motor pins remain inhibited. | +| 41 | `control.attitude_roll_pitch_gain_per_s` | Roll/pitch attitude gain in 1/s. | +| 42 | `control.attitude_yaw_gain_per_s` | Yaw attitude gain in 1/s. | +| 43 | `control.yaw_rate_pid.proportional` | Yaw rate proportional gain. | +| 44 | `control.yaw_rate_pid.integral` | Yaw rate integral gain. | + +Roll/pitch rate gains use the rate-PID fields. Derivative and contribution limits +are shared. + +Use the generated `kaptan-flight-config::SCHEMA_ID` from the source/build being +used. Schema identity describes the field contract; it does not qualify a +physical vehicle or identify calibrated values. + +The test fixture has a 10-ms control period, 20-ms control deadline, 250-ms +maximum command-admission TTL, 300-ms offboard stream timeout and one-second +fallback hold. Its configured output lifetime ceiling is 50 ms, further limited +by the 20-ms control deadline and live evidence. The navigation time-error +ceiling is 2000 us. These are library contracts, not measured F722 timing +guarantees or a qualified aircraft profile. + +Navigation owns `MAXIMUM_FREE_ACCELERATION_MPS2 = 25`. Complete-profile +validation checks maximum collective thrust `T_max <= m * (25 - g)` using `f64` +arithmetic, and requires thrust above weight. The bound applies to free flight, +not contact impulses. Mixer and installation remain compiled capabilities. + +The 1.2-kg/18-N fixture and its installation values exist only in test support. +They do not calibrate physical sensors or identify a propulsion system. ## Web configurator @@ -27,155 +67,230 @@ as `/dev/serial/by-id/` or `/dev/ttyACM0`. For UART, connect a 3.3 V USB-UART adapter to the board-specific RX/TX pads and enter its device path instead. -The web tool defaults to MAVLink source system `255` and baud `115200`. -`--source-system` and `--baud` remain optional command-line overrides. Every -simultaneously connected GCS must use a different nonzero system ID so transfer -ownership and retries cannot alias another client. +The tool defaults to MAVLink source system `255` and baud `115200`. +`--source-system` and `--baud` are command-line overrides. Match the adapter baud +to the UART's boot-active value. For example, from `firmware/`, launch +`cargo run -p kaptan-configurator --locked --offline -- --baud 230400`. +Peers sharing a link need distinct nonzero source identities, but those +identities do not authenticate them. -Open `http://127.0.0.1:4861`. On discovery, the configurator downloads the -complete `/config/user.kcfg` artifact, validates it, and renders the metadata -and values contained in that file. Save produces one complete edited KCFG -artifact and uploads it. The target validates the complete candidate before -atomically persisting it. The boot-active runtime does not change; manually -power-cycle the controller after a successful save when the UI reports that a -restart is required. +1. Connect and wait for discovery to read both saved and boot-active artifacts. + Each field shows **Boot-active**, **Saved** and **Draft** values. +2. Edit the draft and press **Save**. The target checks the downloaded baseline, + validates the complete replacement and persists it. After success, the tool + reloads both artifacts to establish the next save's baseline. +3. To replace saved settings with the target's factory values, press + **Reset saved configuration**. This is a persistence operation, not a reboot. + It also works when saved state is unknown, provided active metadata was read. +4. Save and factory reset leave active roles, baud, identity, rates and logging + unchanged. When restart is required, manually reset or power-cycle the board, + then reconnect and check the reloaded values. A UART may now use another baud + or no longer carry MAVLink. Normal USB remains the configuration route. -Saving erases one 128-KiB STM32F7 journal sector synchronously. Expect roughly -one second without telemetry or sensor servicing while the target flash -controller is busy; the host gives the final close a separate five-second -timeout. Keep the vehicle unpowered except for setup power and do not disconnect -it during Save. +A stale baseline returns `REQUEST_CONFLICT` without writing flash. The tool +discards that draft. Disconnect and reconnect to reload before editing again. +If saved state is unknown, ordinary saves are blocked. Reconnect to inspect it, +then use factory reset or reboot recovery as described below. -The artifact carries generated field IDs, native scalar kinds, display keys, -units, bounds, enum choices, factory values, committed values, schema identity, -and the derived restart-required state. A new board or field therefore needs no -configurator release. `u8`, `u16`, and enum-backed values retain their native -width rather than being widened at the configuration boundary. +A changed save or explicit factory reset synchronously erases a 128-KiB journal +sector. Flash activity can interrupt telemetry and sensor servicing. The host +allows five seconds for the final close, which is not a measured erase-time +guarantee. Keep setup power connected until the operation completes. An unchanged +ordinary replacement skips flash writes. -The Incoming MAVLink table groups live, CRC-valid Kaptan packets by source and -message ID and shows receive frequency and total count. Selecting a message -watches its latest decoded value. The separate Runtime logs view retains -individual `LOG` and `ERROR` events in receive order, with local receive time, -level, and readable text. +See [MAVLink](mavlink.md) and [Logs](logs.md) for the packet and runtime-log views. ## Board ownership -Each F722 board declares one `UserConfig` root for the settings consumed by its -compiled driver graph. The explicit `FACTORY_USER_CONFIG` literal is the sole -erased-flash value. There is no `Default` implementation and no platform-wide -union of board schemas. +Each F722 board selects `F722UserConfig`. The shared factory values +leave every configurable UART disabled. USB always carries Kaptan MAVLink and +has no configurable role. Board catalogs determine which UART fields exist. -Both current boards expose MAVLink identity, IMU and heartbeat publication -rates, and the MAVLink log level filter. Kakute also exposes range and -optical-flow rates because its immutable hardware manifest contains an MTF01. -Those fields are absent from GEPRC rather than present-but-disabled. +Both current boards expose MAVLink identity, publication rates, log filtering, +and `ports` fields from `kaptan-ports`. Range and optical-flow rates are present +on both boards because either can assign an inventoried UART to MTF01. Their +streams are enabled only when the selected sensor service supplies them. -The adjacent `IMMUTABLE_CONFIG` literal remains the sole source of facts that -cannot safely be edited as scalar configuration: +The adjacent `IMMUTABLE_CONFIG` manifest holds: -- output-inhibit and status-LED routes; -- IMU model, bus, interrupt route, mount, and driver setup; -- MAVLink UART and optional vertical-sensor routes and bootstrap baud rates; -- the fixed F722 USB CDC-ACM transport; -- service-timer frequency; and -- compiled image role. +- the IMU route, buses and interrupt wiring +- sensor mount and driver setup +- the available serial inventory, status LED and optional I²C range route +- service-timer frequency. -The F722 platform consumes stable semantic field IDs through the generic -`Config::field_value` boundary. It does not know a board section, Rust field, -or aggregate shape. Whole-candidate validation checks both generated local -rules and relationships such as publication budget and immutable hardware -capabilities. +Motor pins are inhibited at startup. The board images have no physical actuator +driver. + +UART roles and baud are writable settings applied only at boot. Each inventoried +UART supports `Disabled = 0`, `KaptanMavlink = 1` (`Role::Mavlink`), and +`Mtf01 = 2`. Disabled ports do not initialize their peripheral or pins. +One board-owned `PortCatalog::DESCRIPTORS` array supplies both +`Ports` metadata and `SerialInventory`. The same const `N` sizes the +boot plan, I/O slots and protocol slots. An absent UART has no slot or field. +A declared but disabled UART retains its bounded slot storage. + +| Keys | Role / baud field IDs | MCU TX / RX | Board inventory | +| --- | --- | --- | --- | +| `ports.uart1.role`, `ports.uart1.baud` | 201 / 202 | PA9 / PA10 | Both | +| `ports.uart2.role`, `ports.uart2.baud` | 203 / 204 | PA2 / PA3 | Both | +| `ports.uart3.role`, `ports.uart3.baud` | 205 / 206 | PB10 / PB11 | Both | +| `ports.uart4.role`, `ports.uart4.baud` | 207 / 208 | PA0 / PA1 | Both | +| `ports.uart6.role`, `ports.uart6.baud` | 211 / 212 | PC6 / PC7 | Both | + +Roles are native `u8` values and baud is `u32`. The F722 allowlist is exactly +**115200, 230400, 460800**. Intermediate values are rejected even though the +displayed bounds span that range. The current MTF01 driver requires **115200**. +At most one UART may use MTF01, and it cannot coexist with the immutable I²C +range source. These restrictions come from current platform capabilities. + +Whole-candidate validation checks resource conflicts and the enabled publication +schedule against the 100-Hz service budget. Each MAVLink UART must also carry +the scheduled framing-inclusive bytes at 8N1. This is an admission check, not a +delivery guarantee for unscheduled file or diagnostic traffic. USB has no role +or baud field. See [Boards](boards/index.md) for factory assignments. + +## Recovery and activation + +`ConfigManager::recover_with` owns recovery state. F722 supplies an explicit +setup policy that validates factory settings with all UART roles disabled when +stored settings cannot be activated. Normal USB MAVLink is initialized +independently of the UART plan. It is not ROM DFU, and its availability still +depends on successful clocks, USB hardware and application initialization. + +- Empty storage selects the board factory configuration. +- Readable but invalid committed settings remain available for inspection and + repair through USB. The boot-active export shows the UART-disabled fallback. +- Unreadable storage or a storage error leaves committed state **unknown**. + Active values and factory metadata remain readable. Ordinary writes are + blocked until reboot recovery establishes a baseline or factory reset succeeds. + +Fallback selection never rewrites storage. A persistence error also makes the +committed view unknown because the attempted write may already be durable. +Reboot may therefore activate that attempted value. Explicit factory reset +always writes the validated factory, even if values appear unchanged. A failed +reset leaves durability unknown. Neither operation changes the immutable +boot-active view or its boot diagnostic. + +## Configuration endpoints + +| Path | Access | Meaning | +| --- | --- | --- | +| `/config/user.kcfg` | Read/write | Read the known committed values. Write a complete replacement guarded by its expected base. | +| `/config/active.kcfg` | Read-only | Inspect the actual boot selection, including recovery fallback. | +| `/config/reset.kcfg` | Write-only | Accept only exact factory requested values. Ignore the expected base and force persistence. | + +Reset still requires a complete schema-matching KCFG artifact. The configurator +builds it from active metadata. There is no file command to reboot or activate +settings live. ## KCFG artifact KCFG is the public, canonical import/export representation. It is independent of MAVLink and of the private storage format. A file contains: -- `KCFG` magic and one format version; -- generated 16-byte schema identity and exact field count; -- one canonical self-describing record for every field; and +- `KCFG` magic, format version **3** and a restart flag +- generated 16-byte schema identity and exact field count +- one record per field with ID, native kind, requested/current value, factory + value and expected-base value at native width, plus field flags, bounds, + display key, unit and enum choices - a big-endian CRC-32C trailer covering the preceding artifact bytes. -The trailer uses network byte order deliberately. The generic file dialect -also calculates CRC-32C over every complete file byte; network-order storage -keeps that independent transfer checksum content-specific instead of producing -the fixed residue obtained by appending a reflected CRC in little-endian order. +Exports put the selected view's value in both current and expected-base slots. +Edits change only requested values. On ordinary save, the manager compares every +expected-base scalar's native kind and bits with current committed values before +whole-candidate validation, no-op detection or persistence. This is a full-value +compare-and-swap (CAS). Native-kind-and-bit equality also governs no-op and +restart comparisons. Positive and negative floating-point zero differ, while +identical NaN payload bits compare equal when the field's rules allow NaN. +It is not a CRC, generation counter or history check. A change away from and +back to the same values still matches. + +Field flag bits 0 and 1 indicate minimum and maximum bounds. Bit 2 is `finite` +and bit 3 is `nonzero`. Other bits are reserved zero. These constraints are +independent: `finite` rejects NaN and infinities, while `nonzero` rejects numeric +zero, including both floating-point signed zeros. `nonzero` alone does not imply +finite values. Any declared floating-point range rejects NaN as unordered even +without `finite`. Infinities are subject to the declared bounds and `finite` +rule. These flags are descriptor metadata and contribute to schema identity. + +The configurator exchanges floating-point values with the browser as strings. +Finite decimal strings preserve signed zero. Nonfinite values use exact IEEE +bit strings, `bits:0x` followed by 8 hexadecimal digits for `f32` or 16 for `f64`, +preserving NaN sign and payload. This representation does not bypass validation. + +CRC-32C detects accidental corruption. The big-endian trailer avoids the fixed +residue produced by a little-endian reflected CRC under the outer transfer CRC, +but neither checksum proves unique content or authenticates a peer. An imported file must match the target-generated schema, descriptor metadata, factory values, field order, native scalar kinds, and exact length. Unknown, duplicate, missing, malformed, noncanonical, out-of-range, and relationally -invalid values are rejected. The target stages only the typed candidate and a -small parser state; it never buffers the complete public file. Exports are also -generated by range directly from compiled metadata and current committed -values. One small export plan containing the exact length and checksums is -prepared when a read opens and reused for every requested range. +invalid requested values are rejected. The target stages typed requested and +expected-base candidates with a small parser state, not a whole-file buffer. +Exports are generated by range from metadata and the selected view, with one +length/checksum plan prepared at open. -Schema identity is required because a complete file can otherwise be validly -encoded while belonging to another firmware schema. It is generated from the -schema and is not a hand-maintained board ID. +The board-agnostic `decode_kcfg` API instead borrows a complete input artifact +and requires caller-owned 8192-byte field-ID scratch. Hosted callers can allocate +that bitmap directly on the heap. Decoding visits fields once, with work +O(input bytes + 8192), and the returned view borrows only the input, so scratch +is immediately reusable. This dynamic decoder is distinct from the bounded +typed streaming importer used by firmware. -The artifact header's restart bit describes the committed values in an export. -It is not an instruction accepted from an upload: after persistence the target -derives restart state by comparing committed values with immutable boot-active -values. +Schema identity detects schema mismatch. It is not a device identity, +authentication token or proof of all hardware compatibility. Complete-candidate +validation against the immutable context remains authoritative. + +The restart bit reports manager state, including on the active export. It is not +an upload instruction. The target derives it from committed versus boot-active +values, or sets it when durable state is unknown. ## Generic file dialect -Kaptan uses two custom messages rather than standard MAVLink FTP or a family of -configuration-specific requests: - -```text -FILE_CONTROL -FILE_DATA -``` +Kaptan uses custom `FILE_CONTROL` and `FILE_DATA` messages rather than standard +MAVLink FTP or configuration-specific messages. `FILE_CONTROL` opens, closes, aborts, and acknowledges a transfer. It carries a client-selected nonzero `u16` transfer ID, exact target, position, complete file size, CRC-32C, operation/status, and at most 64 bytes of path or -result detail. A source does not reuse an ID during the ten-second replay lease. -A new ID from the same source may supersede an open only before the first byte -has advanced; a progressed abandoned transfer remains pinned until lease expiry. +result detail. A peer does not reuse an ID during the ten-second replay lease. +A new ID from the same source and ingress may supersede an open only before the +first byte advances. A progressed abandoned transfer stays pinned until expiry. `FILE_DATA` uses the full MAVLink payload budget for a 246-byte chunk. The logical final-chunk length follows from the opened file size, so no redundant length field is sent with every data packet. An upload request carries status `UNSPECIFIED`; a read-data response carries status `OK`. All failures remain typed `FILE_CONTROL` responses. -The service owns one bounded stop-and-wait transfer. Chunks are sequential; -immediate retries are replay-safe, conflicting retries are rejected, and stale -transfer IDs cannot mutate a later transfer. A receive fault or bounded lease -expiry aborts staged provider state. The generic layer knows only paths, bytes, -positions, checksums, ownership, and retry state. It does not parse KCFG or -contain configuration policy. +One stop-and-wait transfer is shared across all links. Ownership includes the +claimed source system/component and the platform-supplied ingress identity. +Chunks are sequential. Immediate retries compare exact retained bytes, not just +checksums. Conflicting retries are rejected. Accepted owner activity renews the +ten-second monotonic lease, while rejected traffic does not. Expiry aborts staged +provider state. A receive discontinuity resets framing without itself releasing +the file lease. Responses use the requesting link. -The current provider exposes exactly `/config/user.kcfg`. A successful write -close has generic status `OK` and content beginning with the authoritative -seven-byte KCFG result prefix: restart flag, primary field, related field, and -validation-rule code. Its exact layout is `flags:u8 | primary_field:u16 LE | -related_field:u16 LE | validation_rule:u16 LE`; flag bit 0, when set, means a +For either writable KCFG endpoint, a successful close has generic status `OK` +and an authoritative seven-byte KCFG result prefix: +`flags:u8 | primary_field:u16 LE | +related_field:u16 LE | validation_rule:u16 LE`. Flag bit 0, when set, means a restart is required, and every other flag bit is reserved zero. The field -sentinel is `0xffff`; rule zero means no rule. +sentinel is `0xffff`. Rule zero means no rule. KCFG `CONTENT_REJECTED` responses use the same prefix and may append bounded UTF-8 detail. The configurator rejects a successful close with a missing, malformed, or failure-bearing prefix. The file layer passes these bytes through without interpreting them. -Local USB CDC-ACM and the directly wired UART are the F722 transports. CRC -validates framing integrity, not peer authenticity. Use one directly connected -client and disconnect other transmitters from the board RX line while using the -UART. A shared or remote GCS still requires signing, source/key binding, -persistent anti-replay state, and provisioning before this inbound service can -be exposed safely. +See [MAVLink](mavlink.md) for the local-link trust boundary. ## Private persistence -F722 uses private persistence separate from the public KCFG artifact. +The F722 private storage format is independent of the public KCFG artifact. ### F722 flash journal -F722 stores a compact private `KCSP` scalar snapshot inside a two-slot journal. -Each entry stores its stable field ID, scalar kind, and value at that kind's -native width; an `u8` is one byte rather than a widened integer slot. Each +F722 stores a `KCSP` scalar snapshot inside a two-slot journal. +Each entry stores its stable field ID, scalar kind and value at native width. Each journal record includes payload length, CRC-32C, commit marker, and a private monotonic sequence. Saving erases the inactive slot, programs and verifies the complete replacement, and writes the commit marker last. The previous valid diff --git a/docs/src/errors.md b/docs/src/errors.md index 206d4e4..68c80f3 100644 --- a/docs/src/errors.md +++ b/docs/src/errors.md @@ -78,14 +78,25 @@ different reportable error is already pending. `PersistConfigurationError` ID that distinguishes journal geometry, model, sequence, flash-controller, bounds, unlock, and verification failures. Replaying the cached close result does not repeat the report or - flash operation. + flash operation. Any persistence error makes committed state unknown and + blocks ordinary saves until reboot recovery or a successful explicit factory + reset. Boot-active values remain unchanged. +- A stale KCFG expected base returns `REQUEST_CONFLICT` before persistence. + It is a reload-required editing conflict, not a reportable firmware error. + Invalid boot configuration uses the UART-disabled USB setup path and retains + readable saved values for repair. See [Configuration](configuration.md). +- SPI completion errors never copy partial data into the caller's receive + buffer. If bounded shutdown cannot establish quiescence, the SPI route + quarantines its static DMA storage and refuses further transfers. This is + distinct from transient `Busy`. Physical SPI shutdown and fault behavior + remain hardware-validation work. - Reported `ReportableError` values reach the F722 MAVLink worker as a `ERROR`, subject to bounded transport admission. The payload is `{ error_id, raised_at[96] }`. `raised_at` is a zero-padded UTF-8 `file:line` prefix for the reporting point. Dynamic human-readable context belongs in `LOG`, while structured protocol failures remain in their owning response message. -- Control, navigation, and safety error types are derive-ready but +- Control, navigation and safety error types are derive-ready but their live owners are not yet attached to a MAVLink worker. F722 uses `panic-halt`, without live panic reporting or retained-record replay. diff --git a/docs/src/getting-started.md b/docs/src/getting-started.md index 02abb04..6af039a 100644 --- a/docs/src/getting-started.md +++ b/docs/src/getting-started.md @@ -30,8 +30,8 @@ target-specific commands in [Development and verification](development.md). | Path | Responsibility | | --- | --- | -| `firmware/boards/` | Board-owned user schemas, factory values, immutable hardware declarations, and image composition roots. | -| `firmware/platforms/stm32f7/` | Reviewed F722 RTIC platform, peripheral ownership, and shared linker layout. | +| `firmware/boards/` | Hardware declarations, UART catalogs and image roots. | +| `firmware/platforms/stm32f7/` | F722 RTIC platform, user schema, factory values, peripheral ownership and linker layout. | | `firmware/libs/` | Typed math, estimation, control, safety, protocol, and driver crates. | | `firmware/tools/configurator/` | Board-agnostic loopback web UI and serial bridge. | | `docs/` | This Markdown documentation site. | diff --git a/docs/src/index.md b/docs/src/index.md index a91fb35..16483eb 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -4,9 +4,9 @@ Kaptan is an embedded Rust flight-controller project. This manual is the single source of project documentation: board support, safety boundaries, development workflow, and design references live here as Markdown. -> **Status:** the checked-in firmware is telemetry-only. It does not expose -> physical motor authority, and this site does not make a physical-flight -> release claim. +> **Status:** The board images do not yet run the flight runtime or implement +> motor drivers. Motor pins are inhibited at startup. Physical flight has not +> been qualified. ## Index diff --git a/docs/src/logs.md b/docs/src/logs.md index 27cef95..74b7d73 100644 --- a/docs/src/logs.md +++ b/docs/src/logs.md @@ -60,11 +60,12 @@ Rust error variant. Renaming or reordering a variant does not change it. diagnostics. Protocol failures remain in their owning response; for example, a rejected KCFG upload returns a `FILE_CONTROL` failure and readable bounded detail. -Delivery is bounded and lossy under link pressure; heartbeat and safety +Delivery is bounded and lossy under link pressure. Heartbeat and sensor telemetry use separate paths. Logs have no publication rate setting and are -not periodic streams. A retained event leaves its MAVLink sink only once the -lower transport admits it; temporary queue pressure only defers that same -event. +not periodic streams. The worker offers each retained event to USB and every +MAVLink UART, then removes it once at least one transport admits it. Admission +does not prove receipt or delivery to every link. If none admits it, the event +remains pending. Opening USB does not take diagnostics away from UART. ## Filtering @@ -73,8 +74,11 @@ Trace (1), Debug (2), Info (3), Warn (4), and Error (5). `Info`, for example, accepts Info, Warn, and Error log events. `Off` suppresses `LOG` on MAVLink without suppressing reportable `ERROR` events or logs sent to another sink. There is no separate enable flag and no MAVLink-specific set of extra -runtime log levels. F722 uses the validated filter recovered at boot, falling -back to its board factory when the journal is empty or invalid. +runtime log levels. F722 uses the validated boot-active filter. Empty storage +uses factory values. Invalid storage selects the setup fallback, which also +uses the factory filter but disables UART roles. Saved changes and factory +reset do not alter the active filter until reboot. See +[configuration recovery](configuration.md#recovery-and-activation). Events retain the compact typed `Level` and sink configuration retains the compact typed `LevelFilter`. Schema metadata supplies the filter choices to the @@ -82,6 +86,4 @@ configurator. Only the MAVLink adapter converts the five event levels to the typed Kaptan wire enum. -The log ingress is transport-neutral and unfiltered. F722 images accept the -unsigned Kaptan file service on local USB CDC-ACM and their directly wired UART; that physical-link assumption does not extend to a shared -or remote MAVLink network. +The log ingress is transport-neutral and unfiltered. diff --git a/docs/src/mavlink.md b/docs/src/mavlink.md index bb9dfa1..7cd1110 100644 --- a/docs/src/mavlink.md +++ b/docs/src/mavlink.md @@ -9,48 +9,67 @@ dialect. Inbound processing is narrowly scoped: - Signed frames are discarded because no signature verifier or persistent replay state exists. - CRC-valid unsigned MAVLink 2 frames are retained only when their message ID - and extra CRC belong to the Kaptan dialect. Exact-target Kaptan file requests - are the only inbound records with a current firmware consumer. + and extra CRC belong to the Kaptan dialect. The board firmware consumes + exact-target Kaptan file requests. Shared or remote inbound authority is deliberately unsupported. Enabling it requires verified MAVLink 2 signing, configured source/target binding, persistent anti-replay state, and a provisioning/key-rotation policy as one reviewed feature. -Each F722 image exposes MAVLink on its board-selected UART and as a USB CDC-ACM -serial device. Linux normally names the application USB link `/dev/ttyACM*`; -this is distinct from the STM32 ROM DFU device used while the Boot pin is -asserted. UART and USB have independent receive framers. A host that opens the -CDC port and asserts DTR becomes the publication and diagnostic link; UART is -used while USB is closed. A file response is always returned on the link that -carried its request, so connecting USB cannot redirect a UART transaction. +Each F722 board image initializes USB CDC-ACM as a MAVLink link independently +of UART roles. Linux normally names it `/dev/ttyACM*`. It is separate from the +STM32 ROM DFU device used for flashing. Every UART assigned `KaptanMavlink` at +boot has its own link. USB needs an enumerated device and asserted DTR to carry +traffic. Opening or closing it does not redirect UART publications. + +Each link has independent scheduling, encoding, framing and bounded TX state. +Current links share the same boot-active endpoint identity and publication +rates, with one shared sensor snapshot per service turn. There are no per-link +rate settings. A stalled link does not consume another link's publication state, +though all links share the protocol worker and flash-operation stalls. File +responses always return on the requesting link. + The current descriptor uses the pid.codes shared development identity `1209:0001` plus the STM32 factory device ID as its serial number. Obtain and substitute a project-specific PID before distributing hardware as a product. -These local links are operational file transports, not authenticated +The normal telemetry links are operational file transports, not authenticated remote-control channels. Any transmitter physically connected to either link can issue file requests, so use a direct USB cable or USB-UART adapter and disconnect other transmitters while configuring. The platform retains its startup-configured low motor-output state throughout this workflow; no marker -type is treated as GPIO ownership or command authority. A networked GCS must -add verified signing, source binding, and durable replay protection before it -can receive authority. +type is treated as GPIO ownership or command authority. -The custom Kaptan file service admits one exact source principal for a bounded -stop-and-wait transfer. A receive fault or lease expiry aborts its staged -provider state. Completed control replies have a bounded replay path so a lost -close response does not repeat a flash operation. This serialization does not -authenticate its peer. +One file service serializes transfers across USB and all MAVLink UARTs. Its +owner is the claimed source system/component plus ingress identity, so the same +wire identity on another port cannot take over a transfer. Completed replies +have an exact bounded replay path so a lost close response does not repeat a +flash operation. Neither source identity, schema identity nor CRC authenticates +a peer. [Configuration](configuration.md#generic-file-dialect) defines the lease +and retry contract. -Telemetry transmission uses bounded frame classes. A critical frame may -replace pending best-effort traffic, but never a UART-DMA-owned or active USB -frame. UART completion retries preserve ordering, while USB advances a retained -frame across bounded CDC endpoint writes. DTR close, suspend, and -deconfiguration pause USB transmission and return publications to UART. Only a -bus reset discards USB transmission state. Closing DTR also discards partial -input. File-transfer and response-replay state expire under the protocol lease, -not on transport disconnect. +Telemetry transmission uses bounded frame classes. A critical frame may replace +pending best-effort traffic, but never an active frame. UART TX is IRQ-owned: +each interrupt writes at most one byte and retires at most one completed frame. +USB advances a retained frame through bounded CDC endpoint writes. DTR close, +suspend and deconfiguration pause USB TX. Only a bus reset discards its TX state. +Closing DTR discards partial input. UART receive faults reset framing. File and +replay state remain governed by the protocol lease rather than disconnects. + +USB receive backpressure masks only `GINTMSK.RXFLVLM` while application packet +bytes remain unconsumed. The final byte cursor and gate update share the USB +resource lock. The OTG FS NVIC vector stays enabled, as do IN/TX completion, +reset and suspend interrupt sources. This does not make control reception +independent: EP0 shares the RX FIFO and cannot bypass a blocked bulk packet at +its head. Once buffered bytes are consumed, RXFLVLM is restored. An incomplete +MAVLink frame alone does not keep RX masked. + +UART role and baud changes take effect after reboot. See +[Configuration](configuration.md) for allowed values and USB repair, and +[Architecture](architecture.md#io-ownership) for RX buffering and ownership. +Normal USB configuration recovery does not guarantee availability after an +arbitrary clock, USB hardware or initialization failure. The local web configurator exposes a bounded live view of CRC-valid inbound frames. It groups packets by source and message ID and measures receive @@ -71,35 +90,35 @@ one-message leaves grouped by interaction contract: | Directory | Contract | | --- | --- | | `publish/` | Zero-reply records for any interested receiver. | -| `exchange/` | Exact-endpoint requests with one terminal response; currently empty. | | `transfer/` | Exact-endpoint, bounded multi-message sessions. | The root includes every message leaf directly; there are no redundant category index files or separate enum catalogs. Placement is a wire-contract classification, while bindgen still generates one flat `generated::kaptan` module. Bare message names are scoped by that sole dialect and must not be -merged with another MAVLink dialect. `exchange/` gains its first XML only when -an exchange has a current consumer. +merged with another MAVLink dialect. `LOG` is an outbound human-readable four-field record. `ERROR` is the separate bounded channel for reportable firmware errors, carrying a stable explicit ID plus the `file:line` of its reporting point, and needs no redundant level field. `FILE_CONTROL` and `FILE_DATA` implement Kaptan's generic virtual-file transport without standard MAVLink FTP -compatibility. The current configurator reads and replaces -`/config/user.kcfg`. See +compatibility. The configurator reads saved and boot-active KCFG values and can +save replacements or reset saved settings to factory values. See [Configuration](configuration.md), [Logs](logs.md), and [Errors](errors.md) for the exact contracts. -The current scheduled publications are deliberately native rather than +The board firmware's scheduled publications are native rather than projections into a broader dialect: | Publication | Current payload | | --- | --- | | `HEARTBEAT` (51027) | The single current initialized/output-inhibited state. | -| `IMU` (51028) | Monotonic microseconds, body acceleration in mg, angular rate in mrad/s, and temperature in cdegC. | +| `IMU` (51028) | `u64` monotonic microseconds, `i16[3]` body acceleration in mg, `i32[3]` angular rate in mrad/s, and `i16` temperature in cdegC. | | `RANGE` (51029) | Monotonic microseconds and downward distance in mm. | | `OPTICAL_FLOW` (51030) | Monotonic microseconds, integer ground-relative velocity in mm/s, distance in mm, and quality. | -These records preserve the firmware's bounded native values. They do not -carry unused sensor IDs, repeated board topology, float projections, narrowed -timestamps, or clamped measurements. + +The IMU payload is 28 bytes with MAVLink extra CRC **22**. Wire field ordering +places time at byte 0, angular rate at byte 8, acceleration at byte 20 and +temperature at byte 26. The signed 32-bit gyro units preserve the full decoded +MPU6000 ±2000°/s range through mounting, mailbox handoff and encoding. diff --git a/docs/src/safety-boundary.md b/docs/src/safety-boundary.md index 0c9a592..30bc321 100644 --- a/docs/src/safety-boundary.md +++ b/docs/src/safety-boundary.md @@ -1,34 +1,88 @@ # Safety boundary -Every checked-in F722 image drives PB0, PB1, PB4, and PB5 low before normal -board initialization. This startup output inhibition configures each pin as a -low, push-pull GPIO output; it runs before clock-tree setup, board routing, -UART setup, or RTIC tasks. There is no marker or token standing in for the -register writes. +F722 startup drives PB0, PB1, PB4 and PB5 low, plus PB6 and PB7 on Kakute. +It configures them as push-pull GPIO outputs before clock-tree setup, board +routing, UART setup or RTIC tasks. This is a firmware startup property, not a hardware latch. Reset temporarily returns GPIO configuration to the MCU reset state, and the code cannot prove -external routing, pull resistors, ESC polarity, or ESC power behavior. Board -support must document those electrical assumptions; continuous inhibition +external routing, pull resistors, ESC polarity or ESC power behavior. Board +support must document those electrical assumptions. Continuous inhibition through reset requires board hardware that provides it. -There is no checked-in platform actuator route: - -- no DShot or PWM peripheral implementation is linked; -- no TIM3/TIM5 motor task or DMA output path exists; -- only CRC-valid Kaptan-dialect MAVLink input reaches the bounded virtual-file - service, which has no actuator path; -- no arm command is defined or admitted. - -The framing and file-transfer crates do not grant hardware access or actuator -authority. +The board images have no DShot, PWM or other actuator driver. The portable +flight runtime is not yet connected to physical motors. File requests grant +no actuator access. Run the structural guard after any platform or board change: ```sh -make firmware -- test -p platform-stm32f7 \ +cd firmware +cargo test -p platform-stm32f7 \ --test platform_safety_boundary --locked --offline ``` -This boundary is deliberately stronger than an application-level disarm flag: -it removes the physical actuator implementation from the platform image. +## Shared runtime commit contract + +`FlightEndpoint` uses `SafetySupervisor::evaluate_and_commit` to +evaluate current evidence and invoke an enabled-output callback at most once +while exclusively borrowing that supervisor. The callback synchronously retains +a core-owned `OutputRecord` in caller-owned storage. This establishes local +publication only. The receiving consumer must check the record's binding, +sequence and absolute validity at application time. A queued record or successful +USB write is not a reusable permission or proof of remote application. + +Runtime ingress carries a producer session and runtime generation, with separate +command-owner, arm, offboard and origin bindings. Commands and setpoints have +independent strictly increasing sequences. Retransmission retains its original +identity and bytes. Replays and wrong bindings cannot renew authority, and checked +authority sequences cannot wrap. A local `begin_epoch` is allowed only while +disarmed after successful inhibited publication. It resets navigation and request +watermarks while preserving the producer's output sequence and clock history. +These typed runtime identities do not authenticate a peer. + +Observation admission receives independently captured processing time in that +epoch. The measurement-time upper bound must not follow receipt time, receipt +must not follow processing time, and conservative measurement age must satisfy +the stream's freshness bound. +Receipt alone does not establish accepted or current evidence. Explicit clock +ticks advance navigation and supervision even when input stops. The current +control period is 10 ms on an absolute due grid with a 20-ms control deadline. +`supervise` checks time, cached health and deadlines without integrating control +or renewing enabled output. A deadline miss is a fault, not permission to replay +an old allocation indefinitely. + +Returned enabled-output errors latch a fault and trigger an inhibition attempt. +The sink must remain callable for that attempt. At terminal landing, the endpoint +clears arm authority and attempts inhibition before publishing terminal command +outcomes. Successful stop completion requires accepted inhibition; a rejected +inhibition leaves the command failed. Callers must reconcile committed outcomes after +both successful and failed core calls. This ordering does not prove physical +touchdown or motor stop. + +Output callbacks must not panic. Cleanup guarantees apply to +returned errors, not unwinding. A host that catches an unwind must discard the +affected endpoint and output owner rather than resume them or use an epoch reset +as recovery. Platform panic containment owns the physical +response. Abort or halt alone does not establish zero output. + +## Core failure policy + +`TakeoffHoverLand` starts only from `ArmedIdle` on a fresh command. Settled +takeoff starts a 30-second hold, followed by landing under the existing deadline +and landing reserve. It completes after accepted output inhibition. A superseding +command cancels the mission. The board integration prerequisites above still apply. + +With required navigation and output health present, offboard stream expiry +after 300 ms latches fallback, holds for the configured duration (at most one +second) from expiry, then lands. +Late packets do not resume the expired stream. Loss of required IMU/range or +other navigation capability, numerical failure, control-deadline failure or +output-publication failure can terminate and inhibit instead. Hold-then-land is +not promised without its required evidence. + +The core's configured output lifetime is at most 50 ms, further shortened by +capability evidence and the current 20-ms control deadline. A future physical +composition must enforce output validity at its actuator boundary and establish +its own hardware timing and failure behavior. The 1.2-kg/18-N test fixture is +uncalibrated. Component verification does not qualify a vehicle. diff --git a/firmware/Cargo.lock b/firmware/Cargo.lock index 34e3cfb..e5110d0 100644 --- a/firmware/Cargo.lock +++ b/firmware/Cargo.lock @@ -52,6 +52,7 @@ checksum = "8ec610d8f49840a5b376c69663b6369e71f4b34484b9b2eb29fb918d92516cb9" dependencies = [ "bare-metal", "bitfield", + "critical-section", "embedded-hal", "volatile-register", ] @@ -222,6 +223,7 @@ dependencies = [ name = "kaptan-data-channel" version = "0.1.0" dependencies = [ + "critical-section", "kaptan-err-reportable", "kaptan-mavlink", "kaptan-sensor", @@ -239,10 +241,6 @@ dependencies = [ "kaptan-mavlink", ] -[[package]] -name = "kaptan-dma" -version = "0.1.0" - [[package]] name = "kaptan-drivers" version = "0.1.0" @@ -283,9 +281,9 @@ name = "kaptan-flight-config" version = "0.1.0" dependencies = [ "kaptan-config", - "kaptan-err-reportable", - "kaptan-log", - "minicbor", + "kaptan-control", + "kaptan-math", + "kaptan-navigation", ] [[package]] @@ -293,15 +291,18 @@ name = "kaptan-flight-runtime" version = "0.1.0" dependencies = [ "kaptan-config", + "kaptan-control", "kaptan-flight-config", - "kaptan-log", + "kaptan-math", + "kaptan-navigation", + "kaptan-safety", + "kaptan-time", ] [[package]] name = "kaptan-geprc-f722-v2" version = "0.1.0" dependencies = [ - "kaptan-config", "platform-stm32f7", "rtic", ] @@ -309,12 +310,14 @@ dependencies = [ [[package]] name = "kaptan-i2c" version = "0.1.0" +dependencies = [ + "kaptan-time", +] [[package]] name = "kaptan-kakute-f7-mini-v3" version = "0.1.0" dependencies = [ - "kaptan-config", "platform-stm32f7", "rtic", ] @@ -338,11 +341,8 @@ name = "kaptan-mavlink" version = "0.1.0" dependencies = [ "bitflags", - "kaptan-dma", - "kaptan-err-reportable", "kaptan-sensor", "kaptan-time", - "kaptan-tx", "mavlink-bindgen", "mavlink-core", "num-derive", @@ -353,12 +353,18 @@ dependencies = [ name = "kaptan-navigation" version = "0.1.0" dependencies = [ - "kaptan-err-reportable", "kaptan-math", "kaptan-sensor", "kaptan-time", ] +[[package]] +name = "kaptan-ports" +version = "0.1.0" +dependencies = [ + "kaptan-config", +] + [[package]] name = "kaptan-safety" version = "0.1.0" @@ -424,26 +430,6 @@ version = "2.8.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "cf8baf1c55e62ffcace7a9f06f4bd9cd3f0c4beb022d3b367256b91b87513d98" -[[package]] -name = "minicbor" -version = "0.25.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c0452a60c1863c1f50b5f77cd295e8d2786849f35883f0b9e18e7e6e1b5691b0" -dependencies = [ - "minicbor-derive", -] - -[[package]] -name = "minicbor-derive" -version = "0.15.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bd2209fff77f705b00c737016a48e73733d7fbccb8b007194db148f03561fb70" -dependencies = [ - "proc-macro2", - "quote", - "syn 2.0.119", -] - [[package]] name = "nb" version = "0.1.3" @@ -491,16 +477,17 @@ version = "0.1.0" dependencies = [ "cortex-m", "cortex-m-rt", + "critical-section", "kaptan-config", "kaptan-data-channel", "kaptan-diagnostics-mavlink", - "kaptan-dma", "kaptan-drivers", "kaptan-err-reportable", "kaptan-file-mavlink", "kaptan-i2c", "kaptan-log", "kaptan-mavlink", + "kaptan-ports", "kaptan-sensor", "kaptan-spi", "kaptan-time", @@ -724,10 +711,9 @@ dependencies = [ [[package]] name = "synopsys-usb-otg" version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e948d523b316939545d8b21a48c27aef150ce25321b9f95ff7978647a806a6fe" dependencies = [ "cortex-m", + "critical-section", "embedded-hal", "usb-device", "vcell", diff --git a/firmware/Cargo.toml b/firmware/Cargo.toml index 83e3784..9af237b 100644 --- a/firmware/Cargo.toml +++ b/firmware/Cargo.toml @@ -8,7 +8,6 @@ members = [ "libs/kaptan-config-derive", "libs/kaptan-file-mavlink", "libs/kaptan-data-channel", - "libs/kaptan-dma", "libs/kaptan-drivers", "libs/kaptan-err-reportable", "libs/kaptan-err-reportable-macros", @@ -20,6 +19,7 @@ members = [ "libs/kaptan-log", "libs/kaptan-diagnostics-mavlink", "libs/kaptan-navigation", + "libs/kaptan-ports", "libs/kaptan-safety", "libs/kaptan-sensor", "libs/kaptan-spi", @@ -28,6 +28,7 @@ members = [ "platforms/stm32f7", "tools/configurator", ] +exclude = ["vendor/synopsys-usb-otg"] resolver = "2" [workspace.package] @@ -49,3 +50,6 @@ codegen-units = 1 lto = "fat" panic = "abort" strip = "symbols" + +[patch.crates-io] +synopsys-usb-otg = { path = "vendor/synopsys-usb-otg" } diff --git a/firmware/README.md b/firmware/README.md index 976d249..4d195ac 100644 --- a/firmware/README.md +++ b/firmware/README.md @@ -1,15 +1,13 @@ # Firmware -This directory is the Kaptan flight-controller firmware workspace. It owns the -Rust workspace manifest, board declarations, platform implementation, reusable -firmware crates and firmware scripts. +Kaptan flight-controller firmware workspace. ```text firmware/ -├── boards/ immutable board declarations and linker layouts +├── boards/ board hardware declarations and entry points ├── libs/ reusable no_std firmware crates -├── platforms/ hardware platform composition -├── tools/ firmware development tooling +├── platforms/ hardware composition, configuration and linker layouts +├── tools/ local configuration and telemetry tools └── scripts/ packaging, analysis, and coverage helpers ``` @@ -25,3 +23,5 @@ test suite. Build and package every supported board with `make firmware/boards`, or select one board by its directory path. Run `make firmware clippy` for Rust lints and `make firmware docs` for API documentation with warnings denied. +The board images do not yet run the flight runtime or implement motor drivers. +Motor pins are inhibited at startup. Physical flight has not been qualified. diff --git a/firmware/boards/geprc-f722-v2/Cargo.toml b/firmware/boards/geprc-f722-v2/Cargo.toml index e367945..4f9a3b0 100644 --- a/firmware/boards/geprc-f722-v2/Cargo.toml +++ b/firmware/boards/geprc-f722-v2/Cargo.toml @@ -10,10 +10,8 @@ build = "build.rs" workspace = true [dependencies] -kaptan-config = { path = "../../libs/kaptan-config" } platform-stm32f7 = { path = "../../platforms/stm32f7" } -# RTIC's attribute expansion emits absolute `rtic` support paths in the final -# board crate; the task graph and all RTIC source remain platform-owned. +# RTIC's macro emits absolute `rtic` paths in the board crate. rtic = { version = "2.3", default-features = false, features = [ "thumbv7-backend", ] } diff --git a/firmware/boards/geprc-f722-v2/src/board.rs b/firmware/boards/geprc-f722-v2/src/board.rs index 79d5d3a..6dbd09e 100644 --- a/firmware/boards/geprc-f722-v2/src/board.rs +++ b/firmware/boards/geprc-f722-v2/src/board.rs @@ -1,79 +1,35 @@ -//! Declarative support for the GEPRC TAKER F722 BLS 60A V2 stack. +//! Declarative support for the GEPRC GEP-F722-HD V2. +#![forbid(unsafe_code)] + +use platform_stm32f7::kaptan_ports::{PortCatalog, PortDescriptor}; use platform_stm32f7::{ - F7Board, F722Hardware, F722ImmutableConfig as ImmutableConfig, Icm42688, ImuMount, LevelFilter, - Mavlink, Pa13ActiveHigh, Pb0Pb1Pb4Pb5Low, Pc3, VerticalSensor, + F7Board, F722Hardware, F722ImmutableConfig, F722UserConfig, Icm42688Spi3, ImuMount, MotorPins, + SerialInventory, StatusLed, UART1, UART2, UART3, UART4, UART6, VerticalSensor, }; -/// User-writable MAVLink endpoint policy. -#[derive(Clone, kaptan_config::Config)] -struct MavlinkConfig { - #[config(id = 101, key = "mavlink.system_id", unit = "id", range(min = 1))] - system_id: u8, - #[config(id = 102, key = "mavlink.component_id", unit = "id", range(min = 1))] - component_id: u8, - #[config( - enum, - id = 105, - key = "mavlink.logging.level_filter", - unit = "log_level" - )] - log_level_filter: LevelFilter, -} - -/// User-writable telemetry publication policy. -#[derive(Clone, kaptan_config::Config)] -struct TelemetryConfig { - #[config( - id = 103, - key = "telemetry.imu_hz", - unit = "Hz", - range(min = 1, max = 100) - )] - imu_hz: u8, - #[config( - id = 104, - key = "telemetry.heartbeat_hz", - unit = "Hz", - range(min = 1, max = 10) - )] - heartbeat_hz: u8, -} - -/// Complete user-writable configuration advertised by this board. -#[derive(Clone, kaptan_config::Config)] -pub struct UserConfig { - mavlink: MavlinkConfig, - telemetry: TelemetryConfig, -} - -/// ICM-42688-P-based GEP-F722-HD V2 paired with the 60 A stack. pub struct GeprcF722V2; -impl F7Board for GeprcF722V2 { - type UserConfig = UserConfig; - type ImmutableConfig = ImmutableConfig; +impl PortCatalog<5> for GeprcF722V2 { + const DESCRIPTORS: [PortDescriptor; 5] = [UART1, UART2, UART3, UART4, UART6]; +} - const FACTORY_USER_CONFIG: UserConfig = UserConfig { - mavlink: MavlinkConfig { - system_id: 1, - component_id: 1, - log_level_filter: LevelFilter::Info, - }, - telemetry: TelemetryConfig { - imu_hz: 20, - heartbeat_hz: 2, - }, - }; - const IMMUTABLE_CONFIG: ImmutableConfig = ImmutableConfig { +impl F7Board for GeprcF722V2 { + type UserConfig = F722UserConfig; + type ImmutableConfig = F722ImmutableConfig; + + const FACTORY_USER_CONFIG: Self::UserConfig = Self::UserConfig::FACTORY; + const IMMUTABLE_CONFIG: Self::ImmutableConfig = F722ImmutableConfig { name: "GEPRC TAKER F722 V2 (ICM-42688-P)", service_hz: 100, - hardware: F722Hardware::new() - .output_inhibit(Pb0Pb1Pb4Pb5Low) - .imu(Icm42688::on_spi3().with_drdy(Pc3).mounted(ImuMount::Yaw180)) - .mavlink(Mavlink::on_usart2().baud(115_200)) - .vertical_sensor(VerticalSensor::none()) - .status_led(Pa13ActiveHigh) - .telemetry_only(), + hardware: F722Hardware::new( + Icm42688Spi3 { + mount: ImuMount::Yaw180, + }, + SerialInventory::new(&Self::DESCRIPTORS), + StatusLed::ActiveHighPa13, + VerticalSensor::None, + MotorPins::Four, + ), }; } diff --git a/firmware/boards/kakute-f7-mini-v3/Cargo.toml b/firmware/boards/kakute-f7-mini-v3/Cargo.toml index 70e5131..c71e656 100644 --- a/firmware/boards/kakute-f7-mini-v3/Cargo.toml +++ b/firmware/boards/kakute-f7-mini-v3/Cargo.toml @@ -9,10 +9,8 @@ version.workspace = true workspace = true [dependencies] -kaptan-config = { path = "../../libs/kaptan-config" } platform-stm32f7 = { path = "../../platforms/stm32f7" } -# RTIC's attribute expansion emits absolute `rtic` support paths in the final -# board crate; the task graph and all RTIC source remain platform-owned. +# RTIC's macro emits absolute `rtic` paths in the board crate. rtic = { version = "2.3", default-features = false, features = [ "thumbv7-backend", ] } diff --git a/firmware/boards/kakute-f7-mini-v3/src/board.rs b/firmware/boards/kakute-f7-mini-v3/src/board.rs index 27f1b2f..ba5905a 100644 --- a/firmware/boards/kakute-f7-mini-v3/src/board.rs +++ b/firmware/boards/kakute-f7-mini-v3/src/board.rs @@ -1,110 +1,42 @@ //! Declarative Kakute F7 Mini V3 board description. -//! -//! This module deliberately contains no MCU registers, RTIC concepts, unsafe -//! code, interrupt names, DMA handling, or task scheduling. Those concerns -//! belong to the STM32F7 platform crate. +#![forbid(unsafe_code)] + +use platform_stm32f7::kaptan_ports::{PortCatalog, PortDescriptor}; use platform_stm32f7::{ - AccelerometerRange, DigitalLowPassFilter, F7Board, F722Hardware, - F722ImmutableConfig as ImmutableConfig, GyroscopeRange, ImuMount, LevelFilter, Mavlink, - Mpu6000, Mpu6000Config, Pa4, Pb0Pb1Pb4Pb5Low, Pd2ActiveLow, VerticalSensor, + AccelerometerRange, DigitalLowPassFilter, F7Board, F722Hardware, F722ImmutableConfig, + F722UserConfig, GyroscopeRange, ImuMount, MotorPins, Mpu6000Config, Mpu6000Spi1, + SerialInventory, StatusLed, UART1, UART2, UART3, UART4, UART6, VerticalSensor, }; -/// User-writable MAVLink endpoint policy. -#[derive(Clone, kaptan_config::Config)] -struct MavlinkConfig { - #[config(id = 101, key = "mavlink.system_id", unit = "id", range(min = 1))] - system_id: u8, - #[config(id = 102, key = "mavlink.component_id", unit = "id", range(min = 1))] - component_id: u8, - #[config( - enum, - id = 105, - key = "mavlink.logging.level_filter", - unit = "log_level" - )] - log_level_filter: LevelFilter, -} - -/// User-writable telemetry publication policy. -#[derive(Clone, kaptan_config::Config)] -struct TelemetryConfig { - #[config( - id = 103, - key = "telemetry.imu_hz", - unit = "Hz", - range(min = 1, max = 100) - )] - imu_hz: u8, - #[config( - id = 104, - key = "telemetry.heartbeat_hz", - unit = "Hz", - range(min = 1, max = 10) - )] - heartbeat_hz: u8, - #[config( - id = 106, - key = "telemetry.range_hz", - unit = "Hz", - range(min = 1, max = 100) - )] - range_hz: u8, - #[config( - id = 107, - key = "telemetry.optical_flow_hz", - unit = "Hz", - range(min = 1, max = 100) - )] - optical_flow_hz: u8, -} - -/// Complete user-writable configuration advertised by this board. -#[derive(Clone, kaptan_config::Config)] -pub struct UserConfig { - mavlink: MavlinkConfig, - telemetry: TelemetryConfig, -} - -/// Holybro Kakute F7 Mini V3. pub struct KakuteF7MiniV3; -impl F7Board for KakuteF7MiniV3 { - type UserConfig = UserConfig; - type ImmutableConfig = ImmutableConfig; +impl PortCatalog<5> for KakuteF7MiniV3 { + const DESCRIPTORS: [PortDescriptor; 5] = [UART1, UART2, UART3, UART4, UART6]; +} - const FACTORY_USER_CONFIG: UserConfig = UserConfig { - mavlink: MavlinkConfig { - system_id: 1, - component_id: 1, - log_level_filter: LevelFilter::Info, - }, - telemetry: TelemetryConfig { - imu_hz: 20, - heartbeat_hz: 2, - range_hz: 20, - optical_flow_hz: 20, - }, - }; - const IMMUTABLE_CONFIG: ImmutableConfig = ImmutableConfig { +impl F7Board for KakuteF7MiniV3 { + type UserConfig = F722UserConfig; + type ImmutableConfig = F722ImmutableConfig; + + const FACTORY_USER_CONFIG: Self::UserConfig = Self::UserConfig::FACTORY; + const IMMUTABLE_CONFIG: Self::ImmutableConfig = F722ImmutableConfig { name: "Kakute F7 Mini V3", service_hz: 100, - hardware: F722Hardware::new() - .output_inhibit(Pb0Pb1Pb4Pb5Low) - .imu( - Mpu6000::on_spi1() - .with_drdy(Pa4) - .mounted(ImuMount::BodyAligned) - .configured(Mpu6000Config { - accelerometer: AccelerometerRange::PlusMinus4G, - gyroscope: GyroscopeRange::PlusMinus500DegreesPerSecond, - digital_low_pass_filter: DigitalLowPassFilter::Bandwidth42Hz, - sample_rate_divider: 9, - }), - ) - .mavlink(Mavlink::on_usart1().baud(115_200)) - .vertical_sensor(VerticalSensor::mtf01_on_uart4().baud(115_200)) - .status_led(Pd2ActiveLow) - .telemetry_only(), + hardware: F722Hardware::new( + Mpu6000Spi1 { + mount: ImuMount::Yaw270, + config: Mpu6000Config { + accelerometer: AccelerometerRange::PlusMinus4G, + gyroscope: GyroscopeRange::PlusMinus500DegreesPerSecond, + digital_low_pass_filter: DigitalLowPassFilter::Bandwidth42Hz, + sample_rate_divider: 9, + }, + }, + SerialInventory::new(&Self::DESCRIPTORS), + StatusLed::ActiveLowPd2, + VerticalSensor::None, + MotorPins::Six, + ), }; } diff --git a/firmware/libs/kaptan-config-derive/src/expand.rs b/firmware/libs/kaptan-config-derive/src/expand.rs index f2a3952..ec0b584 100644 --- a/firmware/libs/kaptan-config-derive/src/expand.rs +++ b/firmware/libs/kaptan-config-derive/src/expand.rs @@ -38,7 +38,7 @@ fn expand_scalars( checks: field_checks, } = expand_field(&crate_path, field)?; descriptors.push(descriptor); - checks.extend(field_checks); + checks.extend(field_checks.iter().cloned()); let id = field.id.expect("scalar configuration fields have IDs"); let ty = &field.ty; let field_ident = &field.ident; @@ -90,7 +90,11 @@ fn expand_scalars( }; let previous = self.#field_ident; self.#field_ident = next; - if let Err(violation) = self.__validate_local_configuration() { + let validation = (|| -> Result<(), #crate_path::ConfigViolation> { + #(#field_checks)* + Ok(()) + })(); + if let Err(violation) = validation { self.#field_ident = previous; return Err(#crate_path::FieldWriteError::Violation(violation)); } @@ -272,8 +276,35 @@ fn expand_field(crate_path: &syn::Path, field: &crate::parse::FieldModel) -> Res Check::Maximum(value) => Some(value), _ => None, }); + let finite = field + .checks + .iter() + .any(|check| matches!(check, Check::Finite)); + let nonzero = field + .checks + .iter() + .any(|check| matches!(check, Check::Nonzero)); + let float = matches!(&field.ty, syn::Type::Path(path) + if path.path.is_ident("f32") || path.path.is_ident("f64")); + if float && !finite && (minimum.is_some() || maximum.is_some()) { + let field_ident = &field.ident; + checks.push(quote! { + if self.#field_ident.is_nan() { + return Err(#crate_path::ConfigViolation::field( + #crate_path::FieldId::new(#id), + #crate_path::ValidationRule::Unordered, + )); + } + }); + } for check in &field.checks { - checks.push(expand_check(crate_path, &field.ident, id, check.clone())); + checks.push(expand_check( + crate_path, + &field.ident, + id, + check.clone(), + float, + )); } let key = field.display_key.as_ref().expect("id fields have a key"); let unit = field.unit.as_ref().expect("id fields have a unit"); @@ -302,6 +333,8 @@ fn expand_field(crate_path: &syn::Path, field: &crate::parse::FieldModel) -> Res maximum: #maximum, enum_options: #enum_options, unit: #unit, + finite: #finite, + nonzero: #nonzero, } }, checks, @@ -344,7 +377,18 @@ fn config_value_constructor_tokens(crate_path: &syn::Path, ty: &syn::Type) -> Re Ok(value) } -fn expand_check(crate_path: &syn::Path, field: &syn::Ident, id: u16, check: Check) -> TokenStream { +fn expand_check( + crate_path: &syn::Path, + field: &syn::Ident, + id: u16, + check: Check, + float: bool, +) -> TokenStream { + let zero = if float { + quote! { 0.0 } + } else { + quote! { 0 } + }; match check { Check::Finite => quote! { if !self.#field.is_finite() { @@ -355,7 +399,7 @@ fn expand_check(crate_path: &syn::Path, field: &syn::Ident, id: u16, check: Chec } }, Check::Nonzero => quote! { - if self.#field == 0 { + if self.#field == #zero { return Err(#crate_path::ConfigViolation::field( #crate_path::FieldId::new(#id), #crate_path::ValidationRule::Zero, diff --git a/firmware/libs/kaptan-config-derive/src/parse.rs b/firmware/libs/kaptan-config-derive/src/parse.rs index 9080390..006b857 100644 --- a/firmware/libs/kaptan-config-derive/src/parse.rs +++ b/firmware/libs/kaptan-config-derive/src/parse.rs @@ -482,10 +482,12 @@ fn scalar_literal(expression: &Expr, ty: &str) -> Result { })?; Ok(ScalarLiteral::Unsigned(value)) } - "f32" => Ok(ScalarLiteral::Float(f64::from( - float_literal(expression)? as f32 - ))), - "f64" => float_literal(expression).map(ScalarLiteral::Float), + // Parse at the emitted literal's native precision. Parsing through f64 + // first can double-round an f32 bound across an ordering boundary. + "f32" => Ok(ScalarLiteral::Float(f64::from(float_literal::( + expression, + )?))), + "f64" => float_literal::(expression).map(ScalarLiteral::Float), _ => Err(syn::Error::new( expression.span(), "configuration metadata supports bool, i32, u8, u16, u32, u64, f32, and f64", @@ -542,7 +544,11 @@ fn unsigned_literal(expression: &Expr) -> Result { } } -fn float_literal(expression: &Expr) -> Result { +fn float_literal(expression: &Expr) -> Result +where + T: std::str::FromStr + core::ops::Neg, + T::Err: core::fmt::Display, +{ let (negative, literal) = match expression { Expr::Lit(expression) => (false, &expression.lit), Expr::Unary(expression) if matches!(expression.op, UnOp::Neg(_)) => { @@ -563,10 +569,10 @@ fn float_literal(expression: &Expr) -> Result { }; let value = match literal { Lit::Int(value) => value - .base10_parse::() + .base10_parse::() .map_err(|error| syn::Error::new(value.span(), error))?, Lit::Float(value) => value - .base10_parse::() + .base10_parse::() .map_err(|error| syn::Error::new(value.span(), error))?, _ => { return Err(syn::Error::new( @@ -601,30 +607,14 @@ fn require_float(ty: &Type, span: &impl Spanned) -> Result<()> { } fn require_numeric(ty: &Type, span: &impl Spanned) -> Result<()> { - if scalar_type_name(ty).is_some_and(|name| { - matches!( - name, - "f32" - | "f64" - | "i8" - | "i16" - | "i32" - | "i64" - | "i128" - | "isize" - | "u8" - | "u16" - | "u32" - | "u64" - | "u128" - | "usize" - ) - }) { + if scalar_type_name(ty) + .is_some_and(|name| matches!(name, "f32" | "f64" | "i32" | "u8" | "u16" | "u32" | "u64")) + { Ok(()) } else { Err(syn::Error::new( span.span(), - "nonzero and range require a primitive numeric field", + "nonzero and range require a supported primitive numeric field", )) } } @@ -644,18 +634,11 @@ fn scalar_type_name(ty: &Type) -> Option<&str> { ident if ident == "bool" => "bool", ident if ident == "f32" => "f32", ident if ident == "f64" => "f64", - ident if ident == "i8" => "i8", - ident if ident == "i16" => "i16", ident if ident == "i32" => "i32", - ident if ident == "i64" => "i64", - ident if ident == "i128" => "i128", - ident if ident == "isize" => "isize", ident if ident == "u8" => "u8", ident if ident == "u16" => "u16", ident if ident == "u32" => "u32", ident if ident == "u64" => "u64", - ident if ident == "u128" => "u128", - ident if ident == "usize" => "usize", _ => return None, }) } diff --git a/firmware/libs/kaptan-config-derive/tests/section.rs b/firmware/libs/kaptan-config-derive/tests/section.rs index 857d6f8..cbdedd9 100644 --- a/firmware/libs/kaptan-config-derive/tests/section.rs +++ b/firmware/libs/kaptan-config-derive/tests/section.rs @@ -290,3 +290,91 @@ fn recursive_tree_rejects_duplicate_display_keys() { Err(issue) if issue.rule() == ValidationRule::Inconsistent )); } + +#[derive(Config)] +struct FloatRules { + #[config(id = 1, key = "finite", unit = "", finite)] + finite: f32, + #[config(id = 2, key = "nonzero", unit = "", nonzero)] + nonzero: f64, + #[config(id = 3, key = "lower", unit = "", range(min = 0.0))] + lower: f32, + #[config(id = 4, key = "upper", unit = "", range(max = 1.0))] + upper: f64, + #[config(id = 5, key = "free", unit = "")] + free: f64, + #[config(id = 6, key = "nonzero32", unit = "", nonzero)] + nonzero32: f32, +} + +#[test] +fn scalar_rules_preserve_local_repair_and_float_domains() { + let mut values = FloatRules { + finite: f32::NAN, + nonzero: 0.0, + lower: f32::NAN, + upper: f64::NAN, + free: f64::NAN, + nonzero32: 0.0, + }; + // Every other field is invalid. A local repair must still succeed. + values + .set_field(FieldId::new(1), ConfigValue::F32(1.0)) + .unwrap(); + for (id, value, rule) in [ + ( + 1, + ConfigValue::F32(f32::INFINITY), + ValidationRule::NonFinite, + ), + (2, ConfigValue::F64(0.0), ValidationRule::Zero), + (2, ConfigValue::F64(-0.0), ValidationRule::Zero), + (3, ConfigValue::F32(f32::NAN), ValidationRule::Unordered), + (4, ConfigValue::F64(f64::NAN), ValidationRule::Unordered), + (6, ConfigValue::F32(0.0), ValidationRule::Zero), + (6, ConfigValue::F32(-0.0), ValidationRule::Zero), + ] { + let id = FieldId::new(id); + let before = values.field_value(id); + assert_eq!( + values.set_field(id, value), + Err(FieldWriteError::Violation(ConfigViolation::field(id, rule))) + ); + assert_eq!(values.field_value(id), before); + } + // Neither nonzero nor a one-sided range implies finite. + for (id, value) in [ + (2, ConfigValue::F64(f64::from_bits(0xfff0_0000_0000_0042))), + (3, ConfigValue::F32(f32::INFINITY)), + (4, ConfigValue::F64(f64::NEG_INFINITY)), + (5, ConfigValue::F64(f64::from_bits(0x7ff8_0000_0000_1234))), + (6, ConfigValue::F32(f32::INFINITY)), + ] { + values.set_field(FieldId::new(id), value).unwrap(); + assert_eq!(values.field_value(FieldId::new(id)), Some(value)); + } + values.validate().unwrap(); +} + +#[test] +fn rule_metadata_changes_fingerprints() { + let descriptor = FloatRules::DESCRIPTORS[0]; + assert!(descriptor.finite); + assert!(!descriptor.nonzero); + assert!(FloatRules::DESCRIPTORS[1].nonzero); + let original = kaptan_config::validation::__schema_id(&[descriptor]); + assert_ne!( + original, + kaptan_config::validation::__schema_id(&[kaptan_config::FieldDescriptor { + finite: false, + ..descriptor + }]) + ); + assert_ne!( + original, + kaptan_config::validation::__schema_id(&[kaptan_config::FieldDescriptor { + nonzero: true, + ..descriptor + }]) + ); +} diff --git a/firmware/libs/kaptan-config-derive/tests/ui/f32_double_rounded_bounds.rs b/firmware/libs/kaptan-config-derive/tests/ui/f32_double_rounded_bounds.rs new file mode 100644 index 0000000..eb590f5 --- /dev/null +++ b/firmware/libs/kaptan-config-derive/tests/ui/f32_double_rounded_bounds.rs @@ -0,0 +1,9 @@ +use kaptan_config_derive::Config; + +#[derive(Config)] +struct InvertedNativeBounds { + #[config(id = 1, key = "value", unit = "", range(min = 1.0000000596046448, max = 1.0))] + value: f32, +} + +fn main() {} diff --git a/firmware/libs/kaptan-config-derive/tests/ui/f32_double_rounded_bounds.stderr b/firmware/libs/kaptan-config-derive/tests/ui/f32_double_rounded_bounds.stderr new file mode 100644 index 0000000..09dfa18 --- /dev/null +++ b/firmware/libs/kaptan-config-derive/tests/ui/f32_double_rounded_bounds.stderr @@ -0,0 +1,5 @@ +error: configuration range minimum exceeds maximum + --> tests/ui/f32_double_rounded_bounds.rs:6:5 + | +6 | value: f32, + | ^^^^^ diff --git a/firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_bool.rs b/firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_bool.rs new file mode 100644 index 0000000..a569448 --- /dev/null +++ b/firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_bool.rs @@ -0,0 +1,24 @@ +use kaptan_config_derive::Config; + +#[derive(Config)] +struct NonzeroBool { + #[config(id = 1, key = "value", unit = "")] + #[config(nonzero)] + value: bool, +} + +#[derive(Config)] +struct RangedBool { + #[config(id = 1, key = "value", unit = "")] + #[config(range(min = 0, max = 1))] + value: bool, +} + +#[derive(Config)] +struct FiniteBool { + #[config(id = 1, key = "value", unit = "")] + #[config(finite)] + value: bool, +} + +fn main() {} diff --git a/firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_bool.stderr b/firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_bool.stderr new file mode 100644 index 0000000..6c4054f --- /dev/null +++ b/firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_bool.stderr @@ -0,0 +1,17 @@ +error: nonzero and range require a supported primitive numeric field + --> tests/ui/numeric_rules_on_bool.rs:6:14 + | +6 | #[config(nonzero)] + | ^^^^^^^ + +error: nonzero and range require a supported primitive numeric field + --> tests/ui/numeric_rules_on_bool.rs:13:14 + | +13 | #[config(range(min = 0, max = 1))] + | ^^^^^ + +error: finite requires an f32 or f64 field + --> tests/ui/numeric_rules_on_bool.rs:20:14 + | +20 | #[config(finite)] + | ^^^^^^ diff --git a/firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_enum.rs b/firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_enum.rs new file mode 100644 index 0000000..4e4136a --- /dev/null +++ b/firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_enum.rs @@ -0,0 +1,31 @@ +use kaptan_config_derive::{Config, ConfigEnum}; + +#[derive(Clone, Copy, ConfigEnum)] +#[repr(u8)] +enum Mode { + Off = 0, + On = 1, +} + +#[derive(Config)] +struct NonzeroEnum { + #[config(enum, id = 1, key = "value", unit = "")] + #[config(nonzero)] + value: Mode, +} + +#[derive(Config)] +struct RangedEnum { + #[config(enum, id = 1, key = "value", unit = "")] + #[config(range(min = 0, max = 1))] + value: Mode, +} + +#[derive(Config)] +struct FiniteEnum { + #[config(enum, id = 1, key = "value", unit = "")] + #[config(finite)] + value: Mode, +} + +fn main() {} diff --git a/firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_enum.stderr b/firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_enum.stderr new file mode 100644 index 0000000..4ce155a --- /dev/null +++ b/firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_enum.stderr @@ -0,0 +1,17 @@ +error: nonzero and range require a supported primitive numeric field + --> tests/ui/numeric_rules_on_enum.rs:13:14 + | +13 | #[config(nonzero)] + | ^^^^^^^ + +error: nonzero and range require a supported primitive numeric field + --> tests/ui/numeric_rules_on_enum.rs:20:14 + | +20 | #[config(range(min = 0, max = 1))] + | ^^^^^ + +error: finite requires an f32 or f64 field + --> tests/ui/numeric_rules_on_enum.rs:27:14 + | +27 | #[config(finite)] + | ^^^^^^ diff --git a/firmware/libs/kaptan-config-derive/tests/ui/unsupported_scalar.rs b/firmware/libs/kaptan-config-derive/tests/ui/unsupported_scalar.rs new file mode 100644 index 0000000..0d56d8d --- /dev/null +++ b/firmware/libs/kaptan-config-derive/tests/ui/unsupported_scalar.rs @@ -0,0 +1,15 @@ +use kaptan_config_derive::Config; + +#[derive(Config)] +struct UnsupportedScalar { + #[config(id = 1, key = "wide", unit = "")] + wide: u128, +} + +#[derive(Config)] +struct UnsupportedCheckedScalar { + #[config(id = 2, key = "signed", unit = "", nonzero)] + signed: i64, +} + +fn main() {} diff --git a/firmware/libs/kaptan-config-derive/tests/ui/unsupported_scalar.stderr b/firmware/libs/kaptan-config-derive/tests/ui/unsupported_scalar.stderr new file mode 100644 index 0000000..bd96748 --- /dev/null +++ b/firmware/libs/kaptan-config-derive/tests/ui/unsupported_scalar.stderr @@ -0,0 +1,11 @@ +error: non-primitive configuration fields require config(enum) + --> tests/ui/unsupported_scalar.rs:6:11 + | +6 | wide: u128, + | ^^^^ + +error: nonzero and range require a supported primitive numeric field + --> tests/ui/unsupported_scalar.rs:11:49 + | +11 | #[config(id = 2, key = "signed", unit = "", nonzero)] + | ^^^^^^^ diff --git a/firmware/libs/kaptan-config/src/artifact.rs b/firmware/libs/kaptan-config/src/artifact.rs index 34f4cb3..1df335d 100644 --- a/firmware/libs/kaptan-config/src/artifact.rs +++ b/firmware/libs/kaptan-config/src/artifact.rs @@ -2,7 +2,9 @@ //! //! The artifact is independent of any transport and of the private flash //! journal. It contains the generated field metadata, one complete committed -//! value set, and the board-selected factory values required by a generic UI. +//! value set, the board-selected factory values, and exact expected-base values +//! for guarded full replacement. Each field carries current, factory, then +//! expected-base scalars at native width before bounds and text metadata. use core::str; @@ -24,10 +26,16 @@ const CHECKSUM_LENGTH: usize = 4; const FIELD_PREFIX_LENGTH: usize = 7; const FIELD_HAS_MINIMUM: u8 = 1 << 0; const FIELD_HAS_MAXIMUM: u8 = 1 << 1; -const FIELD_FLAGS: u8 = FIELD_HAS_MINIMUM | FIELD_HAS_MAXIMUM; +const FIELD_FINITE: u8 = 1 << 2; +const FIELD_NONZERO: u8 = 1 << 3; +const FIELD_FLAGS: u8 = FIELD_HAS_MINIMUM | FIELD_HAS_MAXIMUM | FIELD_FINITE | FIELD_NONZERO; + +/// Caller-owned bitmap bytes required by dynamic KCFG decoding. +/// Hosted callers can initialize this directly on the heap with a byte vector. +pub const KCFG_FIELD_ID_SCRATCH_BYTES: usize = 8192; /// Current canonical KCFG artifact format. -const KCFG_VERSION: u8 = 1; +const KCFG_VERSION: u8 = 3; /// KCFG header bytes preceding the first field record. const KCFG_HEADER_LENGTH: usize = FIELDS_OFFSET; /// KCFG trailer bytes containing a big-endian CRC-32C. @@ -208,7 +216,7 @@ pub enum KcfgEncodeError { } /// Failure while decoding untrusted KCFG bytes. -#[derive(Debug)] +#[derive(Clone, Copy, Debug, PartialEq)] pub enum KcfgDecodeError { /// Input ends before a required fixed or variable-length component. Truncated, @@ -309,12 +317,15 @@ pub struct KcfgField<'a> { kind: ValueKind, current: ConfigValue, factory: ConfigValue, + expected_base: ConfigValue, minimum: Option, maximum: Option, key: &'a str, unit: &'a str, options: KcfgEnumOptions<'a>, current_offset: usize, + finite: bool, + nonzero: bool, } impl<'a> KcfgField<'a> { @@ -338,6 +349,12 @@ impl<'a> KcfgField<'a> { self.factory } + /// Exact committed scalar observed when this draft was first exported. + /// Editing requested values preserves this optimistic concurrency guard. + pub const fn expected_base_value(self) -> ConfigValue { + self.expected_base + } + /// Inclusive generated lower bound. pub const fn minimum(self) -> Option { self.minimum @@ -362,6 +379,16 @@ impl<'a> KcfgField<'a> { pub const fn enum_options(self) -> KcfgEnumOptions<'a> { self.options } + + /// Whether NaN and infinities are forbidden. + pub const fn finite(self) -> bool { + self.finite + } + + /// Whether numeric zero is forbidden. + pub const fn nonzero(self) -> bool { + self.nonzero + } } /// Iterator over validated field records in canonical artifact order. @@ -428,7 +455,15 @@ impl<'a> KcfgArtifact<'a> { } /// Validates and borrows one board-agnostic KCFG artifact. -pub fn decode_kcfg(input: &[u8]) -> Result, KcfgDecodeError> { +/// +/// Clears the caller's 8-KiB bitmap and visits each field once. Work is +/// O(input bytes + 8192), including checksum, text and enum validation, even +/// on a late duplicate. Per-field enum scratch is 32 bytes. The returned view +/// borrows only the input, so the scratch can be reused immediately. +pub fn decode_kcfg<'a>( + input: &'a [u8], + field_ids: &mut [u8; KCFG_FIELD_ID_SCRATCH_BYTES], +) -> Result, KcfgDecodeError> { if input.len() < KCFG_HEADER_LENGTH + KCFG_CHECKSUM_LENGTH { return Err(KcfgDecodeError::Truncated); } @@ -449,9 +484,13 @@ pub fn decode_kcfg(input: &[u8]) -> Result, KcfgDecodeError> { } let field_count = read_u16(&input[FIELD_COUNT_OFFSET..FIELDS_OFFSET]); + field_ids.fill(0); let mut cursor = FIELDS_OFFSET; for _ in 0..field_count { - let (_, next) = parse_field(input, cursor, checksum_offset)?; + let (field, next) = parse_field(input, cursor, checksum_offset)?; + if mark_seen(field_ids, usize::from(field.id.get())) { + return Err(KcfgDecodeError::DuplicateField(field.id)); + } cursor = next; } if cursor != checksum_offset { @@ -466,7 +505,6 @@ pub fn decode_kcfg(input: &[u8]) -> Result, KcfgDecodeError> { field_count, restart_required: artifact_flags & KCFG_RESTART_REQUIRED != 0, }; - reject_duplicate_fields(&artifact)?; Ok(artifact) } @@ -482,7 +520,7 @@ pub fn kcfg_length() -> Result { ))?; validate_descriptor(descriptor)?; let width = descriptor.kind.width(); - let values = 2 + let values = 3 + usize::from(descriptor.minimum.is_some()) + usize::from(descriptor.maximum.is_some()); let mut field_length = FIELD_PREFIX_LENGTH @@ -507,11 +545,7 @@ pub fn kcfg_length() -> Result { Ok(length) } -/// Prepared immutable metadata for one canonical generated artifact. -/// -/// A file endpoint can retain this small value for the lifetime of an open -/// read and reconstruct range exporters without recomputing the file length or -/// either checksum for every transport chunk. +/// Cached length and checksums for an immutable export. #[derive(Clone, Copy)] pub struct KcfgExportPlan { length: usize, @@ -520,12 +554,8 @@ pub struct KcfgExportPlan { restart_required: bool, } -/// Allocation-free view of one canonical generated artifact. -/// -/// The exporter can fill an arbitrary transport range directly from the typed -/// configuration. Firmware therefore does not need a buffer sized for the -/// complete public file. Construction also computes both the embedded KCFG -/// checksum and the checksum of the complete file bytes. +/// Exports arbitrary byte ranges directly from typed configuration. +/// Construction computes both the KCFG checksum and the complete-file checksum. pub struct KcfgExporter<'a, C: ConfigModel> { current: &'a C, factory: &'a C, @@ -611,19 +641,31 @@ impl<'a, C: ConfigModel> KcfgExporter<'a, C> { } } +/// Complete requested values and the exact committed base observed by an editor. +#[derive(Clone, Debug, PartialEq)] +pub struct KcfgUpdate { + /// Requested replacement, subject to complete validation before persistence. + pub candidate: C, + /// Expected committed values, compared before no-op detection or persistence. + pub expected_base: C, +} + /// Bounded sequential decoder for a typed KCFG replacement. /// -/// Only the candidate plus eight scalar bytes are retained. Generated -/// metadata and factory bytes are checked as they arrive, and a field setter +/// Only two typed candidates plus fixed scalar/parser state are retained. +/// Generated metadata and factory bytes are checked as they arrive, and a field setter /// is called only after its complete canonical record has been received. /// Discarding the importer discards every partially imported candidate. pub struct KcfgImporter { candidate: C, + expected_base: C, + error: Option, phase: ImportPhase, header_offset: usize, field_index: u16, field_offset: usize, current_bytes: [u8; 8], + expected_base_bytes: [u8; 8], checksum_bytes: [u8; CHECKSUM_LENGTH], checksum_offset: usize, crc: Crc32c, @@ -646,31 +688,48 @@ impl KcfgImporter { emit_body(factory, factory, false, |_| true)?; Ok(Self { candidate: factory.clone(), + expected_base: factory.clone(), + error: None, phase: ImportPhase::Header, header_offset: 0, field_index: 0, field_offset: 0, current_bytes: [0; 8], + expected_base_bytes: [0; 8], checksum_bytes: [0; CHECKSUM_LENGTH], checksum_offset: 0, crc: Crc32c::new(), }) } - /// Consumes the next contiguous file bytes. + /// Consumes the next contiguous file bytes. Any parser error is terminal: + /// subsequent pushes and finish return the original error. pub fn push(&mut self, factory: &C, input: &[u8]) -> Result<(), KcfgDecodeError> { + if let Some(error) = self.error { + return Err(error); + } for byte in input { - self.push_byte(factory, *byte)?; + if let Err(error) = self.push_byte(factory, *byte) { + self.error = Some(error); + return Err(error); + } } Ok(()) } - /// Returns the complete candidate after checksum and schema validation. - pub fn finish(self) -> Result { + /// Returns requested and expected-base candidates after checksum and schema + /// validation. Relational validation belongs to the manager at commit time. + pub fn finish(self) -> Result, KcfgDecodeError> { + if let Some(error) = self.error { + return Err(error); + } if self.phase != ImportPhase::Done { return Err(KcfgDecodeError::Truncated); } - Ok(self.candidate) + Ok(KcfgUpdate { + candidate: self.candidate, + expected_base: self.expected_base, + }) } fn push_byte(&mut self, factory: &C, byte: u8) -> Result<(), KcfgDecodeError> { @@ -743,6 +802,7 @@ impl KcfgImporter { } match expected_field_byte(descriptor, factory, self.field_offset) { Some(ExpectedFieldByte::Current(index)) => self.current_bytes[index] = byte, + Some(ExpectedFieldByte::ExpectedBase(index)) => self.expected_base_bytes[index] = byte, Some(ExpectedFieldByte::Factory(expected)) if byte != expected => { return Err(KcfgDecodeError::FactoryMismatch(descriptor.id)); } @@ -762,7 +822,16 @@ impl KcfgImporter { self.candidate .set_field(descriptor.id, value) .map_err(KcfgDecodeError::InvalidWrite)?; + let base = value_from_bytes( + descriptor.id, + descriptor.kind, + &self.expected_base_bytes[..width], + )?; + self.expected_base + .set_field(descriptor.id, base) + .map_err(KcfgDecodeError::InvalidWrite)?; self.current_bytes = [0; 8]; + self.expected_base_bytes = [0; 8]; self.field_offset = 0; self.field_index += 1; if self.field_index == C::field_count() { @@ -809,8 +878,8 @@ fn encode_kcfg<'a, C: ConfigModel>( /// /// A board-agnostic host can retain the received artifact bytes, present the /// borrowed metadata to its UI, and provide edited values by stable field ID. -/// The generated metadata, schema, factory values, and enum choices are copied -/// unchanged and the checksum is regenerated here. +/// The expected-base scalars, metadata, schema, factory values, and enum choices +/// are copied unchanged and the checksum is regenerated here. pub fn encode_edited_kcfg<'a, F>( source: &KcfgArtifact<'_>, restart_required: bool, @@ -848,31 +917,9 @@ where #[cfg(test)] fn decode_kcfg_for(input: &[u8], factory: &C) -> Result { - validate_model::().map_err(KcfgDecodeError::InvalidModel)?; - let artifact = decode_kcfg(input)?; - if artifact.schema_id != C::SCHEMA_ID { - return Err(KcfgDecodeError::WrongSchema); - } - if artifact.field_count != C::field_count() { - return Err(KcfgDecodeError::WrongFieldCount); - } - - let mut candidate = factory.clone(); - for (index, field) in artifact.fields().enumerate() { - let descriptor = C::descriptor(index as u16).ok_or(KcfgDecodeError::InvalidModel( - SnapshotModelError::MissingDescriptor(index as u16), - ))?; - if !same_descriptor(&field, descriptor) { - return Err(KcfgDecodeError::DescriptorMismatch(field.id)); - } - if factory.field_value(field.id) != Some(field.factory) { - return Err(KcfgDecodeError::FactoryMismatch(field.id)); - } - candidate - .set_field(field.id, field.current) - .map_err(KcfgDecodeError::InvalidWrite)?; - } - Ok(candidate) + let mut importer = KcfgImporter::new(factory).expect("valid test model"); + importer.push(factory, input)?; + Ok(importer.finish()?.candidate) } fn emit_body( @@ -940,6 +987,10 @@ where if !emit_value(factory, &mut emit) { return Ok(()); } + // A fresh export establishes the exact base for subsequent host edits. + if !emit_value(current, &mut emit) { + return Ok(()); + } if let Some(minimum) = descriptor.minimum && !emit_value(minimum, &mut emit) { @@ -973,16 +1024,17 @@ fn validate_descriptor(descriptor: &crate::FieldDescriptor) -> Result<(), KcfgEn || descriptor.unit.contains('\0') || descriptor.enum_options.len() > u8::MAX as usize || (!descriptor.enum_options.is_empty() && descriptor.kind != ValueKind::U8) + || (descriptor.finite && !matches!(descriptor.kind, ValueKind::F32 | ValueKind::F64)) + || (descriptor.nonzero && descriptor.kind == ValueKind::Bool) { return Err(KcfgEncodeError::InvalidMetadata(descriptor.id)); } - for (index, (value, label)) in descriptor.enum_options.iter().enumerate() { + let mut seen = [0_u8; 32]; + for (value, label) in descriptor.enum_options { if label.is_empty() || label.len() > u8::MAX as usize || label.contains('\0') - || descriptor.enum_options[..index] - .iter() - .any(|previous| previous.0 == *value) + || mark_seen(&mut seen, usize::from(*value)) { return Err(KcfgEncodeError::InvalidMetadata(descriptor.id)); } @@ -995,6 +1047,8 @@ fn parse_field( start: usize, limit: usize, ) -> Result<(KcfgField<'_>, usize), KcfgDecodeError> { + #[cfg(test)] + tests::FIELD_PARSES.with(|count| count.set(count.get() + 1)); let prefix_end = start .checked_add(FIELD_PREFIX_LENGTH) .ok_or(KcfgDecodeError::Truncated)?; @@ -1007,7 +1061,10 @@ fn parse_field( let kind = ValueKind::try_from_code(raw_kind).ok_or(KcfgDecodeError::InvalidValueKind(raw_kind))?; let flags = bytes[start + 3]; - if flags & !FIELD_FLAGS != 0 { + if flags & !FIELD_FLAGS != 0 + || (flags & FIELD_FINITE != 0 && !matches!(kind, ValueKind::F32 | ValueKind::F64)) + || (flags & FIELD_NONZERO != 0 && kind == ValueKind::Bool) + { return Err(KcfgDecodeError::InvalidFieldFlags(id)); } let key_length = usize::from(bytes[start + 4]); @@ -1021,6 +1078,7 @@ fn parse_field( let current_offset = cursor; let current = parse_value(bytes, &mut cursor, limit, id, kind)?; let factory = parse_value(bytes, &mut cursor, limit, id, kind)?; + let expected_base = parse_value(bytes, &mut cursor, limit, id, kind)?; let minimum = if flags & FIELD_HAS_MINIMUM != 0 { Some(parse_value(bytes, &mut cursor, limit, id, kind)?) } else { @@ -1047,7 +1105,8 @@ fn parse_field( canonical_text(&bytes[key_end..unit_end], true).ok_or(KcfgDecodeError::InvalidText(id))?; cursor = unit_end; let options_start = cursor; - for option_index in 0..enum_count { + let mut seen = [0_u8; 32]; + for _ in 0..enum_count { if cursor.checked_add(2).is_none_or(|end| end > limit) { return Err(KcfgDecodeError::Truncated); } @@ -1062,11 +1121,7 @@ fn parse_field( if canonical_text(&bytes[cursor + 2..label_end], false).is_none() { return Err(KcfgDecodeError::InvalidText(id)); } - let mut previous = KcfgEnumOptions { - bytes: &bytes[options_start..cursor], - remaining: option_index, - }; - if previous.any(|option| option.value == value) { + if mark_seen(&mut seen, usize::from(value)) { return Err(KcfgDecodeError::DuplicateEnumValue(id)); } cursor = label_end; @@ -1078,6 +1133,7 @@ fn parse_field( kind, current, factory, + expected_base, minimum, maximum, key, @@ -1087,22 +1143,21 @@ fn parse_field( remaining: enum_count, }, current_offset, + finite: flags & FIELD_FINITE != 0, + nonzero: flags & FIELD_NONZERO != 0, }, cursor, )) } -fn reject_duplicate_fields(artifact: &KcfgArtifact<'_>) -> Result<(), KcfgDecodeError> { - for (index, field) in artifact.fields().enumerate() { - if artifact - .fields() - .take(index) - .any(|previous| previous.id == field.id) - { - return Err(KcfgDecodeError::DuplicateField(field.id)); - } - } - Ok(()) +fn mark_seen(bitmap: &mut [u8], value: usize) -> bool { + #[cfg(test)] + tests::BITMAP_PROBES.with(|count| count.set(count.get() + 1)); + let byte = &mut bitmap[value / 8]; + let bit = 1 << (value % 8); + let duplicate = *byte & bit != 0; + *byte |= bit; + duplicate } fn parse_value( @@ -1132,21 +1187,6 @@ fn value_from_bytes( ConfigValue::from_le_bytes(kind, bytes).ok_or(KcfgDecodeError::InvalidValue(id)) } -#[cfg(test)] -fn same_descriptor(field: &KcfgField<'_>, descriptor: &crate::FieldDescriptor) -> bool { - field.id == descriptor.id - && field.kind == descriptor.kind - && field.minimum == descriptor.minimum - && field.maximum == descriptor.maximum - && field.key == descriptor.display_key - && field.unit == descriptor.unit - && field - .options - .zip(descriptor.enum_options.iter().copied()) - .all(|(actual, expected)| (actual.value, actual.label) == expected) - && field.options.len() == descriptor.enum_options.len() -} - fn canonical_text(bytes: &[u8], allow_empty: bool) -> Option<&str> { let text = str::from_utf8(bytes).ok()?; if (!allow_empty && text.is_empty()) || text.contains('\0') { @@ -1166,6 +1206,7 @@ fn read_u32_be(bytes: &[u8]) -> u32 { enum ExpectedFieldByte { Current(usize), + ExpectedBase(usize), Factory(u8), Descriptor(u8), } @@ -1202,6 +1243,10 @@ fn expected_field_byte( ))); } cursor += width; + if offset < cursor + width { + return Some(ExpectedFieldByte::ExpectedBase(offset - cursor)); + } + cursor += width; if let Some(minimum) = descriptor.minimum { if offset < cursor + width { return Some(ExpectedFieldByte::Descriptor(value_byte( @@ -1254,7 +1299,7 @@ fn expected_field_byte( fn field_record_length(descriptor: &crate::FieldDescriptor) -> usize { let width = descriptor.kind.width(); let value_count = - 2 + usize::from(descriptor.minimum.is_some()) + usize::from(descriptor.maximum.is_some()); + 3 + usize::from(descriptor.minimum.is_some()) + usize::from(descriptor.maximum.is_some()); FIELD_PREFIX_LENGTH + width * value_count + descriptor.display_key.len() @@ -1269,6 +1314,8 @@ fn field_record_length(descriptor: &crate::FieldDescriptor) -> usize { fn field_flags(descriptor: &crate::FieldDescriptor) -> u8 { (u8::from(descriptor.minimum.is_some()) * FIELD_HAS_MINIMUM) | (u8::from(descriptor.maximum.is_some()) * FIELD_HAS_MAXIMUM) + | (u8::from(descriptor.finite) * FIELD_FINITE) + | (u8::from(descriptor.nonzero) * FIELD_NONZERO) } fn value_byte(value: ConfigValue, offset: usize) -> u8 { @@ -1321,8 +1368,78 @@ impl<'a> RangeSink<'a> { #[cfg(test)] mod tests { + extern crate std; + use super::*; use crate::{ConfigViolation, FieldDescriptor, ValidationRule}; + use std::{vec, vec::Vec}; + + std::thread_local! { + pub(super) static FIELD_PARSES: core::cell::Cell = const { core::cell::Cell::new(0) }; + pub(super) static BITMAP_PROBES: core::cell::Cell = const { core::cell::Cell::new(0) }; + } + + fn raw_artifact(count: u16, last_id: u16, options: &[u8]) -> Vec { + let mut bytes = vec![0; KCFG_HEADER_LENGTH]; + bytes[..4].copy_from_slice(&MAGIC); + bytes[VERSION_OFFSET] = KCFG_VERSION; + bytes[FIELD_COUNT_OFFSET..FIELDS_OFFSET].copy_from_slice(&count.to_le_bytes()); + for index in 0..count { + let last = index == count - 1; + let id = if last { last_id } else { index + 1 }; + bytes.extend_from_slice(&id.to_le_bytes()); + bytes.extend_from_slice(&[ + ValueKind::U8 as u8, + 0, + 1, + 0, + if last { options.len() as u8 } else { 0 }, + ]); + bytes.extend_from_slice(&[1, 1, 1, b'k']); + if last { + for value in options { + bytes.extend_from_slice(&[*value, 1, b'x']); + } + } + } + bytes.extend_from_slice(&crc32c(&bytes).to_be_bytes()); + bytes + } + + #[test] + fn maximum_counts_and_late_duplicates_have_one_parse_and_bitmap_probe_per_item() { + let mut scratch = vec![0xff; KCFG_FIELD_ID_SCRATCH_BYTES]; + let scratch: &mut [u8; KCFG_FIELD_ID_SCRATCH_BYTES] = + scratch.as_mut_slice().try_into().unwrap(); + let mut options: Vec = (0..u8::MAX).collect(); + let valid = raw_artifact(u16::MAX - 1, u16::MAX - 1, &options); + FIELD_PARSES.set(0); + BITMAP_PROBES.set(0); + assert!(decode_kcfg(&valid, scratch).is_ok()); + assert_eq!(FIELD_PARSES.get(), 65_534); + assert_eq!(BITMAP_PROBES.get(), 65_534 + 255); + + let duplicate = raw_artifact(u16::MAX, 1, &options); + FIELD_PARSES.set(0); + BITMAP_PROBES.set(0); + assert!( + matches!(decode_kcfg(&duplicate, scratch), Err(KcfgDecodeError::DuplicateField(id)) if id == FieldId::new(1)) + ); + assert_eq!(FIELD_PARSES.get(), 65_535); + assert_eq!(BITMAP_PROBES.get(), 65_535 + 255); + + options[254] = 0; + let duplicate = raw_artifact(u16::MAX - 1, u16::MAX - 1, &options); + FIELD_PARSES.set(0); + BITMAP_PROBES.set(0); + assert!( + matches!(decode_kcfg(&duplicate, scratch), Err(KcfgDecodeError::DuplicateEnumValue(id)) if id == FieldId::new(65_534)) + ); + assert_eq!(FIELD_PARSES.get(), 65_534); + assert_eq!(BITMAP_PROBES.get(), 65_533 + 255); + // A failed parse cannot poison subsequent calls using the same scratch. + assert!(decode_kcfg(&valid, scratch).is_ok()); + } const RATE: FieldId = FieldId::new(1); const PORT: FieldId = FieldId::new(2); @@ -1337,6 +1454,8 @@ mod tests { maximum: Some(ConfigValue::U8(100)), enum_options: &[], unit: "Hz", + finite: false, + nonzero: false, }, FieldDescriptor { id: PORT, @@ -1346,6 +1465,8 @@ mod tests { maximum: Some(ConfigValue::U16(u16::MAX)), enum_options: &[], unit: "id", + finite: false, + nonzero: false, }, FieldDescriptor { id: MODE, @@ -1355,6 +1476,8 @@ mod tests { maximum: None, enum_options: MODE_OPTIONS, unit: "level", + finite: false, + nonzero: false, }, ]; @@ -1472,7 +1595,8 @@ mod tests { }; let mut bytes = [0; 256]; let encoded = encode_kcfg(¤t, &factory, true, &mut bytes).unwrap(); - let artifact = decode_kcfg(encoded).unwrap(); + let artifact = decode_kcfg(encoded, &mut [0; KCFG_FIELD_ID_SCRATCH_BYTES]).unwrap(); + assert_eq!(encoded[VERSION_OFFSET], 3); assert_eq!(artifact.schema_id(), Candidate::SCHEMA_ID); assert!(artifact.restart_required()); let mut fields = artifact.fields(); @@ -1489,6 +1613,20 @@ mod tests { } }) .unwrap(); + let edited_artifact = decode_kcfg(edited, &mut [0; KCFG_FIELD_ID_SCRATCH_BYTES]).unwrap(); + for field in edited_artifact.fields() { + assert_eq!( + field.expected_base_value(), + current.field_value(field.id()).unwrap() + ); + } + let mut importer = KcfgImporter::new(&factory).unwrap(); + for byte in edited { + importer + .push(&factory, core::slice::from_ref(byte)) + .unwrap(); + } + assert_eq!(importer.finish().unwrap().expected_base, current); assert_eq!( decode_kcfg_for(edited, &factory).unwrap(), Candidate { @@ -1553,7 +1691,9 @@ mod tests { for chunk in contiguous[..exporter.length()].chunks(chunk_length) { importer.push(&factory, chunk).unwrap(); } - assert_eq!(importer.finish().unwrap(), current); + let update = importer.finish().unwrap(); + assert_eq!(update.candidate, current); + assert_eq!(update.expected_base, current); } for truncated_length in 0..exporter.length() { @@ -1597,11 +1737,11 @@ mod tests { .unwrap() .len(); - storage[FIELDS_OFFSET + FIELD_PREFIX_LENGTH + 4] ^= 1; + storage[FIELDS_OFFSET + FIELD_PREFIX_LENGTH + 5] ^= 1; let checksum_offset = length - CHECKSUM_LENGTH; let checksum = crc32c(&storage[..checksum_offset]); storage[checksum_offset..length].copy_from_slice(&checksum.to_be_bytes()); - assert!(decode_kcfg(&storage[..length]).is_ok()); + assert!(decode_kcfg(&storage[..length], &mut [0; KCFG_FIELD_ID_SCRATCH_BYTES]).is_ok()); assert!(matches!( decode_kcfg_for(&storage[..length], &factory), Err(KcfgDecodeError::DescriptorMismatch(RATE)) @@ -1609,8 +1749,42 @@ mod tests { storage[checksum_offset] ^= 1; assert!(matches!( - decode_kcfg(&storage[..length]), + decode_kcfg(&storage[..length], &mut [0; KCFG_FIELD_ID_SCRATCH_BYTES]), Err(KcfgDecodeError::ChecksumMismatch) )); } + + #[test] + fn parser_errors_are_terminal_including_checksum_and_trailing_data() { + let factory = factory(); + let mut bytes = [0; 256]; + let length = encode_kcfg(&factory, &factory, false, &mut bytes) + .unwrap() + .len(); + // Header/version, metadata, scalar validation, and checksum errors must + // never allow a later push or finish to revive a partial update. + for (offset, bad_byte) in [ + (0, b'X'), + (VERSION_OFFSET, 1), + (FIELDS_OFFSET, 42), + (FIELDS_OFFSET + FIELD_PREFIX_LENGTH, 0), + (FIELDS_OFFSET + FIELD_PREFIX_LENGTH + 2, 0), + (length - 1, bytes[length - 1] ^ 1), + ] { + let mut malformed = bytes; + malformed[offset] = bad_byte; + let mut importer = KcfgImporter::new(&factory).unwrap(); + let error = importer.push(&factory, &malformed[..length]).unwrap_err(); + assert_eq!(importer.push(&factory, &bytes[..length]), Err(error)); + assert_eq!(importer.push(&factory, &[]), Err(error)); + assert_eq!(importer.finish(), Err(error)); + } + let mut importer = KcfgImporter::new(&factory).unwrap(); + importer.push(&factory, &bytes[..length]).unwrap(); + assert_eq!( + importer.push(&factory, &[0]), + Err(KcfgDecodeError::TrailingData) + ); + assert_eq!(importer.finish(), Err(KcfgDecodeError::TrailingData)); + } } diff --git a/firmware/libs/kaptan-config/src/error.rs b/firmware/libs/kaptan-config/src/error.rs index 6e43984..f0ecec0 100644 --- a/firmware/libs/kaptan-config/src/error.rs +++ b/firmware/libs/kaptan-config/src/error.rs @@ -22,6 +22,8 @@ pub enum ValidationRule { UnsupportedFormat = 7, /// A scalar does not name a declared variant of its typed enum field. InvalidEnumValue = 8, + /// An ordered range cannot contain NaN. + Unordered = 9, } impl ValidationRule { @@ -41,6 +43,7 @@ impl ValidationRule { 6 => Self::UnsupportedCapability, 7 => Self::UnsupportedFormat, 8 => Self::InvalidEnumValue, + 9 => Self::Unordered, _ => return None, }; Some(rule) diff --git a/firmware/libs/kaptan-config/src/field.rs b/firmware/libs/kaptan-config/src/field.rs index 5f9ea4a..9576f3c 100644 --- a/firmware/libs/kaptan-config/src/field.rs +++ b/firmware/libs/kaptan-config/src/field.rs @@ -3,11 +3,7 @@ use crate::{ConfigViolation, ValidationRule}; use kaptan_err_reportable::KaptanError; -/// Maximum choices representable by one enum-backed field. -/// -/// Enum values and indexed discovery use their native `u8` domain. Options -/// are immutable flash metadata, so no smaller runtime resource limit is -/// necessary. +/// Maximum choice count representable by indexed `u8` discovery. pub(crate) const MAX_ENUM_OPTIONS_PER_FIELD: usize = u8::MAX as usize; /// Maximum UTF-8 bytes advertised for one enum variant name. pub(crate) const MAX_ENUM_OPTION_LABEL_BYTES: usize = 32; @@ -47,9 +43,9 @@ impl FieldId { /// The scalar representation of a configuration value. /// -/// The variants deliberately preserve native widths: durations, for example, -/// remain `u64` microseconds instead of being lossy display-unit conversions. -#[derive(Clone, Copy, Debug, PartialEq)] +/// Preserves native widths. Equality compares kind and raw bits, including +/// signed zero and NaN payloads. +#[derive(Clone, Copy, Debug)] pub enum ConfigValue { /// Boolean value. Bool(bool), @@ -69,6 +65,16 @@ pub enum ConfigValue { F64(f64), } +/// Configuration identity is native kind plus exact scalar bits. Signed zeros +/// differ and identical NaN payloads compare equal, including in optional values. +impl PartialEq for ConfigValue { + fn eq(&self, other: &Self) -> bool { + self.kind() == other.kind() && self.raw_bits() == other.raw_bits() + } +} + +impl Eq for ConfigValue {} + /// Failure to project a generic configuration scalar into its native type. #[doc(hidden)] #[derive(Clone, Copy, Debug, Eq, PartialEq)] @@ -79,11 +85,7 @@ pub enum ScalarValueError { OutOfDomain, } -/// Native representation used by generated configuration fields. -/// -/// Generic artifacts and private snapshots retain the native scalar kind while -/// board and runtime structures keep the narrow or enum type that models the -/// value. +/// Conversion between native field types and configuration scalars. #[doc(hidden)] pub trait ConfigScalar: Copy { /// Generic scalar kind used at schema and transport boundaries. @@ -246,6 +248,10 @@ pub struct FieldDescriptor { pub enum_options: &'static [(u8, &'static str)], /// Human-readable unit for schema metadata. pub unit: &'static str, + /// Reject NaN and infinities. Only meaningful for floating-point fields. + pub finite: bool, + /// Reject numeric zero, including either floating-point signed zero. + pub nonzero: bool, } /// Failure while changing one candidate field. diff --git a/firmware/libs/kaptan-config/src/journal.rs b/firmware/libs/kaptan-config/src/journal.rs index f170a37..1635807 100644 --- a/firmware/libs/kaptan-config/src/journal.rs +++ b/firmware/libs/kaptan-config/src/journal.rs @@ -4,9 +4,12 @@ //! independently erasable slots; the generic scalar snapshot codec supplies //! the payload. The previous committed slot is never erased until the complete //! replacement is programmed, verified, and marked committed. +//! A persistence error can occur after that marker is durable, so it does not +//! establish which candidate recovery will select. use crate::{ - ConfigModel, ConfigStore, checksum::Crc32c, decode_scalar_snapshot, encode_scalar_snapshot, + ConfigModel, ConfigStore, StoreRecovery, checksum::Crc32c, decode_scalar_snapshot, + encode_scalar_snapshot, }; const MAGIC: u32 = u32::from_le_bytes(*b"KJNL"); @@ -213,23 +216,25 @@ where Ok(selected.map(|record| record.candidate)) } - fn load_matching

(&mut self, factory: &C, mut compatible: P) -> Result, Self::Error> - where - P: FnMut(&C) -> bool, - { - self.geometry()?; - let left = self.record(Slot::Left, factory)?; - let right = self.record(Slot::Right, factory)?; - let left = left.filter(|record| compatible(&record.candidate)); - let right = right.filter(|record| compatible(&record.candidate)); - let selected = match (left, right) { - (Some(left), Some(right)) if right.sequence > left.sequence => Some(right), - (Some(left), Some(_)) | (Some(left), None) => Some(left), - (None, Some(right)) => Some(right), - (None, None) => None, - }; - self.protected_slot = selected.as_ref().map(|record| record.slot); - Ok(selected.map(|record| record.candidate)) + fn load_recovery(&mut self, factory: &C) -> Result, Self::Error> { + if let Some(candidate) = self.load(factory)? { + return Ok(StoreRecovery::Committed(candidate)); + } + // Only the no-readable-record case needs an erased-byte scan. Do not + // infer factory state from erased headers with occupied payload bytes. + let mut bytes = [0; VERIFY_CHUNK_LENGTH]; + for slot in 0..2 { + for offset in (0..self.flash.slot_size()).step_by(bytes.len()) { + let length = bytes.len().min(self.flash.slot_size() - offset); + self.flash + .read(slot, offset, &mut bytes[..length]) + .map_err(JournalError::Storage)?; + if bytes[..length].iter().any(|&byte| byte != 0xff) { + return Ok(StoreRecovery::Unreadable); + } + } + } + Ok(StoreRecovery::Empty) } fn persist(&mut self, candidate: &C) -> Result<(), Self::Error> { @@ -294,6 +299,8 @@ where self.flash .program_word(target_slot.index(), COMMIT_OFFSET, COMMIT_MARKER) .map_err(JournalError::Storage)?; + // The candidate is now durable even if this final read fails. Callers + // must recover from storage rather than assume an error rolled it back. let Some(record) = self.record(target_slot, candidate)? else { return Err(JournalError::Verification); }; @@ -330,7 +337,8 @@ fn read_u64(bytes: &[u8]) -> u64 { mod tests { use super::*; use crate::{ - ConfigValue, ConfigViolation, FieldDescriptor, FieldId, FieldWriteError, ValueKind, + ConfigManager, ConfigValue, ConfigViolation, FieldDescriptor, FieldId, FieldWriteError, + ReplaceError, ValueKind, }; const VALUE: FieldId = FieldId::new(401); @@ -342,6 +350,8 @@ mod tests { maximum: Some(ConfigValue::U32(100)), enum_options: &[], unit: "ticks", + finite: false, + nonzero: false, }; #[derive(Clone, Copy, Debug, Eq, PartialEq)] @@ -398,15 +408,26 @@ mod tests { #[derive(Clone, Copy, Debug, Eq, PartialEq)] enum FakeError { PowerLoss, + Read, + Program, Bounds, ZeroToOne, } + #[derive(Clone, Copy, Debug)] + enum CommitFailure { + Program, + Read(usize), + CorruptRead(usize), + } + #[derive(Clone)] struct FakeNor { slots: [[u8; 256]; 2], operations: usize, fail_at: Option, + commit_failure: Option, + commit_written: bool, } impl FakeNor { @@ -415,6 +436,8 @@ mod tests { slots: [[0xff; 256]; 2], operations: 0, fail_at: None, + commit_failure: None, + commit_written: false, } } @@ -446,6 +469,13 @@ mod tests { .and_then(|slot| slot.get(offset..offset + output.len())) .ok_or(FakeError::Bounds)?; output.copy_from_slice(source); + if self.commit_written { + match self.commit_failure { + Some(CommitFailure::Read(at)) if at == offset => return Err(FakeError::Read), + Some(CommitFailure::CorruptRead(at)) if at == offset => output[0] ^= 1, + _ => {} + } + } Ok(()) } @@ -479,6 +509,12 @@ mod tests { return Err(FakeError::ZeroToOne); } destination.copy_from_slice(&bytes); + if offset == COMMIT_OFFSET && word == COMMIT_MARKER { + self.commit_written = self.commit_failure.is_some(); + if matches!(self.commit_failure, Some(CommitFailure::Program)) { + return Err(FakeError::Program); + } + } Ok(()) } } @@ -486,6 +522,78 @@ mod tests { type Journal = DualSlotJournal; const FACTORY: Candidate = Candidate(20); + // Borrowing lets a simulated reboot recover the same flash after dropping + // the manager, without adding a production store-extraction API. + impl ConfigStore for &mut Journal { + type Error = JournalError; + + fn load(&mut self, factory: &Candidate) -> Result, Self::Error> { + (**self).load(factory) + } + + fn persist(&mut self, candidate: &Candidate) -> Result<(), Self::Error> { + (**self).persist(candidate) + } + } + + #[test] + fn errors_after_durable_marker_make_committed_unknown_until_reboot() { + let a = Candidate(30); + let b = Candidate(40); + let mut initial = Journal::new(FakeNor::erased()); + initial.persist(&a).unwrap(); + let baseline = initial.into_flash(); + + for failure in [ + CommitFailure::Program, + CommitFailure::Read(0), + CommitFailure::Read(HEADER_LENGTH), + CommitFailure::CorruptRead(0), + CommitFailure::CorruptRead(HEADER_LENGTH), + ] { + let mut flash = baseline.clone(); + flash.commit_failure = Some(failure); + let mut journal = Journal::new(flash); + { + let mut manager = ConfigManager::recover(FACTORY, (), &mut journal).unwrap(); + assert_eq!(manager.active(), &a); + assert_eq!(manager.committed(), Some(&a)); + let expected = match failure { + CommitFailure::Program => JournalError::Storage(FakeError::Program), + CommitFailure::Read(_) => JournalError::Storage(FakeError::Read), + CommitFailure::CorruptRead(_) => JournalError::Verification, + }; + assert_eq!(manager.replace(b), Err(ReplaceError::Store(expected))); + assert_eq!(manager.active(), &a); + assert_eq!(manager.factory(), &FACTORY); + assert_eq!(manager.committed(), None); + assert!(manager.restart_required()); + + // Restoring the pre-error values must not falsely succeed as a + // no-op, and retrying B cannot silently clear the unknown state. + for candidate in [a, b, Candidate(50)] { + assert_eq!( + manager.replace(candidate), + Err(ReplaceError::DurableStateUnknown) + ); + assert_eq!(manager.active(), &a); + assert_eq!(manager.committed(), None); + } + } + + let mut flash = journal.into_flash(); + assert_eq!( + read_u32(&flash.slots[1][COMMIT_OFFSET..COMMIT_OFFSET + 4]), + COMMIT_MARKER + ); + flash.commit_failure = None; + let rebooted = ConfigManager::recover(FACTORY, (), Journal::new(flash)).unwrap(); + assert_eq!(rebooted.active(), &b, "failure {failure:?}"); + assert_eq!(rebooted.committed(), Some(&b)); + assert!(!rebooted.restart_required()); + } + } + #[test] fn alternates_slots_and_recovers_the_newest_record() { let mut journal = Journal::new(FakeNor::erased()); @@ -538,67 +646,4 @@ mod tests { let mut recovered = Journal::new(flash); assert_eq!(recovered.load(&FACTORY).unwrap(), Some(Candidate(30))); } - - #[test] - fn compatibility_filter_can_select_the_older_structurally_valid_slot() { - let mut journal = Journal::new(FakeNor::erased()); - journal.persist(&Candidate(30)).unwrap(); - journal.persist(&Candidate(40)).unwrap(); - - assert_eq!( - journal - .load_matching(&FACTORY, |candidate| candidate.0 <= 35) - .unwrap(), - Some(Candidate(30)) - ); - assert_eq!( - journal - .load_matching(&FACTORY, |candidate| candidate.0 < 10) - .unwrap(), - None - ); - } - - #[test] - fn replacing_an_older_compatible_record_preserves_it_across_every_interruption() { - let mut initial = Journal::new(FakeNor::erased()); - initial.persist(&Candidate(30)).unwrap(); - initial.persist(&Candidate(40)).unwrap(); - let baseline = initial.into_flash(); - - let mut successful = Journal::new(baseline.clone()); - assert_eq!( - successful - .load_matching(&FACTORY, |candidate| candidate.0 <= 35) - .unwrap(), - Some(Candidate(30)) - ); - let start = successful.flash.operations; - successful.persist(&Candidate(35)).unwrap(); - let operation_count = successful.flash.operations - start; - - for interrupted_at in 0..operation_count { - let mut journal = Journal::new(baseline.clone()); - assert_eq!( - journal - .load_matching(&FACTORY, |candidate| candidate.0 <= 35) - .unwrap(), - Some(Candidate(30)) - ); - journal.flash.fail_at = Some(journal.flash.operations + interrupted_at); - assert!(matches!( - journal.persist(&Candidate(35)), - Err(JournalError::Storage(FakeError::PowerLoss)) - )); - - let mut recovered = Journal::new(journal.into_flash()); - assert_eq!( - recovered - .load_matching(&FACTORY, |candidate| candidate.0 <= 35) - .unwrap(), - Some(Candidate(30)), - "mutation {interrupted_at} erased the selected compatible record" - ); - } - } } diff --git a/firmware/libs/kaptan-config/src/lib.rs b/firmware/libs/kaptan-config/src/lib.rs index dd18088..7db187e 100644 --- a/firmware/libs/kaptan-config/src/lib.rs +++ b/firmware/libs/kaptan-config/src/lib.rs @@ -1,11 +1,11 @@ //! Runtime-configuration primitives shared by vehicle-specific schemas. //! -//! This crate has no knowledge of MAVLink, filesystems, board routing, motors, -//! or a particular airframe. It owns the committed-versus-active lifecycle and -//! parameter metadata; vehicle crates provide the typed schema and complete -//! validation. It also owns the canonical public KCFG artifact and the compact -//! private scalar snapshot. Transport and storage framing remain separate -//! adapters. +//! Provides configuration lifecycle, field metadata, KCFG artifacts and storage +//! snapshots. Vehicle crates supply the schema and validation. +//! +//! A persistence error makes the manager's committed view unknown and blocks +//! ordinary saves until reboot recovery or explicit factory reset. The active +//! and factory views remain available; an errored save may become active at boot. #![no_std] @@ -22,9 +22,9 @@ pub mod storage; pub mod validation; pub use artifact::{ - KcfgArtifact, KcfgDecodeError, KcfgEncodeError, KcfgEnumOption, KcfgEnumOptions, - KcfgExportPlan, KcfgExporter, KcfgField, KcfgFields, KcfgImporter, KcfgResultDetail, - decode_kcfg, encode_edited_kcfg, kcfg_length, + KCFG_FIELD_ID_SCRATCH_BYTES, KcfgArtifact, KcfgDecodeError, KcfgEncodeError, KcfgEnumOption, + KcfgEnumOptions, KcfgExportPlan, KcfgExporter, KcfgField, KcfgFields, KcfgImporter, + KcfgResultDetail, KcfgUpdate, decode_kcfg, encode_edited_kcfg, kcfg_length, }; pub use error::{ConfigViolation, ValidationRule}; pub use field::{ @@ -33,10 +33,12 @@ pub use field::{ }; pub use journal::{DualSlotJournal, DualSlotNor, JournalError}; pub use kaptan_config_derive::{Config, ConfigEnum}; -pub use lifecycle::{ConfigManager, ReplaceError, ReplaceOutcome, StartupError}; +pub use lifecycle::{ + ConfigManager, RecoveryDisposition, ReplaceError, ReplaceOutcome, StartupError, +}; pub use snapshot::{ ScalarSnapshotDecodeError, ScalarSnapshotEncodeError, SnapshotModelError, decode_scalar_snapshot, encode_scalar_snapshot, scalar_snapshot_length, }; -pub use storage::ConfigStore; +pub use storage::{ConfigStore, StoreRecovery}; pub use validation::Config; diff --git a/firmware/libs/kaptan-config/src/lifecycle.rs b/firmware/libs/kaptan-config/src/lifecycle.rs index ae3a43f..d9bc9ae 100644 --- a/firmware/libs/kaptan-config/src/lifecycle.rs +++ b/firmware/libs/kaptan-config/src/lifecycle.rs @@ -2,7 +2,7 @@ use kaptan_err_reportable::KaptanError; -use crate::{ConfigModel, ConfigStore, ConfigViolation}; +use crate::{ConfigModel, ConfigStore, ConfigViolation, StoreRecovery}; /// Failure while recovering a boot configuration from a store. #[derive(Debug, KaptanError)] @@ -16,6 +16,23 @@ pub enum StartupError { /// The board's compiled factory candidate violates its immutable context. #[kaptan_error(id = 1103)] InvalidFactory(ConfigViolation), + /// Occupied storage contains no readable candidate for this schema. + #[kaptan_error(id = 1104)] + UnreadablePersisted, + /// The explicit recovery policy supplied an invalid fallback candidate. + #[kaptan_error(id = 1105)] + InvalidRecovery(ConfigViolation), +} + +/// Immutable diagnostic describing the configuration selected at boot. +#[derive(Clone, Copy, Debug, PartialEq)] +pub enum RecoveryDisposition { + /// The selected boot policy accepted a candidate without a recovery diagnostic. + Healthy, + /// The readable committed candidate was retained, but could not be activated. + InvalidPersisted(ConfigViolation), + /// Storage could not establish a readable committed candidate. + StorageUnknown, } /// Result of replacing the complete committed configuration. @@ -31,9 +48,15 @@ pub enum ReplaceError { /// The complete candidate violates a relational or capability invariant. #[kaptan_error(id = 1402)] InvalidCandidate(ConfigViolation), - /// The storage backend could not commit the validated candidate. + /// The storage backend failed; the candidate may already be durable. #[kaptan_error(id = 1403)] Store(E), + /// Durability is unknown; ordinary saving requires recovery or factory reset. + #[kaptan_error(id = 1404)] + DurableStateUnknown, + /// The editor's expected base differs from the current committed values. + #[kaptan_error(id = 1405)] + Conflict, } /// Owns the immutable boot-active configuration and latest durable candidate. @@ -41,12 +64,15 @@ pub enum ReplaceError { /// There is deliberately no in-place activation operation: [`Self::active`] /// never changes after boot. A file endpoint owns its in-progress candidate; /// this manager receives only complete candidates for validation and commit. +/// Any persistence error invalidates the committed view and blocks further +/// ordinary replacements until reboot recovery or an explicit factory reset. pub struct ConfigManager { active: C::Active, factory: C, - committed: C, + committed: Option, context: C::Context, store: S, + diagnostic: RecoveryDisposition, } impl ConfigManager @@ -55,58 +81,69 @@ where S: ConfigStore, { /// Recovers the newest valid committed snapshot or uses the supplied - /// factory candidate when the store is empty. + /// factory candidate when the store is empty. Invalid or unreadable + /// persisted state fails startup rather than selecting factory values. pub fn recover( factory: C, context: C::Context, - mut store: S, + store: S, ) -> Result> { - let factory_active = factory - .validate(&context) - .map_err(StartupError::InvalidFactory)?; - let (active, committed) = match store.load(&factory).map_err(StartupError::Store)? { - Some(committed) => { - let active = committed - .validate(&context) - .map_err(StartupError::InvalidPersisted)?; - (active, committed) - } - None => (factory_active, factory.clone()), - }; - Ok(Self { - active, - factory, - committed, - context, - store, - }) + Self::recover_with(factory, context, store, |_, _, _| None) } - /// Recovers the newest candidate compatible with the current context, - /// falling back to the supplied factory when no compatible record exists. - /// - /// This is an explicit recovery policy for output-inhibited setup images. - /// Safety-critical compositions should use [`Self::recover`] if an - /// incompatible durable record must prevent initialization. - pub fn recover_compatible_or_factory( + /// Explicit setup recovery policy. On invalid or unreadable storage the + /// callback may supply a fallback candidate, which this manager validates + /// before activation. Returning `None` preserves strict startup failure. + /// Readable invalid committed values remain available for repair. Neither + /// fallback activation nor inspection writes anything to storage. + /// The fallback supplies only the active view. It never replaces the + /// committed candidate or turns unknown durable state into known state. + pub fn recover_with( factory: C, context: C::Context, mut store: S, + recovery: impl FnOnce(&C, &C::Context, RecoveryDisposition) -> Option, ) -> Result> { let factory_active = factory .validate(&context) .map_err(StartupError::InvalidFactory)?; - let committed = store - .load_matching(&factory, |candidate| candidate.validate(&context).is_ok()) - .map_err(StartupError::Store)?; - let (active, committed) = match committed { - Some(committed) => { - let active = committed + let (committed, active, diagnostic) = match store.load_recovery(&factory) { + Ok(StoreRecovery::Committed(committed)) => { + let active = committed.validate(&context); + let diagnostic = match active { + Ok(_) => RecoveryDisposition::Healthy, + Err(issue) => RecoveryDisposition::InvalidPersisted(issue), + }; + ( + Some(committed), + active.map_err(StartupError::InvalidPersisted), + diagnostic, + ) + } + Ok(StoreRecovery::Empty) => ( + Some(factory.clone()), + Ok(factory_active), + RecoveryDisposition::Healthy, + ), + Ok(StoreRecovery::Unreadable) => ( + None, + Err(StartupError::UnreadablePersisted), + RecoveryDisposition::StorageUnknown, + ), + Err(error) => ( + None, + Err(StartupError::Store(error)), + RecoveryDisposition::StorageUnknown, + ), + }; + let active = match active { + Ok(active) => active, + Err(error) => { + let fallback = recovery(&factory, &context, diagnostic).ok_or(error)?; + fallback .validate(&context) - .map_err(StartupError::InvalidPersisted)?; - (active, committed) + .map_err(StartupError::InvalidRecovery)? } - None => (factory_active, factory.clone()), }; Ok(Self { active, @@ -114,6 +151,7 @@ where committed, context, store, + diagnostic, }) } @@ -122,9 +160,12 @@ where &self.active } - /// Returns the complete last durably committed candidate. - pub const fn committed(&self) -> &C { - &self.committed + /// Returns the readable committed candidate (possibly invalid), or the + /// empty-store factory selected at boot. Returns `None` when boot could not + /// establish durable state or after any persistence error, since the + /// attempted replacement may already be durable. + pub const fn committed(&self) -> Option<&C> { + self.committed.as_ref() } /// Returns the complete board/image factory candidate. @@ -132,28 +173,76 @@ where &self.factory } - /// Whether committed values differ from the immutable boot-active view. + /// Returns the boot recovery disposition; saving does not change boot state. + pub const fn diagnostic(&self) -> RecoveryDisposition { + self.diagnostic + } + + /// Whether every candidate scalar exactly matches the factory, including + /// native floating-point bits (so positive and negative zero differ). + pub fn is_factory(&self, candidate: &C) -> bool { + same_values(candidate, &self.factory) + } + + /// Whether committed values differ from the immutable boot-active view, + /// or reboot recovery is required because the durable state is unknown. pub fn restart_required(&self) -> bool { - !active_matches_candidate::(&self.active, &self.committed) + self.committed + .as_ref() + .is_none_or(|committed| !active_matches_candidate::(&self.active, committed)) } /// Validates and atomically persists one complete replacement candidate. /// /// An unchanged candidate does not consume a flash erase/write cycle. The /// boot-active view remains immutable until a physical restart. + /// After any persistence error, all replacements (including unchanged + /// candidates) fail with [`ReplaceError::DurableStateUnknown`] until reset + /// or reboot recovery. pub fn replace(&mut self, candidate: C) -> Result> { + let committed = self + .committed + .as_ref() + .ok_or(ReplaceError::DurableStateUnknown)?; candidate .validate(&self.context) .map_err(ReplaceError::InvalidCandidate)?; - if same_values(&candidate, &self.committed) { + if same_values(&candidate, committed) { return Ok(ReplaceOutcome { restart_required: self.restart_required(), }); } - self.store - .persist(&candidate) - .map_err(ReplaceError::Store)?; - self.committed = candidate; + self.persist(candidate) + } + + /// Guarded full replacement. The expected base is compared bit-for-bit + /// before validation, no-op detection, or any persistence attempt. + pub fn replace_if_current( + &mut self, + candidate: C, + expected_base: &C, + ) -> Result> { + let committed = self.committed().ok_or(ReplaceError::DurableStateUnknown)?; + if !same_values(expected_base, committed) { + return Err(ReplaceError::Conflict); + } + self.replace(candidate) + } + + /// Explicit recovery action: always persists the validated factory, even + /// when durability is unknown or values appear unchanged. Active remains + /// immutable. A failed reset leaves durability unknown; success reopens + /// ordinary guarded saves. + pub fn reset_to_factory(&mut self) -> Result> { + self.persist(self.factory.clone()) + } + + fn persist(&mut self, candidate: C) -> Result> { + if let Err(error) = self.store.persist(&candidate) { + self.committed = None; + return Err(ReplaceError::Store(error)); + } + self.committed = Some(candidate); let restart_required = self.restart_required(); if restart_required { kaptan_log::info!("configuration saved; restart required"); @@ -175,7 +264,7 @@ fn same_values(left: &C, right: &C) -> bool { ) else { return false; }; - if left.kind() != right.kind() || left.raw_bits() != right.raw_bits() { + if left != right { return false; } } @@ -193,7 +282,7 @@ fn active_matches_candidate(active: &C::Active, candidate: &C) - ) else { return false; }; - if active.kind() != candidate.kind() || active.raw_bits() != candidate.raw_bits() { + if active != candidate { return false; } } @@ -220,6 +309,8 @@ mod tests { maximum: Some(ConfigValue::U8(10)), enum_options: &[], unit: "count", + finite: false, + nonzero: false, }, FieldDescriptor { id: OTHER, @@ -229,6 +320,8 @@ mod tests { maximum: Some(ConfigValue::U8(10)), enum_options: &[], unit: "count", + finite: false, + nonzero: false, }, ]; @@ -319,9 +412,24 @@ mod tests { } } - struct IncompatibleStore; + struct FailingStore<'a>(&'a Cell); - impl ConfigStore for IncompatibleStore { + impl ConfigStore for FailingStore<'_> { + type Error = (); + + fn load(&mut self, _: &Candidate) -> Result, Self::Error> { + Ok(None) + } + + fn persist(&mut self, _: &Candidate) -> Result<(), Self::Error> { + self.0.set(self.0.get() + 1); + Err(()) + } + } + + struct IncompatibleStore<'a>(&'a Cell); + + impl ConfigStore for IncompatibleStore<'_> { type Error = (); fn load(&mut self, _: &Candidate) -> Result, Self::Error> { @@ -329,6 +437,7 @@ mod tests { } fn persist(&mut self, _: &Candidate) -> Result<(), Self::Error> { + self.0.set(self.0.get() + 1); Ok(()) } } @@ -362,7 +471,7 @@ mod tests { ValidationRule::Inconsistent, ))) ); - assert_eq!(manager.committed(), &factory); + assert_eq!(manager.committed(), Some(&factory)); assert_eq!(writes.get(), 0); assert_eq!( @@ -396,22 +505,147 @@ mod tests { restart_required: false, }) ); - assert_eq!(manager.committed(), &factory); + assert_eq!(manager.committed(), Some(&factory)); assert_eq!(manager.factory(), &factory); assert_eq!(writes.get(), 2); } #[test] - fn explicit_recovery_policy_uses_factory_when_no_record_matches_context() { + fn any_persistence_error_blocks_all_replacements_until_recovery() { + use kaptan_err_reportable::ReportableError; + + let attempts = Cell::new(0); let factory = Candidate { value: 2, other: 4 }; + let mut manager = + ConfigManager::recover(factory.clone(), (), FailingStore(&attempts)).unwrap(); + assert_eq!(manager.committed(), Some(&factory)); + assert!(!manager.restart_required()); + + assert_eq!( + manager.replace(Candidate { value: 3, other: 4 }), + Err(ReplaceError::Store(())) + ); + assert_eq!(manager.committed(), None); + assert_eq!(manager.active(), &Active { value: 2, other: 4 }); + assert_eq!(manager.factory(), &factory); + assert!(manager.restart_required()); + + for candidate in [ + factory, + Candidate { value: 3, other: 4 }, + Candidate { value: 4, other: 4 }, + Candidate { value: 5, other: 4 }, + ] { + assert_eq!( + manager.replace(candidate), + Err(ReplaceError::DurableStateUnknown) + ); + assert_eq!(manager.committed(), None); + } + assert_eq!(attempts.get(), 1); + assert_eq!( + ReplaceError::<()>::DurableStateUnknown.error_id().get(), + 1404 + ); + } + + #[test] + fn explicit_fallback_retains_invalid_committed_values_and_validates_recovery() { + let factory = Candidate { value: 2, other: 4 }; + let invalid = Candidate { value: 9, other: 1 }; + let writes = Cell::new(0); assert!(matches!( - ConfigManager::recover(factory.clone(), (), IncompatibleStore), + ConfigManager::recover(factory.clone(), (), IncompatibleStore(&writes)), Err(StartupError::InvalidPersisted(_)) )); - - let manager = - ConfigManager::recover_compatible_or_factory(factory, (), IncompatibleStore).unwrap(); + let mut manager = ConfigManager::recover_with( + factory.clone(), + (), + IncompatibleStore(&writes), + |factory, _, disposition| { + assert!(matches!( + disposition, + RecoveryDisposition::InvalidPersisted(_) + )); + Some(factory.clone()) + }, + ) + .unwrap(); + assert_eq!(manager.committed(), Some(&invalid)); assert_eq!(manager.active(), &Active { value: 2, other: 4 }); + assert_eq!(writes.get(), 0); + assert_eq!( + manager.replace_if_current(factory.clone(), &factory), + Err(ReplaceError::Conflict) + ); + assert_eq!(writes.get(), 0); + manager + .replace_if_current(factory.clone(), &invalid) + .unwrap(); + assert_eq!(writes.get(), 1, "factory repair must persist, not no-op"); + assert_eq!(manager.committed(), Some(&factory)); + assert!(matches!( + manager.diagnostic(), + RecoveryDisposition::InvalidPersisted(_) + )); + assert!(matches!( + ConfigManager::recover_with(factory, (), IncompatibleStore(&writes), |_, _, _| { + Some(invalid) + }), + Err(StartupError::InvalidRecovery(_)) + )); + } + + #[test] + fn factory_reset_is_explicit_forced_and_preserves_active_and_uncertainty_on_error() { + struct Store<'a> { + attempts: &'a Cell, + fail: &'a Cell, + } + impl ConfigStore for Store<'_> { + type Error = (); + fn load(&mut self, _: &Candidate) -> Result, ()> { + Ok(Some(Candidate { value: 3, other: 4 })) + } + fn persist(&mut self, _: &Candidate) -> Result<(), ()> { + self.attempts.set(self.attempts.get() + 1); + if self.fail.get() { Err(()) } else { Ok(()) } + } + } + let attempts = Cell::new(0); + let fail = Cell::new(true); + let factory = Candidate { value: 2, other: 4 }; + let mut manager = ConfigManager::recover( + factory.clone(), + (), + Store { + attempts: &attempts, + fail: &fail, + }, + ) + .unwrap(); + assert_eq!( + manager.replace(factory.clone()), + Err(ReplaceError::Store(())) + ); + assert_eq!( + manager.replace_if_current(factory.clone(), &factory), + Err(ReplaceError::DurableStateUnknown) + ); + assert_eq!(attempts.get(), 1); + assert_eq!(manager.reset_to_factory(), Err(ReplaceError::Store(()))); + assert_eq!(manager.committed(), None); + fail.set(false); + assert!(manager.reset_to_factory().unwrap().restart_required); + assert_eq!(attempts.get(), 3); + assert_eq!(manager.committed(), Some(&factory)); + assert_eq!(manager.active(), &Active { value: 3, other: 4 }); + manager + .replace_if_current(factory.clone(), &factory) + .unwrap(); + assert_eq!(attempts.get(), 3, "same-base no-op must skip flash"); + manager.reset_to_factory().unwrap(); + assert_eq!(attempts.get(), 4, "explicit reset must always persist"); } #[test] @@ -422,7 +656,9 @@ mod tests { Err(StartupError::InvalidFactory(_)) )); assert!(matches!( - ConfigManager::recover_compatible_or_factory(invalid_factory, (), ValidStore), + ConfigManager::recover_with(invalid_factory, (), ValidStore, |_, _, _| { + panic!("invalid factory must fail before the recovery callback") + }), Err(StartupError::InvalidFactory(_)) )); } diff --git a/firmware/libs/kaptan-config/src/snapshot.rs b/firmware/libs/kaptan-config/src/snapshot.rs index 5013aed..78b0b6a 100644 --- a/firmware/libs/kaptan-config/src/snapshot.rs +++ b/firmware/libs/kaptan-config/src/snapshot.rs @@ -12,9 +12,7 @@ //! field_id:u16 | value_kind:u8 | value:[value_kind.width] //! ``` //! -//! Field entries are emitted and decoded in descriptor order. The schema -//! fingerprint already fixes that order, so accepting permutations would add -//! work without adding a compatible persistence state. +//! Field entries follow descriptor order, as fixed by the schema fingerprint. use kaptan_err_reportable::KaptanError; @@ -38,12 +36,15 @@ pub(crate) const SCALAR_SNAPSHOT_ENTRY_PREFIX_LENGTH: usize = 3; /// Returns the exact encoded length for a schema's field and value widths. /// -/// The `u16` field-count domain makes this arithmetic safe on all targets -/// supported by this crate, including 32-bit microcontrollers. +/// `value_bytes` must sum the native widths of all fields, each 1 through 8 +/// bytes. Panics if that total is outside the range allowed by `field_count`. pub const fn scalar_snapshot_length(field_count: u16, value_bytes: usize) -> usize { - SCALAR_SNAPSHOT_HEADER_LENGTH - + field_count as usize * SCALAR_SNAPSHOT_ENTRY_PREFIX_LENGTH - + value_bytes + let count = field_count as usize; + assert!( + value_bytes >= count && value_bytes <= count * ValueKind::F64.width(), + "snapshot value bytes do not match the field-count domain" + ); + SCALAR_SNAPSHOT_HEADER_LENGTH + count * SCALAR_SNAPSHOT_ENTRY_PREFIX_LENGTH + value_bytes } /// A broken [`ConfigModel`] registry detected before encoding or decoding. @@ -225,11 +226,8 @@ pub fn encode_scalar_snapshot<'a, C: ConfigModel>( /// Decodes a complete scalar snapshot into a clone of `factory`. /// -/// Structural validation completes before any field setter runs. Each write -/// then passes through [`ConfigModel::set_field`], preserving the model's local -/// validation. Whole-candidate capability validation remains the composition -/// owner's responsibility because its context is deliberately not part of a -/// storage codec. +/// Checks structure before applying writes through [`ConfigModel::set_field`]. +/// The caller must validate whole-candidate relations and capabilities. pub fn decode_scalar_snapshot( input: &[u8], factory: &C, @@ -378,6 +376,8 @@ fn read_u16(bytes: &[u8]) -> u16 { #[cfg(test)] mod tests { + extern crate std; + use super::*; use crate::{ConfigViolation, FieldDescriptor, ValidationRule}; @@ -403,6 +403,15 @@ mod tests { descriptor(U16, ValueKind::U16), ]; + #[test] + fn snapshot_length_bounds_raw_aggregate_widths() { + assert_eq!(scalar_snapshot_length(0, 0), 23); + assert_eq!(scalar_snapshot_length(u16::MAX, 524_280), 720_908); + for (count, bytes) in [(0, 1), (1, 0), (1, 9), (u16::MAX, usize::MAX)] { + assert!(std::panic::catch_unwind(|| scalar_snapshot_length(count, bytes)).is_err()); + } + } + const fn descriptor(id: FieldId, kind: ValueKind) -> FieldDescriptor { FieldDescriptor { id, @@ -412,6 +421,8 @@ mod tests { maximum: None, enum_options: &[], unit: "", + finite: false, + nonzero: false, } } @@ -682,8 +693,7 @@ mod tests { }) ); - // Every numeric kind now occupies its native width, so unlike the old - // widened u64 representation it cannot carry out-of-width high bits. + // Native-width encodings cannot carry out-of-width high bits. } #[test] diff --git a/firmware/libs/kaptan-config/src/storage.rs b/firmware/libs/kaptan-config/src/storage.rs index 337f616..552de84 100644 --- a/firmware/libs/kaptan-config/src/storage.rs +++ b/firmware/libs/kaptan-config/src/storage.rs @@ -1,5 +1,15 @@ //! Persistence interface for complete validated candidate snapshots. +/// Storage recovery without conflating erased storage with unreadable records. +pub enum StoreRecovery { + /// No configuration has been stored. + Empty, + /// The newest readable candidate; relational validation is the caller's job. + Committed(C), + /// Storage is occupied, but no candidate can be decoded by this schema. + Unreadable, +} + /// Durable storage of complete configuration snapshots. /// /// Flight implementations use a two-slot journal: a record contains a header, @@ -14,20 +24,21 @@ pub trait ConfigStore { /// Loads the newest valid committed candidate, if one exists. fn load(&mut self, factory: &C) -> Result, Self::Error>; - /// Loads the newest structurally valid candidate accepted by `compatible`. - /// - /// Single-record stores inherit this implementation. Journals should - /// override it so an incompatible newest record can fall back to an older - /// compatible slot without weakening structural integrity checks. - fn load_matching(&mut self, factory: &C, mut compatible: F) -> Result, Self::Error> - where - F: FnMut(&C) -> bool, - { - Ok(self - .load(factory)? - .filter(|candidate| compatible(candidate))) + /// Recovers a candidate while distinguishing empty from undecodable storage. + /// Stores whose `load(None)` can mean occupied storage must override this. + fn load_recovery(&mut self, factory: &C) -> Result, Self::Error> { + Ok(match self.load(factory)? { + Some(candidate) => StoreRecovery::Committed(candidate), + None => StoreRecovery::Empty, + }) } /// Persists one complete already-validated candidate atomically. + /// + /// Success confirms that the candidate is durable. An error does not imply + /// rollback: the commit marker may already be durable when a later write + /// acknowledgement or verification read fails. Callers must treat durable + /// state as unknown after any error until recovery reloads it from storage + /// or an explicit factory reset successfully establishes a new record. fn persist(&mut self, candidate: &C) -> Result<(), Self::Error>; } diff --git a/firmware/libs/kaptan-config/src/validation.rs b/firmware/libs/kaptan-config/src/validation.rs index 1f7a7a0..fdc0d33 100644 --- a/firmware/libs/kaptan-config/src/validation.rs +++ b/firmware/libs/kaptan-config/src/validation.rs @@ -14,7 +14,8 @@ pub trait Config { /// Exact value bytes in the flattened private snapshot. const SNAPSHOT_VALUE_BYTES: usize; - /// Fingerprint generated from the complete declared schema. + /// Fingerprint of declared scalar metadata and ordered child fingerprints. + /// Post-validator code and runtime capability context are not hashed. const SCHEMA_ID: [u8; 16]; /// Returns a descriptor from the flattened declaration order. @@ -85,19 +86,20 @@ pub fn __valid_enum_options(descriptors: &[FieldDescriptor]) -> bool { if descriptor.enum_options.len() > crate::field::MAX_ENUM_OPTIONS_PER_FIELD { return false; } - for (index, option) in descriptor.enum_options.iter().enumerate() { + let mut seen = [0_u8; 32]; + for option in descriptor.enum_options { if option.1.is_empty() || option.1.len() > crate::field::MAX_ENUM_OPTION_LABEL_BYTES || descriptor.kind != crate::ValueKind::U8 { return false; } - if descriptor.enum_options[..index] - .iter() - .any(|previous| previous.0 == option.0) - { + let byte = usize::from(option.0 / 8); + let bit = 1 << (option.0 % 8); + if seen[byte] & bit != 0 { return false; } + seen[byte] |= bit; } } true @@ -173,6 +175,8 @@ pub const fn __schema_id(descriptors: &[FieldDescriptor]) -> [u8; 16] { option += 1; } state = hash_str(state, descriptor.unit); + state = hash_byte(state, descriptor.finite as u8); + state = hash_byte(state, descriptor.nonzero as u8); index += 1; } finish_fingerprint(state) diff --git a/firmware/libs/kaptan-config/tests/transactions.rs b/firmware/libs/kaptan-config/tests/transactions.rs new file mode 100644 index 0000000..1800eb2 --- /dev/null +++ b/firmware/libs/kaptan-config/tests/transactions.rs @@ -0,0 +1,318 @@ +use core::cell::Cell; +use kaptan_config::{ + ConfigManager, ConfigModel, ConfigStore, ConfigValue, ConfigViolation, FieldDescriptor, + FieldId, FieldWriteError, KcfgDecodeError, KcfgExporter, KcfgImporter, KcfgUpdate, + ReplaceError, ValidationRule, decode_kcfg, encode_edited_kcfg, +}; + +#[derive(Clone, Debug, PartialEq, kaptan_config::Config)] +struct Scalars { + #[config(id = 1, key = "test.u8", unit = "")] + u8_value: u8, + #[config(id = 2, key = "test.u16", unit = "")] + u16_value: u16, + #[config(id = 3, key = "test.u32", unit = "")] + u32_value: u32, + #[config(id = 4, key = "test.u64", unit = "")] + u64_value: u64, + #[config(id = 7, key = "test.i32", unit = "")] + i32_value: i32, + #[config(id = 9, key = "test.f32", unit = "")] + f32_value: f32, + #[config(id = 10, key = "test.f64", unit = "")] + f64_value: f64, + #[config(id = 11, key = "test.bool", unit = "")] + bool_value: bool, + #[config(id = 12, key = "test.finite", unit = "", finite)] + finite: f32, + #[config(id = 13, key = "test.nonzero", unit = "", nonzero)] + nonzero: f64, +} + +impl ConfigModel for Scalars { + type Active = Self; + type Context = (); + const SCHEMA_ID: [u8; 16] = ::SCHEMA_ID; + fn descriptor(index: u16) -> Option<&'static FieldDescriptor> { + ::descriptor(usize::from(index)) + } + fn field_count() -> u16 { + ::FIELD_COUNT as u16 + } + fn field_value(&self, id: FieldId) -> Option { + ::field_value(self, id) + } + fn active_field_value(active: &Self::Active, id: FieldId) -> Option { + active.field_value(id) + } + fn set_field(&mut self, id: FieldId, value: ConfigValue) -> Result<(), FieldWriteError> { + ::set_field(self, id, value) + } + fn validate(&self, _: &()) -> Result { + ::validate(self)?; + Ok(self.clone()) + } +} + +fn factory() -> Scalars { + Scalars { + u8_value: u8::MAX, + u16_value: u16::MAX, + u32_value: u32::MAX, + u64_value: u64::MAX, + i32_value: i32::MIN, + f32_value: 0.0, + f64_value: 0.0, + bool_value: true, + finite: 1.0, + nonzero: 1.0, + } +} + +struct Store<'a>(&'a Cell); +impl ConfigStore for Store<'_> { + type Error = (); + fn load(&mut self, _: &Scalars) -> Result, ()> { + Ok(None) + } + fn persist(&mut self, _: &Scalars) -> Result<(), ()> { + self.0.set(self.0.get() + 1); + Ok(()) + } +} + +fn export(current: &Scalars, factory: &Scalars) -> Vec { + let exporter = KcfgExporter::new(current, factory, false).unwrap(); + let mut bytes = vec![0; exporter.length()]; + exporter.read(0, &mut bytes).unwrap(); + bytes +} + +fn edit(bytes: &[u8], candidate: &Scalars) -> Vec { + let source = decode_kcfg(bytes, &mut [0; kaptan_config::KCFG_FIELD_ID_SCRATCH_BYTES]).unwrap(); + let mut edited = vec![0; bytes.len()]; + encode_edited_kcfg(&source, false, &mut edited, |field| { + candidate.field_value(field.id()).unwrap() + }) + .unwrap(); + edited +} + +fn import(bytes: &[u8], factory: &Scalars) -> KcfgUpdate { + let mut importer = KcfgImporter::new(factory).unwrap(); + for byte in bytes { + importer.push(factory, core::slice::from_ref(byte)).unwrap(); + } + importer.finish().unwrap() +} + +fn assert_exact(actual: ConfigValue, expected: ConfigValue) { + match (actual, expected) { + (ConfigValue::F32(a), ConfigValue::F32(b)) => assert_eq!(a.to_bits(), b.to_bits()), + (ConfigValue::F64(a), ConfigValue::F64(b)) => assert_eq!(a.to_bits(), b.to_bits()), + _ => assert_eq!(actual, expected), + } +} + +#[test] +fn independent_editors_compare_the_downloaded_base_before_even_noop_or_validation() { + let a = factory(); + let writes = Cell::new(0); + let mut manager = ConfigManager::recover(a.clone(), (), Store(&writes)).unwrap(); + let editor_a = export(manager.committed().unwrap(), &a); + let editor_b = export(manager.committed().unwrap(), &a); + let mut b = a.clone(); + b.u64_value -= 1; // Above the exact-integer range of floating-point transports. + b.f32_value = -0.0; + b.f64_value = -0.0; + let update = import(&edit(&editor_b, &b), &a); + assert!( + manager + .replace_if_current(update.candidate, &update.expected_base) + .unwrap() + .restart_required + ); + assert_eq!(writes.get(), 1); + + // Editing an already-edited draft must not rebase it. Even a requested + // value set exactly equal to current B is stale when its base was A. + let draft = edit(&edit(&editor_a, &b), &b); + let update = import(&draft, &a); + assert_eq!( + manager.replace_if_current(update.candidate, &update.expected_base), + Err(ReplaceError::Conflict) + ); + let update = import(&editor_a, &a); + assert_eq!( + manager.replace_if_current(update.candidate, &update.expected_base), + Err(ReplaceError::Conflict) + ); + let mut invalid = b.clone(); + invalid.finite = f32::NAN; + let violation = ConfigViolation::field(FieldId::new(12), ValidationRule::NonFinite); + assert_eq!(invalid.validate(&()).unwrap_err(), violation); + assert_eq!( + manager.replace_if_current(invalid.clone(), &a), + Err(ReplaceError::Conflict) + ); + assert_eq!( + manager.replace_if_current(invalid, &b), + Err(ReplaceError::InvalidCandidate(violation)) + ); + assert_eq!(writes.get(), 1); + + let fresh = import(&export(manager.committed().unwrap(), &a), &a); + manager + .replace_if_current(fresh.candidate, &fresh.expected_base) + .unwrap(); + assert_eq!(writes.get(), 1, "same-base no-op does not consume flash"); + assert_eq!(manager.active().f32_value.to_bits(), 0.0_f32.to_bits()); + assert_eq!( + manager.committed().unwrap().f32_value.to_bits(), + (-0.0_f32).to_bits() + ); + assert!(!manager.is_factory(&b)); + assert!(!manager.replace_if_current(a, &b).unwrap().restart_required); + assert_eq!(writes.get(), 2); +} + +#[test] +fn v3_preserves_all_native_widths_and_signed_zero_in_both_value_sets() { + let factory = factory(); + let mut current = factory.clone(); + current.f32_value = -0.0; + current.f64_value = -0.0; + let bytes = export(¤t, &factory); + assert_eq!(bytes[4], 3); + let artifact = + decode_kcfg(&bytes, &mut [0; kaptan_config::KCFG_FIELD_ID_SCRATCH_BYTES]).unwrap(); + assert_eq!(artifact.schema_id(), Scalars::SCHEMA_ID); + for field in artifact.fields() { + assert_eq!(field.finite(), field.id() == FieldId::new(12)); + assert_eq!(field.nonzero(), field.id() == FieldId::new(13)); + let expected = current.field_value(field.id()).unwrap(); + assert_exact(field.value(), expected); + assert_exact(field.expected_base_value(), expected); + } + let update = import(&edit(&bytes, &factory), &factory); + for index in 0..Scalars::field_count() { + let id = Scalars::descriptor(index).unwrap().id; + assert_exact( + update.candidate.field_value(id).unwrap(), + factory.field_value(id).unwrap(), + ); + assert_exact( + update.expected_base.field_value(id).unwrap(), + current.field_value(id).unwrap(), + ); + } + // 24 header + 4 CRC + 70 prefix + 132 value + 87 key bytes. + assert_eq!(bytes.len(), 317); + + let mut importer = KcfgImporter::new(&factory).unwrap(); + assert_eq!( + importer.push(&factory, &export(¤t, ¤t)), + Err(KcfgDecodeError::FactoryMismatch(FieldId::new(9))) + ); + + // Base checks, no-op checks, restart checks, and factory checks must all + // distinguish signed zero even when every other scalar matches. + for id in [FieldId::new(9), FieldId::new(10)] { + let writes = Cell::new(0); + let mut manager = ConfigManager::recover(factory.clone(), (), Store(&writes)).unwrap(); + let mut negative_zero = factory.clone(); + negative_zero + .set_field(id, current.field_value(id).unwrap()) + .unwrap(); + assert!(!manager.is_factory(&negative_zero)); + assert_eq!( + manager.replace_if_current(factory.clone(), &negative_zero), + Err(ReplaceError::Conflict) + ); + assert!( + manager + .replace_if_current(negative_zero.clone(), &factory) + .unwrap() + .restart_required + ); + assert_eq!(writes.get(), 1); + assert_eq!( + manager.replace_if_current(negative_zero, &factory), + Err(ReplaceError::Conflict) + ); + } +} + +#[test] +fn nonfinite_payload_identity_survives_artifacts_cas_noops_and_factory_checks() { + for (single, double) in [ + (0x7f80_0000, 0xfff0_0000_0000_0000), // infinities + (0x7fc0_0042, 0xfff8_0000_0000_0042), // quiet payloads + (0xff80_0042, 0x7ff0_0000_0000_0042), // signaling payloads + ] { + let mut factory = factory(); + factory.f32_value = f32::from_bits(single); + factory.f64_value = f64::from_bits(double); + let writes = Cell::new(0); + let mut manager = ConfigManager::recover(factory.clone(), (), Store(&writes)).unwrap(); + let bytes = export(&factory, &factory); + let update = import(&bytes, &factory); + assert!(manager.is_factory(&update.candidate)); + assert!( + !manager + .replace_if_current(update.candidate, &update.expected_base) + .unwrap() + .restart_required + ); + assert_eq!(writes.get(), 0); + + let mut other = factory.clone(); + other.f64_value = f64::from_bits(double ^ 1); + assert!(!manager.is_factory(&other)); + assert_eq!( + manager.replace_if_current(factory.clone(), &other), + Err(ReplaceError::Conflict) + ); + assert!( + manager + .replace_if_current(other.clone(), &factory) + .unwrap() + .restart_required + ); + assert_eq!(writes.get(), 1); + let update = import(&export(&other, &factory), &factory); + assert_eq!( + update.candidate.field_value(FieldId::new(10)), + Some(ConfigValue::F64(other.f64_value)) + ); + } + assert_ne!(ConfigValue::U32(0), ConfigValue::F32(0.0)); +} + +#[test] +fn streaming_import_checks_rule_flags_and_rejects_the_unshipped_old_format() { + let factory = factory(); + let bytes = export(&factory, &factory); + let mut old = bytes.clone(); + old[4] = 2; + let mut importer = KcfgImporter::new(&factory).unwrap(); + assert_eq!( + importer.push(&factory, &old), + Err(KcfgDecodeError::UnsupportedVersion(2)) + ); + + let mut offset = 24; + for index in 0..Scalars::field_count() { + let field = Scalars::descriptor(index).unwrap(); + if field.finite || field.nonzero { + let mut edited = bytes.clone(); + edited[offset + 3] ^= if field.finite { 4 } else { 8 }; + let mut importer = KcfgImporter::new(&factory).unwrap(); + assert_eq!( + importer.push(&factory, &edited), + Err(KcfgDecodeError::DescriptorMismatch(field.id)) + ); + } + offset += 7 + field.kind.width() * 3 + field.display_key.len() + field.unit.len(); + } +} diff --git a/firmware/libs/kaptan-control/src/kani_proofs.rs b/firmware/libs/kaptan-control/src/kani_proofs.rs new file mode 100644 index 0000000..ec24d69 --- /dev/null +++ b/firmware/libs/kaptan-control/src/kani_proofs.rs @@ -0,0 +1,95 @@ +//! Constructor and one-step inductive PID invariants over all f32 bit patterns. + +use super::{MAXIMUM_CONTROL_STEP_SECONDS, Pid, PidConfig, PidError}; + +fn any_config() -> PidConfig { + PidConfig { + proportional: f32::from_bits(kani::any()), + integral: f32::from_bits(kani::any()), + derivative: f32::from_bits(kani::any()), + output_limit: f32::from_bits(kani::any()), + integral_limit: f32::from_bits(kani::any()), + } +} + +fn bits(pid: Pid) -> [u32; 6] { + [ + pid.config.proportional.to_bits(), + pid.config.integral.to_bits(), + pid.config.derivative.to_bits(), + pid.config.output_limit.to_bits(), + pid.config.integral_limit.to_bits(), + pid.integral_contribution.to_bits(), + ] +} + +#[kani::proof] +// The generated equality check compares up to 24 bytes through memcmp. +#[kani::unwind(25)] +fn pid_constructor_establishes_invariant() { + let config = any_config(); + let result = Pid::new(config); + let valid_gains = [config.proportional, config.integral, config.derivative] + .into_iter() + .all(|gain| gain.is_finite() && gain >= 0.0); + let valid_limits = config.output_limit.is_finite() + && config.output_limit > 0.0 + && config.integral_limit.is_finite() + && config.integral_limit >= 0.0; + + assert_eq!(result.is_ok(), valid_gains && valid_limits); + if let Ok(pid) = result { + assert_eq!(pid.integral_contribution.to_bits(), 0.0_f32.to_bits()); + // Replace the returned configuration with the original input to check + // exact preservation, including the sign of zero-valued gains. + assert_eq!(bits(pid)[..5], bits(Pid { config, ..pid })[..5]); + } + kani::cover!(result.is_ok()); + kani::cover!(result.is_err()); + kani::cover!(config.proportional.is_nan()); +} + +#[kani::proof] +#[kani::unwind(25)] +fn pid_update_preserves_bounds_or_state() { + let Ok(mut pid) = Pid::new(any_config()) else { + return; + }; + + // The constructor establishes zero. This step permits every finite retained + // integral within its limit, a superset of all reachable PID histories. + let initial = f32::from_bits(kani::any()); + kani::assume(initial.is_finite()); + kani::assume(initial.abs() <= pid.config.integral_limit); + pid.integral_contribution = initial; + let before = bits(pid); + + let error = f32::from_bits(kani::any()); + let error_rate = f32::from_bits(kani::any()); + let dt_seconds = f32::from_bits(kani::any()); + let result = pid.update(error, error_rate, dt_seconds); + + if let Ok(output) = result { + assert!(output.is_finite()); + assert!(output.abs() <= pid.config.output_limit); + assert!(pid.integral_contribution.is_finite()); + assert!(pid.integral_contribution.abs() <= pid.config.integral_limit); + assert_eq!(bits(pid)[..5], before[..5]); + } else { + assert_eq!(bits(pid), before); + } + kani::cover!(result.is_ok() && pid.integral_contribution != initial); + kani::cover!(result.is_ok_and(|output| output.abs() == pid.config.output_limit)); + kani::cover!( + result == Err(PidError::NonFinite) + && error.is_finite() + && error_rate.is_finite() + && dt_seconds.is_finite() + && dt_seconds > 0.0 + && dt_seconds <= MAXIMUM_CONTROL_STEP_SECONDS + ); + + pid.reset(); + assert_eq!(pid.integral_contribution.to_bits(), 0.0_f32.to_bits()); + assert_eq!(bits(pid)[..5], before[..5]); +} diff --git a/firmware/libs/kaptan-control/src/lib.rs b/firmware/libs/kaptan-control/src/lib.rs index df4ba78..d16c7fe 100644 --- a/firmware/libs/kaptan-control/src/lib.rs +++ b/firmware/libs/kaptan-control/src/lib.rs @@ -6,10 +6,14 @@ //! saturation, and slew limiting. #![no_std] +#![forbid(unsafe_code)] use kaptan_err_reportable::KaptanError; use kaptan_math::{Frd, Ned, UnitQuaternion, Vector3, sqrt}; +#[cfg(kani)] +mod kani_proofs; + /// Body-frame angular rate in radians per second. #[derive(Clone, Copy, Debug, PartialEq)] pub struct BodyRate(Vector3); @@ -65,8 +69,8 @@ pub enum AttitudeError { } /// Computes a body-to-NED attitude whose body-down axis opposes multicopter -/// thrust force and whose body-forward axis follows the requested heading as -/// closely as geometry allows. +/// thrust force and whose body-forward horizontal azimuth is the requested +/// heading. The heading is defined only in the upright thrust envelope. pub fn force_to_attitude( force: NedForce, heading: HeadingNed, @@ -76,19 +80,26 @@ pub fn force_to_attitude( .scale(-1.0) .normalized() .ok_or(AttitudeError::ZeroForce)?; - let body_right = body_down - .cross(heading.vector()) + if body_down.z() <= 0.0 { + return Err(AttitudeError::SingularHeading); + } + let desired = heading.vector(); + let horizontal_lateral = Vector3::new(-desired.y(), desired.x(), 0.0); + let body_forward = horizontal_lateral + .cross(body_down) .normalized() .ok_or(AttitudeError::SingularHeading)?; - let body_forward = body_right - .cross(body_down) + let body_right = body_down + .cross(body_forward) .normalized() .ok_or(AttitudeError::SingularHeading)?; UnitQuaternion::from_body_axes_in_ned(body_forward, body_right, body_down) .map_err(|_| AttitudeError::SingularHeading) } -/// Bounded scalar PID configuration. +/// Scalar PID configuration with nonnegative gains and bounded output/integral. +/// Input errors have no configured range. Finite inputs whose arithmetic +/// overflows are rejected atomically rather than disguised by saturation. #[derive(Clone, Copy, Debug, PartialEq)] pub struct PidConfig { /// Proportional gain. @@ -106,13 +117,13 @@ pub struct PidConfig { /// Invalid PID input/configuration. #[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)] pub enum PidError { - /// Static PID gains or limits were non-finite or contradictory. + /// Static gains were negative/non-finite or limits were invalid. #[kaptan_error(id = 7101)] InvalidConfiguration, /// Time step was non-finite, non-positive, or exceeded the 100-ms integration bound. #[kaptan_error(id = 7102)] InvalidTime, - /// Error input was non-finite. + /// Dynamic input or an arithmetic intermediate was non-finite. #[kaptan_error(id = 7103)] NonFinite, } @@ -125,13 +136,27 @@ pub struct Pid { } impl Pid { - /// Creates a reset PID. Invalid limits intentionally make all updates fail - /// rather than being silently repaired in the flight path. - pub const fn new(config: PidConfig) -> Self { - Self { + /// Validates finite nonnegative gains, a positive output limit, and a + /// nonnegative integral limit once, then creates a reset PID. The retained + /// configuration is private and cannot change during updates or reset. + pub const fn new(config: PidConfig) -> Result { + if !config.proportional.is_finite() + | (config.proportional < 0.0) + | !config.integral.is_finite() + | (config.integral < 0.0) + | !config.derivative.is_finite() + | (config.derivative < 0.0) + | !config.output_limit.is_finite() + | (config.output_limit <= 0.0) + | !config.integral_limit.is_finite() + | (config.integral_limit < 0.0) + { + return Err(PidError::InvalidConfiguration); + } + Ok(Self { config, integral_contribution: 0.0, - } + }) } /// Clears only the retained integral contribution. pub fn reset(&mut self) { @@ -142,29 +167,6 @@ impl Pid { self.integral_contribution } - fn validate(&self, error: f32, error_rate: f32, dt_seconds: f32) -> Result<(), PidError> { - if !self.config.proportional.is_finite() - | !self.config.integral.is_finite() - | !self.config.derivative.is_finite() - | !self.config.output_limit.is_finite() - | (self.config.output_limit <= 0.0) - | !self.config.integral_limit.is_finite() - | (self.config.integral_limit < 0.0) - { - return Err(PidError::InvalidConfiguration); - } - if !dt_seconds.is_finite() - | (dt_seconds <= 0.0) - | (dt_seconds > MAXIMUM_CONTROL_STEP_SECONDS) - { - return Err(PidError::InvalidTime); - } - if !error.is_finite() || !error_rate.is_finite() { - return Err(PidError::NonFinite); - } - Ok(()) - } - /// Updates with error and error derivative. Conditional integration keeps /// the integrator from increasing saturation in its current direction. pub fn update( @@ -173,7 +175,23 @@ impl Pid { error_rate: f32, dt_seconds: f32, ) -> Result { - self.validate(error, error_rate, dt_seconds)?; + if !valid_control_step(dt_seconds) { + return Err(PidError::InvalidTime); + } + self.update_with_valid_time(error, error_rate, dt_seconds) + } + + // Scalar updates validate time above. RatePid validates its common time + // once before computing derivatives, then updates only private axis copies. + fn update_with_valid_time( + &mut self, + error: f32, + error_rate: f32, + dt_seconds: f32, + ) -> Result { + if !error.is_finite() || !error_rate.is_finite() { + return Err(PidError::NonFinite); + } let proportional_derivative = self.config.proportional * error + self.config.derivative * error_rate; if !proportional_derivative.is_finite() { @@ -204,18 +222,27 @@ impl Pid { -self.config.output_limit, self.config.output_limit, ); - if candidate_output == limited || candidate_output.signum() != error.signum() { - self.integral_contribution = candidate; - } + let next_integral = + if candidate_output == limited || candidate_output.signum() != error.signum() { + candidate + } else { + self.integral_contribution + }; + // The stored integral is in [-limit, limit]. With nonnegative Ki and + // positive dt, the candidate moves it only in error's direction. + // Retention requires candidate_output to have that same sign. Overflow + // in that direction would already have failed candidate_output above. + // Opposite-sign overflow cannot cross zero in one step: the validated + // Ki*error product and dt <= 0.1 bound the change below 0.101*f32::MAX. + let raw_output = proportional_derivative + next_integral; + debug_assert!(raw_output.is_finite()); let output = clamp( - proportional_derivative + self.integral_contribution, + raw_output, -self.config.output_limit, self.config.output_limit, ); - output - .is_finite() - .then_some(output) - .ok_or(PidError::NonFinite) + self.integral_contribution = next_integral; + Ok(output) } } @@ -227,14 +254,14 @@ pub struct RatePid { previous_error: Option>, } impl RatePid { - /// Creates one fixed PID per FRD angular-rate axis. - pub const fn new(roll: PidConfig, pitch: PidConfig, yaw: PidConfig) -> Self { - Self { - roll: Pid::new(roll), - pitch: Pid::new(pitch), - yaw: Pid::new(yaw), + /// Creates the rate loop only when all three axis configurations are valid. + pub fn new(roll: PidConfig, pitch: PidConfig, yaw: PidConfig) -> Result { + Ok(Self { + roll: Pid::new(roll)?, + pitch: Pid::new(pitch)?, + yaw: Pid::new(yaw)?, previous_error: None, - } + }) } /// Clears the retained integral and derivative history on every axis. @@ -262,29 +289,23 @@ impl RatePid { measured: BodyRate, dt_seconds: f32, ) -> Result, PidError> { - let error = target.vector().sub(measured.vector()); - if !dt_seconds.is_finite() - | (dt_seconds <= 0.0) - | (dt_seconds > MAXIMUM_CONTROL_STEP_SECONDS) - { + if !valid_control_step(dt_seconds) { return Err(PidError::InvalidTime); } + let error = target.vector().sub(measured.vector()); let error_rate = self .previous_error .map(|previous| error.sub(previous).scale(1.0 / dt_seconds)) .unwrap_or_else(Vector3::zero); - // Validate all axes before altering any retained state, so a malformed - // per-axis configuration cannot leave a partial controller update. - self.roll.validate(error.x(), error_rate.x(), dt_seconds)?; - self.pitch.validate(error.y(), error_rate.y(), dt_seconds)?; - self.yaw.validate(error.z(), error_rate.z(), dt_seconds)?; + // Each axis checks its computed error and derivative, then performs the + // arithmetic on a copy. Commit axes and history only if all succeed. let mut roll = self.roll; let mut pitch = self.pitch; let mut yaw = self.yaw; let output = Vector3::new( - roll.update(error.x(), error_rate.x(), dt_seconds)?, - pitch.update(error.y(), error_rate.y(), dt_seconds)?, - yaw.update(error.z(), error_rate.z(), dt_seconds)?, + roll.update_with_valid_time(error.x(), error_rate.x(), dt_seconds)?, + pitch.update_with_valid_time(error.y(), error_rate.y(), dt_seconds)?, + yaw.update_with_valid_time(error.z(), error_rate.z(), dt_seconds)?, ); self.roll = roll; self.pitch = pitch; @@ -340,7 +361,10 @@ pub enum TrajectoryError { /// A reference generator must not turn a stalled task into a large implicit /// integration step. The scheduler must reset/reseed it after this bound. -const MAXIMUM_CONTROL_STEP_SECONDS: f32 = 0.1; +pub const MAXIMUM_CONTROL_STEP_MICROSECONDS: u64 = 100_000; +/// Maximum interval accepted by all stateful control primitives, in seconds. +pub const MAXIMUM_CONTROL_STEP_SECONDS: f32 = + MAXIMUM_CONTROL_STEP_MICROSECONDS as f32 / 1_000_000.0; /// Allocation-free velocity-ramped trajectory for one local NED goal. /// @@ -354,6 +378,69 @@ pub struct Trajectory { } impl Trajectory { + /// Seeds a mode transition from measured motion instead of inventing a stop. + pub fn from_state( + position: Vector3, + velocity: Vector3, + limits: KinematicLimits, + ) -> Result { + let mut value = Self::new(position, limits)?; + if !velocity.is_finite() || !velocity.norm().is_finite() { + return Err(TrajectoryError::NonFiniteGoal); + } + value.reference.velocity = velocity; + Ok(value) + } + /// Bounded velocity mode, anchored to measured position rather than an + /// unobserved position integral. Consumers omit position feedback on these axes. + pub fn update_velocity( + &mut self, + position: Vector3, + target: Vector3, + dt: f32, + ) -> Result { + if !valid_control_step(dt) { + return Err(TrajectoryError::InvalidTime); + } + if !target.is_finite() || !position.is_finite() { + return Err(TrajectoryError::NonFiniteGoal); + } + let desired = limit_magnitude( + Vector3::new(target.x(), target.y(), 0.), + self.limits.maximum_horizontal_speed_mps, + ) + .ok_or(TrajectoryError::NumericalOverflow)?; + let old = self.reference.velocity; + let delta = limit_magnitude( + Vector3::new(desired.x() - old.x(), desired.y() - old.y(), 0.), + self.limits.maximum_horizontal_acceleration_mps2 * dt, + ) + .ok_or(TrajectoryError::NumericalOverflow)?; + let z = target.z().clamp( + -self.limits.maximum_vertical_speed_mps, + self.limits.maximum_vertical_speed_mps, + ); + let velocity = Vector3::new( + old.x() + delta.x(), + old.y() + delta.y(), + old.z() + + (z - old.z()).clamp( + -self.limits.maximum_vertical_acceleration_mps2 * dt, + self.limits.maximum_vertical_acceleration_mps2 * dt, + ), + ); + let next = KinematicReference { + position, + velocity, + acceleration: velocity.sub(old).scale(1. / dt), + }; + // Position is the already-validated measurement, not an integrated value. + if !next.acceleration.is_finite() { + return Err(TrajectoryError::NumericalOverflow); + } + self.reference = next; + Ok(next) + } /// Starts at a finite local position with zero velocity and acceleration. pub fn new( initial_position: Vector3, @@ -444,24 +531,28 @@ impl Trajectory { return Err(TrajectoryError::NumericalOverflow); } - // A discrete trajectory can cross the goal by less than one service - // interval. Snap only when the current velocity can be stopped inside - // this interval's declared acceleration envelope; otherwise let the - // bounded braking law decelerate through the crossing normally. + // Stop the horizontal vector and vertical axis independently. Requiring + // both to stop together lets a settled height chatter at full vertical + // acceleration during a horizontal move, consuming actuator slew authority. + // Each stop still requires this interval's existing acceleration envelope. let current_horizontal_speed = current_horizontal_velocity.norm(); let horizontal_error = Vector3::::new(position_error.x(), position_error.y(), 0.0).norm(); if (horizontal_error <= self.limits.maximum_horizontal_acceleration_mps2 * dt_seconds * dt_seconds) - & (position_error.z().abs() - <= self.limits.maximum_vertical_acceleration_mps2 * dt_seconds * dt_seconds) & (current_horizontal_speed <= self.limits.maximum_horizontal_acceleration_mps2 * dt_seconds) + { + next_position = Vector3::new(self.goal.x(), self.goal.y(), next_position.z()); + next_velocity = Vector3::new(0.0, 0.0, next_velocity.z()); + } + if (position_error.z().abs() + <= self.limits.maximum_vertical_acceleration_mps2 * dt_seconds * dt_seconds) & (self.reference.velocity.z().abs() <= self.limits.maximum_vertical_acceleration_mps2 * dt_seconds) { - next_position = self.goal; - next_velocity = Vector3::zero(); + next_position = Vector3::new(next_position.x(), next_position.y(), self.goal.z()); + next_velocity = Vector3::new(next_velocity.x(), next_velocity.y(), 0.0); } let next_reference = KinematicReference { @@ -471,17 +562,25 @@ impl Trajectory { .sub(self.reference.velocity) .scale(1.0 / dt_seconds), }; + // A measured overspeed seed must brake continuously, not be clipped or + // rejected mid-braking. Compare norms without overflowing squared bounds. + let horizontal_speed = + Vector3::::new(next_reference.velocity.x(), next_reference.velocity.y(), 0.) + .norm(); + let allowed_horizontal = self + .limits + .maximum_horizontal_speed_mps + .max(current_horizontal_speed); + let allowed_vertical = self + .limits + .maximum_vertical_speed_mps + .max(self.reference.velocity.z().abs()); if !next_reference.acceleration.is_finite() - | (Vector3::::new( - next_reference.velocity.x(), - next_reference.velocity.y(), - 0.0, - ) - .norm_squared() - > self.limits.maximum_horizontal_speed_mps - * self.limits.maximum_horizontal_speed_mps - + 1.0e-4) - | (next_reference.velocity.z().abs() > self.limits.maximum_vertical_speed_mps + 1.0e-4) + || !horizontal_speed.is_finite() + || horizontal_speed - allowed_horizontal + > 16. * f32::EPSILON * allowed_horizontal.max(1.) + || next_reference.velocity.z().abs() - allowed_vertical + > 16. * f32::EPSILON * allowed_vertical.max(1.) { return Err(TrajectoryError::NumericalOverflow); } @@ -495,10 +594,8 @@ impl Trajectory { & (position_tolerance_metres >= 0.0) & velocity_tolerance_mps.is_finite() & (velocity_tolerance_mps >= 0.0) - & (self.goal.sub(self.reference.position).norm_squared() - <= position_tolerance_metres * position_tolerance_metres) - & (self.reference.velocity.norm_squared() - <= velocity_tolerance_mps * velocity_tolerance_mps) + & (self.goal.sub(self.reference.position).norm() <= position_tolerance_metres) + & (self.reference.velocity.norm() <= velocity_tolerance_mps) } } @@ -551,7 +648,7 @@ fn limit_magnitude(vector: Vector3, maximum: f32) -> Option> { if scale == 0.0 { return Some(vector); } - let scaled = vector.scale(1.0 / scale); + let scaled = Vector3::::new(vector.x() / scale, vector.y() / scale, vector.z() / scale); let magnitude = scale * sqrt(scaled.norm_squared()); if !magnitude.is_finite() || magnitude > maximum { return vector @@ -561,11 +658,7 @@ fn limit_magnitude(vector: Vector3, maximum: f32) -> Option> { Some(vector) } -/// Estimate consumed by the translational controller. -/// -/// This is intentionally a value type. The controller does not accept a -/// simulator object or an aiding measurement; callers pass the estimator's -/// current state after its integrity policy has accepted it. +/// The caller must supply the estimator's current accepted state. #[derive(Clone, Copy, Debug, PartialEq)] pub struct TranslationalEstimate { /// Estimated local NED position in metres. @@ -576,11 +669,8 @@ pub struct TranslationalEstimate { /// Static limits and gains for the position-to-acceleration loop. /// -/// TODO(configuration): this is supplied by a reviewed build-time vehicle -/// profile until persistent ground configuration exists. Gains and all limits -/// must be staged and validated as one controller revision, then activated -/// only while disarmed with controller state reset; field-by-field live tuning -/// is not a safe update protocol. +/// Validate gains and limits together. Activate a replacement only while +/// disarmed, with controller state reset. #[derive(Clone, Copy, Debug, PartialEq)] pub struct TranslationalControlConfig { /// Position-error gain in s⁻². @@ -608,10 +698,8 @@ pub enum TranslationalControlError { /// Stateless outer loop for a cascaded multicopter controller. /// -/// Integral action is deliberately absent here. Position aiding can disappear -/// or be rejected by the estimator; retaining an unobservable position -/// integral across that transition would be unsafe. Any future integral must -/// have an explicit estimator-validity reset and allocator-feedback policy. +/// Integral action is omitted because position error becomes unobservable when +/// position aiding is lost. pub struct TranslationalController { config: TranslationalControlConfig, } @@ -677,18 +765,15 @@ impl TranslationalController { /// Physical conversion and attitude-envelope limits for one multirotor. /// -/// TODO(configuration): mass, thrust capability, and tilt limit are a single -/// vehicle-identification/calibration record. The future configuration path -/// must atomically bind this record to the matching mixer and reject output -/// until it is reviewed for the installed airframe. +/// Mass, thrust capability and tilt limit must match the installed airframe +/// and its mixer before output is authorized. #[derive(Clone, Copy, Debug, PartialEq)] pub struct ThrustEnvelope { /// Vehicle mass in kilograms. pub mass_kg: f32, /// Maximum thrust available from all actuators in newtons. pub maximum_collective_thrust_newtons: f32, - /// Maximum horizontal-to-vertical thrust ratio. For a 35° tilt limit this - /// is `tan(35°)`, calculated and reviewed with the vehicle configuration. + /// Maximum horizontal-to-vertical thrust ratio, equal to tan(maximum tilt). pub maximum_tilt_tangent: f32, } @@ -775,10 +860,6 @@ impl ThrustEnvelope { } /// Runtime command limits for one statically ordered actuator. -/// -/// Physical positions, thrust axes, and reaction coefficients belong to the -/// vehicle's reviewed offline allocation calculation. Keeping a second copy -/// here would create an authority that the rate-path allocator cannot use. #[derive(Clone, Copy, Debug, PartialEq)] pub struct ActuatorLimits { /// Inclusive lower command bound. @@ -789,7 +870,7 @@ pub struct ActuatorLimits { pub slew_per_second: f32, } -/// One reviewed allocation output. Coefficients are ordered as roll torque, +/// One allocation output. Coefficients are ordered as roll torque, /// pitch torque, yaw torque, then collective thrust. #[derive(Clone, Copy, Debug, PartialEq)] pub struct MixerOutput { @@ -799,29 +880,22 @@ pub struct MixerOutput { pub limits: ActuatorLimits, } -/// Reviewed, precomputed allocation map in static actuator order. +/// Precomputed allocation map in static actuator order. /// -/// Pairing each row with its command limits prevents independently indexed -/// geometry and matrix tables from drifting out of correspondence. -/// -/// TODO(configuration): persist and activate this only as an entire, -/// vehicle-bound allocation profile, with actuator output inhibited and -/// allocation history reset. Individual mixer-row editing would break the -/// static-order and physical-review guarantees of this type. +/// Replace the complete vehicle-bound map only with output inhibited and +/// allocation history reset. Rows and limits must retain the same actuator order. #[derive(Clone, Copy, Debug, PartialEq)] pub struct PrecomputedMixer { outputs: [MixerOutput; ACTUATORS], } impl PrecomputedMixer { - /// Creates a reviewed static allocation map. pub const fn new(outputs: [MixerOutput; ACTUATORS]) -> Self { Self { outputs } } } -/// Requested body wrench in vehicle-specific normalized allocation units. -/// Units remain explicit in the vehicle's reviewed mixer calibration; generic -/// allocation never pretends torque and thrust share a physical unit. +/// Requested body wrench in the vehicle mixer's normalized units. +/// Torque and thrust scales come from the vehicle's mixer calibration. #[derive(Clone, Copy, Debug, PartialEq)] pub struct BodyWrench { /// Roll torque request. @@ -864,6 +938,15 @@ pub struct AllocationResult { } /// Fixed-map actuator allocator with no dynamic inversion. +/// +/// The mixer and allocator must have the same number of actuators. +/// ```compile_fail,E0308 +/// use kaptan_control::{Allocator, PrecomputedMixer}; +/// fn quad_allocator(mixer: PrecomputedMixer<6>) { +/// let _ = Allocator::<4>::new(mixer); +/// } +/// ``` +#[derive(Clone, Copy)] pub struct Allocator { mixer: PrecomputedMixer, previous: [f32; ACTUATORS], @@ -871,6 +954,62 @@ pub struct Allocator { } impl Allocator { + /// Enforces a final sum-of-commands cap, including range and slew effects. + /// The caller supplies the physical conversion for its equal-thrust motors. + /// Failed feasibility leaves allocator history unchanged. + pub fn allocate_capped( + &mut self, + wrench: BodyWrench, + dt: f32, + maximum_sum: f32, + ) -> Result, AllocatorError> { + if !maximum_sum.is_finite() || maximum_sum < 0. { + return Err(AllocatorError::InvalidWrench); + } + let mut candidate = *self; + let mut result = candidate.allocate(wrench, dt)?; + let mut floor = [0.; ACTUATORS]; + for (i, out) in self.mixer.outputs.iter().enumerate() { + floor[i] = if self.failed[i] { + out.limits.minimum + } else { + out.limits + .minimum + .max(self.previous[i] - out.limits.slew_per_second * dt) + }; + } + let sum: f32 = result.commands.iter().sum(); + let floor_sum: f32 = floor.iter().sum(); + if !sum.is_finite() || !floor_sum.is_finite() || floor_sum > maximum_sum { + return Err(AllocatorError::InvalidWrench); + } + if sum > maximum_sum { + let span = sum - floor_sum; + if !span.is_finite() { + return Err(AllocatorError::InvalidWrench); + } + // floor_sum <= maximum_sum < sum proves a positive span and a + // finite scale in [0,1]. Project rounding into the valid slew box. + let scale = (maximum_sum - floor_sum) / span; + for (command, minimum) in result.commands.iter_mut().zip(floor) { + *command = (minimum + (*command - minimum) * scale).clamp(minimum, *command); + } + // Floating roundoff must not turn the cap into extra authority. + let excess = (result.commands.iter().sum::() - maximum_sum).max(0.); + if let Some(i) = (0..ACTUATORS).find(|&i| result.commands[i] - floor[i] >= excess) { + result.commands[i] = (result.commands[i] - excess).max(floor[i]); + } else { + return Err(AllocatorError::InvalidWrench); + } + if result.commands.iter().sum::() > maximum_sum { + return Err(AllocatorError::InvalidWrench); + } + result.saturated = true; + candidate.previous = result.commands; + } + *self = candidate; + Ok(result) + } /// Validates the reviewed static allocation map. pub fn new(mixer: PrecomputedMixer) -> Result { validate_mixer_outputs(&mixer.outputs)?; @@ -1016,6 +1155,423 @@ const fn clamp(value: f32, low: f32, high: f32) -> f32 { mod tests { use super::*; + #[test] + fn settled_axes_stay_stopped_during_orthogonal_position_moves() { + let limits = KinematicLimits { + maximum_horizontal_speed_mps: 3.0, + maximum_vertical_speed_mps: 1.5, + maximum_horizontal_acceleration_mps2: 2.0, + maximum_vertical_acceleration_mps2: 1.5, + }; + // The h=500 us trace arrives within a few tens of micrometres of the + // height goal with < 0.015 m/s reference speed. Vertical motion can + // stop in this control interval even while a one-metre move is active. + let mut horizontal = Trajectory::from_state( + Vector3::new(0.0, 0.0, -2.00002), + Vector3::new(0.0, 0.0, 0.0075), + limits, + ) + .unwrap(); + horizontal.set_goal(Vector3::new(1.0, 0.0, -2.0)).unwrap(); + let first = horizontal.update(0.01).unwrap(); + assert_eq!(first.position.z(), -2.0); + assert_eq!(first.velocity.z(), 0.0); + assert!(first.acceleration.z().abs() <= 1.5); + for _ in 0..200 { + // Repeating an unchanged streamed target must not restart any axis. + horizontal.set_goal(Vector3::new(1.0, 0.0, -2.0)).unwrap(); + let r = horizontal.update(0.01).unwrap(); + assert_eq!(r.position.z(), -2.0); + assert_eq!(r.velocity.z(), 0.0); + assert_eq!(r.acceleration.z(), 0.0); + } + assert!(horizontal.is_complete(0.001, 0.001)); + + let mut vertical = Trajectory::from_state( + Vector3::new(-0.00002, 0.00002, 0.0), + Vector3::new(0.005, -0.005, 0.0), + limits, + ) + .unwrap(); + vertical.set_goal(Vector3::new(0.0, 0.0, -2.0)).unwrap(); + let first = vertical.update(0.01).unwrap(); + assert_eq!((first.position.x(), first.position.y()), (0.0, 0.0)); + assert_eq!((first.velocity.x(), first.velocity.y()), (0.0, 0.0)); + assert!( + Vector3::::new(first.acceleration.x(), first.acceleration.y(), 0.0).norm() <= 2.0 + ); + for _ in 0..250 { + let r = vertical.update(0.01).unwrap(); + assert_eq!((r.position.x(), r.position.y()), (0.0, 0.0)); + assert_eq!((r.velocity.x(), r.velocity.y()), (0.0, 0.0)); + assert_eq!((r.acceleration.x(), r.acceleration.y()), (0.0, 0.0)); + } + assert!(vertical.reference().position.z() < -1.9); + } + + #[test] + fn force_direction_preserves_absolute_horizontal_azimuth() { + // Independent f64 geometry, including the diagonal-tilt counterexamples. + for yaw in [-3.0_f64, -1.3, 0.0, 0.7, 2.6] { + for tilt in [0.0_f64, 21.0, 35.0] { + for azimuth in [0.0_f64, 0.4, core::f64::consts::FRAC_PI_4, 1.7, 2.9] { + let angle = tilt.to_radians(); + let down = [ + angle.sin() * azimuth.cos(), + angle.sin() * azimuth.sin(), + angle.cos(), + ]; + let q = force_to_attitude( + NedForce::newtons(-down[0] as f32, -down[1] as f32, -down[2] as f32), + HeadingNed::new(yaw.cos() as f32, yaw.sin() as f32).unwrap(), + ) + .unwrap(); + let forward = q.body_to_ned(Vector3::new(1.0, 0.0, 0.0)); + let actual = f64::from(forward.y()).atan2(f64::from(forward.x())); + let error = (actual - yaw).sin().atan2((actual - yaw).cos()); + assert!(error.abs() < 1e-6); + let z = q.body_to_ned(Vector3::new(0.0, 0.0, 1.0)); + for (actual, expected) in [z.x(), z.y(), z.z()].into_iter().zip(down) { + assert!((f64::from(actual) - expected).abs() < 1e-6); + } + } + } + } + assert_eq!( + force_to_attitude( + NedForce::newtons(0.0, 0.0, 1.0), + HeadingNed::new(1.0, 0.0).unwrap() + ), + Err(AttitudeError::SingularHeading) + ); + } + + #[test] + fn velocity_reference_refuses_invalid_time_state_and_late_overflow() { + let limits = trajectory_limits(); + for velocity in [ + Vector3::new(f32::NAN, 0.0, 0.0), + Vector3::new(f32::MAX, f32::MAX, 0.0), + ] { + assert!(Trajectory::from_state(Vector3::zero(), velocity, limits).is_err()); + } + let mut t = Trajectory::new(Vector3::zero(), limits).unwrap(); + let before = t.reference(); + assert_eq!( + t.update_velocity(Vector3::zero(), Vector3::zero(), 0.0), + Err(TrajectoryError::InvalidTime) + ); + assert_eq!( + t.update_velocity(Vector3::new(f32::NAN, 0.0, 0.0), Vector3::zero(), 0.01), + Err(TrajectoryError::NonFiniteGoal) + ); + assert_eq!( + t.update_velocity(Vector3::zero(), Vector3::zero(), f32::from_bits(1)), + Err(TrajectoryError::NumericalOverflow) + ); + assert_eq!(t.reference(), before); + assert_eq!( + t.update(f32::from_bits(1)), + Err(TrajectoryError::NumericalOverflow) + ); + t.reference.velocity = Vector3::new(f32::MAX, f32::MAX, 0.0); + assert_eq!(t.update(0.01), Err(TrajectoryError::NumericalOverflow)); + for horizontal in [true, false] { + let mut t = Trajectory::new(Vector3::zero(), limits).unwrap(); + let before = t.reference(); + // Invalidate a stored limit to test the final candidate invariant, + // independently of the constructor's configuration checks. + if horizontal { + t.limits.maximum_horizontal_speed_mps = -1.0; + t.goal = Vector3::new(1.0, 0.0, 0.0); + } else { + t.limits.maximum_vertical_speed_mps = -1.0; + t.goal = Vector3::new(0.0, 0.0, 1.0); + } + assert_eq!(t.update(0.01), Err(TrajectoryError::NumericalOverflow)); + assert_eq!(t.reference(), before); + } + } + + #[test] + fn cap_validation_failure_and_floating_residue_preserve_allocation_history() { + let limits = ActuatorLimits { + minimum: 0.0, + maximum: 1.0, + slew_per_second: 8.0, + }; + let mut a = Allocator::new(PrecomputedMixer::new( + [MixerOutput { + coefficients: [0.0, 0.0, 0.0, 1.0], + limits, + }; 4], + )) + .unwrap(); + let request = BodyWrench { + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + collective: 1.0, + }; + for cap in [f32::NAN, -1.0] { + assert_eq!( + a.allocate_capped(request, 0.01, cap), + Err(AllocatorError::InvalidWrench) + ); + } + a.set_failed(0, true).unwrap(); + assert_eq!( + a.allocate_capped(request, 0.01, 4.0).unwrap().commands[0], + 0.0 + ); + a.set_failed(0, false).unwrap(); + // A binary32 cap one ulp above the feasible floor can leave a residue + // larger than each individual channel's remaining slew headroom. + a.previous = [0.18, 0.18, 0.18, 0.28]; + let before = a.previous; + assert_eq!( + a.allocate_capped(request, 0.01, f32::from_bits(0.5_f32.to_bits() + 1)), + Err(AllocatorError::InvalidWrench) + ); + assert_eq!(a.previous, before); + let fixed = |value| MixerOutput { + coefficients: [0.0; 4], + limits: ActuatorLimits { + minimum: value, + maximum: value, + slew_per_second: 0.0, + }, + }; + let mut a = Allocator::new(PrecomputedMixer::new([ + fixed(1e20), + MixerOutput { + coefficients: [0.0, 0.0, 0.0, 1.0], + limits: ActuatorLimits { + minimum: 0.0, + maximum: 1e13, + slew_per_second: 1e15, + }, + }, + fixed(-1e20), + MixerOutput { + coefficients: [0.0, 0.0, 0.0, 1.0], + limits: ActuatorLimits { + minimum: 0.0, + maximum: 1.0, + slew_per_second: 10.0, + }, + }, + ])) + .unwrap(); + let before = a.previous; + let spacing = 1e20_f32.next_up() - 1e20_f32; + assert_eq!( + a.allocate_capped( + BodyWrench { + collective: 1e13, + ..request + }, + 0.1, + spacing.next_down() + ), + Err(AllocatorError::InvalidWrench) + ); + assert_eq!(a.previous, before); + } + + #[test] + fn aggregate_overflows_do_not_turn_into_a_valid_cap_scale() { + let limits = ActuatorLimits { + minimum: -f32::MAX, + maximum: f32::MAX, + slew_per_second: f32::MAX, + }; + let request = BodyWrench { + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + collective: 0.0, + }; + let mut a = Allocator::new(PrecomputedMixer::new( + [MixerOutput { + coefficients: [0.0, 0.0, 0.0, 1.0], + limits, + }; 8], + )) + .unwrap(); + for _ in 0..12 { + a.allocate(request, 0.1).unwrap(); + } + let before = a.previous; + assert_eq!( + a.allocate_capped( + BodyWrench { + collective: f32::MAX * 0.1, + ..request + }, + 0.1, + 0.0 + ), + Err(AllocatorError::InvalidWrench) + ); + assert_eq!(a.previous, before); + for _ in 0..12 { + a.allocate( + BodyWrench { + collective: f32::MAX, + ..request + }, + 0.1, + ) + .unwrap(); + } + let before = a.previous; + assert_eq!( + a.allocate_capped( + BodyWrench { + collective: f32::MAX, + ..request + }, + 0.1, + f32::MAX + ), + Err(AllocatorError::InvalidWrench) + ); + assert_eq!(a.previous, before); + let mut a = Allocator::new(PrecomputedMixer::new([ + MixerOutput { + coefficients: [-1.0, 0.0, 0.0, 1.0], + limits, + }, + MixerOutput { + coefficients: [-1.0, 0.0, 0.0, 1.0], + limits, + }, + MixerOutput { + coefficients: [1.0, 0.0, 0.0, 1.0], + limits, + }, + MixerOutput { + coefficients: [1.0, 0.0, 0.0, 1.0], + limits, + }, + ])) + .unwrap(); + for _ in 0..20 { + a.allocate( + BodyWrench { + roll: 0.45 * f32::MAX, + ..request + }, + 0.1, + ) + .unwrap(); + } + let before = a.previous; + assert_eq!( + a.allocate_capped( + BodyWrench { + roll: 0.4 * f32::MAX, + collective: 0.05 * f32::MAX, + ..request + }, + 0.1, + 0.0 + ), + Err(AllocatorError::InvalidWrench) + ); + assert_eq!(a.previous, before); + } + + #[test] + fn final_collective_cap_respects_slew_and_is_atomic_on_infeasibility() { + let mixer = PrecomputedMixer::new( + [ + output([-1., 1., 1., 1.]), + output([-1., -1., -1., 1.]), + output([1., -1., 1., 1.]), + output([1., 1., -1., 1.]), + ] + .map(|mut output| { + output.limits.slew_per_second = 8.; + output + }), + ); + let mut a = Allocator::new(mixer).unwrap(); + let wrench = BodyWrench { + roll: 0.05, + pitch: -0.03, + yaw: 0.02, + collective: 0.65, + }; + for _ in 0..100 { + a.allocate(wrench, 0.01).unwrap(); + } + let before = a.previous; + assert_eq!( + a.allocate_capped(wrench, 0.01, 0.), + Err(AllocatorError::InvalidWrench) + ); + assert_eq!(a.previous, before); + for k in 1..=100 { + let cap = 2.6 - (k as f32 * 0.016).min(1.6); + let old = a.previous; + let result = a.allocate_capped(wrench, 0.01, cap).unwrap(); + assert!(result.commands.iter().sum::() <= cap + 1e-7); + for (i, previous) in old.iter().enumerate() { + assert!( + (result.commands[i] - previous).abs() + <= mixer.outputs[i].limits.slew_per_second * 0.01 + 1e-7 + ); + } + } + assert!(a.previous.iter().sum::() * 4.5 < 0.5 * 1.2 * 9.80665); + } + + #[test] + fn velocity_mode_and_position_mode_share_nonzero_motion_continuity() { + let mut t = Trajectory::from_state( + Vector3::new(1., 2., -2.), + Vector3::new(0.3, 0., 0.), + trajectory_limits(), + ) + .unwrap(); + let old = t.reference(); + let next = t + .update_velocity( + Vector3::new(1.003, 2., -2.), + Vector3::new(-1., 0., 0.), + 0.01, + ) + .unwrap(); + assert!(next.velocity.x() > 0.); + assert!(next.position.x() > old.position.x()); + assert!( + next.acceleration.norm() + <= trajectory_limits().maximum_horizontal_acceleration_mps2 + 1e-5 + ); + t.set_goal(Vector3::new(0., 0., -2.)).unwrap(); + let next2 = t.update(0.01).unwrap(); + assert!(next2.velocity.sub(next.velocity).norm() < 0.1); + let before = t.reference(); + assert!( + t.update_velocity(before.position, Vector3::new(f32::NAN, 0., 0.), 0.01) + .is_err() + ); + assert_eq!(t.reference(), before); + let limits = trajectory_limits(); + let initial_speed = limits.maximum_horizontal_speed_mps + 1.; + let mut braking = + Trajectory::from_state(Vector3::zero(), Vector3::new(initial_speed, 0., 0.), limits) + .unwrap(); + let reference = braking.update(0.01).unwrap(); + assert!(reference.velocity.x() < initial_speed); + assert!( + reference.velocity.x() + >= initial_speed - limits.maximum_horizontal_acceleration_mps2 * 0.01 - 1e-6 + ); + } + fn trajectory_limits() -> KinematicLimits { KinematicLimits { maximum_horizontal_speed_mps: 4.0, @@ -1180,7 +1736,8 @@ mod tests { derivative: 0.0, output_limit: 1.0, integral_limit: 0.5, - }); + }) + .unwrap(); for _ in 0..100 { assert_eq!(pid.update(1.0, 0.0, 0.01).unwrap(), 1.0); } @@ -1196,11 +1753,89 @@ mod tests { derivative: 0.0, output_limit: 1.0, integral_limit: 0.5, - }); + }) + .unwrap(); assert_eq!(pid.update(2.0, 0.0, 0.01), Err(PidError::NonFinite)); assert_eq!(pid.integral_contribution(), 0.0); } + #[test] + fn pid_rejects_opposite_infinite_products_without_changing_state() { + let mut pid = Pid::new(PidConfig { + proportional: f32::MAX, + integral: 0.0, + derivative: f32::MAX, + output_limit: 1.0, + integral_limit: 1.0, + }) + .unwrap(); + let state_bits = |pid: Pid| { + [ + pid.config.proportional.to_bits(), + pid.config.integral.to_bits(), + pid.config.derivative.to_bits(), + pid.config.output_limit.to_bits(), + pid.config.integral_limit.to_bits(), + pid.integral_contribution.to_bits(), + ] + }; + let before = state_bits(pid); + // Finite inputs overflow the two products to opposite infinities. + // Their NaN sum must be rejected before any retained value changes. + assert_eq!(pid.update(2.0, -2.0, 0.01), Err(PidError::NonFinite)); + assert_eq!(state_bits(pid), before); + } + + #[test] + fn pid_rejects_negative_gains_and_late_output_overflow_atomically() { + let config = PidConfig { + proportional: 1.0, + integral: 1.0, + derivative: 1.0, + output_limit: f32::MAX, + integral_limit: f32::MAX, + }; + for invalid in [ + PidConfig { + proportional: -1.0, + ..config + }, + PidConfig { + integral: -1.0, + ..config + }, + PidConfig { + derivative: -1.0, + ..config + }, + ] { + assert_eq!(Pid::new(invalid), Err(PidError::InvalidConfiguration)); + } + let mut pid = Pid::new(PidConfig { + proportional: 0.0, + ..config + }) + .unwrap(); + // Each product and the tentative integral are finite. Only the final + // sum overflows, after a nonzero integral was retained by real updates. + for _ in 0..6 { + pid.update(-3.0e38, 0.0, 0.1).unwrap(); + } + let before = pid; + assert_eq!(pid.update(-3.0e38, -1.5e38, 0.1), Err(PidError::NonFinite)); + assert_eq!(pid, before); + } + + #[test] + fn trajectory_completion_does_not_compare_overflowed_squares() { + let mut trajectory = Trajectory::new(Vector3::zero(), trajectory_limits()).unwrap(); + trajectory + .set_goal(Vector3::new(3.0e30, 4.0e30, 0.0)) + .unwrap(); + assert!(!trajectory.is_complete(4.0e30, 0.0)); + assert!(trajectory.is_complete(6.0e30, 0.0)); + } + #[test] fn rate_pid_uses_history() { let config = PidConfig { @@ -1210,7 +1845,7 @@ mod tests { output_limit: 20.0, integral_limit: 0.0, }; - let mut pid = RatePid::new(config, config, config); + let mut pid = RatePid::new(config, config, config).unwrap(); assert_eq!( pid.update( BodyRate::radians_per_second(0.0, 0.0, 0.0), @@ -1242,13 +1877,13 @@ mod tests { output_limit: 2.0, integral_limit: 1.0, }; - let mut pid = Pid::new(config); + let mut pid = Pid::new(config).unwrap(); pid.update(1.0, 0.0, 0.1).unwrap(); let pid_before = pid.integral_contribution(); assert_eq!(pid.update(1.0, 0.0, 0.101), Err(PidError::InvalidTime)); assert_eq!(pid.integral_contribution(), pid_before); - let mut rate = RatePid::new(config, config, config); + let mut rate = RatePid::new(config, config, config).unwrap(); rate.update( BodyRate::radians_per_second(1.0, -1.0, 0.5), BodyRate::radians_per_second(0.0, 0.0, 0.0), @@ -1324,11 +1959,19 @@ mod tests { output_limit: 2.0, integral_limit: 1.0, }; - let invalid_yaw = PidConfig { + let overflowing_yaw = PidConfig { proportional: f32::MAX, ..finite }; - let mut rate = RatePid::new(finite, finite, invalid_yaw); + // f32::MAX is a valid gain. A later product, not construction, fails. + let mut rate = RatePid::new(finite, finite, overflowing_yaw).unwrap(); + rate.update( + BodyRate::radians_per_second(0.5, 0.5, 0.0), + BodyRate::radians_per_second(0.0, 0.0, 0.0), + 0.1, + ) + .unwrap(); + let before = (rate.roll, rate.pitch, rate.yaw, rate.previous_error); assert_eq!( rate.update( BodyRate::radians_per_second(1.0, 1.0, 2.0), @@ -1337,10 +1980,91 @@ mod tests { ), Err(PidError::NonFinite) ); - assert_eq!(rate.roll.integral_contribution(), 0.0); - assert_eq!(rate.pitch.integral_contribution(), 0.0); - assert_eq!(rate.yaw.integral_contribution(), 0.0); - assert!(rate.previous_error.is_none()); + assert_eq!( + (rate.roll, rate.pitch, rate.yaw, rate.previous_error), + before + ); + } + + #[test] + fn rate_pid_requires_three_valid_configurations_without_profile_limits() { + const ZERO_GAINS: PidConfig = PidConfig { + proportional: 0.0, + integral: 0.0, + derivative: 0.0, + output_limit: 0.5, + integral_limit: 1.0, + }; + const SCALAR: Result = Pid::new(ZERO_GAINS); + let mut scalar = SCALAR.unwrap(); + assert_eq!(scalar.update(f32::MAX, -f32::MAX, 0.1), Ok(0.0)); + let invalid = PidConfig { + derivative: -1.0, + ..ZERO_GAINS + }; + for (roll, pitch, yaw) in [ + (invalid, ZERO_GAINS, ZERO_GAINS), + (ZERO_GAINS, invalid, ZERO_GAINS), + (ZERO_GAINS, ZERO_GAINS, invalid), + ] { + assert!(matches!( + RatePid::new(roll, pitch, yaw), + Err(PidError::InvalidConfiguration) + )); + } + let mut rate = RatePid::new(ZERO_GAINS, ZERO_GAINS, ZERO_GAINS).unwrap(); + assert_eq!( + rate.update( + BodyRate::radians_per_second(f32::MAX, -f32::MAX, 1.0), + BodyRate::radians_per_second(0.0, 0.0, 0.0), + 0.1, + ), + Ok(Vector3::zero()) + ); + } + + #[test] + fn computed_rate_errors_and_derivatives_are_checked_atomically() { + let config = PidConfig { + proportional: 0.0, + integral: 1.0, + derivative: 0.0, + output_limit: 2.0, + integral_limit: 1.0, + }; + let zero = BodyRate::radians_per_second(0.0, 0.0, 0.0); + let mut rate = RatePid::new(config, config, config).unwrap(); + rate.update(BodyRate::radians_per_second(1.0, 1.0, 1.0), zero, 0.1) + .unwrap(); + let before = (rate.roll, rate.pitch, rate.yaw, rate.previous_error); + for (target, measured) in [ + // Finite operands overflow when forming the pitch error. + ( + BodyRate::radians_per_second(1.0, f32::MAX, 1.0), + BodyRate::radians_per_second(0.0, -f32::MAX, 0.0), + ), + // Every error is finite, but the yaw derivative overflows. This + // remains an error even with Kd=0, after two private axis updates. + (BodyRate::radians_per_second(1.0, 1.0, f32::MAX), zero), + ] { + assert_eq!(rate.update(target, measured, 0.1), Err(PidError::NonFinite)); + assert_eq!( + (rate.roll, rate.pitch, rate.yaw, rate.previous_error), + before + ); + } + assert_eq!( + rate.update( + BodyRate::radians_per_second(f32::MAX, 0.0, 0.0), + BodyRate::radians_per_second(-f32::MAX, 0.0, 0.0), + 0.0, + ), + Err(PidError::InvalidTime) + ); + assert_eq!( + (rate.roll, rate.pitch, rate.yaw, rate.previous_error), + before + ); } #[test] @@ -1526,31 +2250,49 @@ mod tests { }, ]; for config in invalid { - assert_eq!( - Pid::new(config).update(0.0, 0.0, 0.01), - Err(PidError::InvalidConfiguration) - ); + assert_eq!(Pid::new(config), Err(PidError::InvalidConfiguration)); } - let mut pid = Pid::new(valid); + let mut pid = Pid::new(valid).unwrap(); assert_eq!(pid.update(0.0, 0.0, 0.0), Err(PidError::InvalidTime)); assert_eq!(pid.update(f32::NAN, 0.0, 0.01), Err(PidError::NonFinite)); assert_eq!(pid.update(0.0, f32::NAN, 0.01), Err(PidError::NonFinite)); - pid.integral_contribution = f32::MAX; - pid.config.integral = f32::MAX; - assert_eq!(pid.update(1.0, 0.0, 0.1), Err(PidError::NonFinite)); - pid.reset(); - assert_eq!(pid.integral_contribution(), 0.0); + let mut integral_overflow = Pid::new(PidConfig { + proportional: 0.0, + integral: f32::MAX, + derivative: 0.0, + output_limit: f32::MAX, + integral_limit: f32::MAX, + }) + .unwrap(); + // Ten accepted increments retain about 0.91*MAX. Adding another + // 0.1*MAX overflows the sum even though the increment itself is finite. + for _ in 0..10 { + integral_overflow.update(0.91, 0.0, 0.1).unwrap(); + } + let before = integral_overflow; + assert_eq!( + integral_overflow.update(1.0, 0.0, 0.1), + Err(PidError::NonFinite) + ); + assert_eq!(integral_overflow, before); + integral_overflow.reset(); + assert_eq!(integral_overflow.integral_contribution(), 0.0); let mut opposite_sign_recovery = Pid::new(PidConfig { proportional: 0.0, - integral: 0.0, - derivative: 0.0, + integral: 1.0, + derivative: 1.0, output_limit: 0.5, integral_limit: 1.0, - }); - opposite_sign_recovery.integral_contribution = 1.0; + }) + .unwrap(); + // Derivative action offsets the integral while it builds above the + // output bound. Opposite-sign error must then be allowed to unwind it. + assert_eq!(opposite_sign_recovery.update(10.0, -1.0, 0.1), Ok(0.0)); + assert_eq!(opposite_sign_recovery.integral_contribution(), 1.0); assert_eq!(opposite_sign_recovery.update(-1.0, 0.0, 0.1), Ok(0.5)); + assert_eq!(opposite_sign_recovery.integral_contribution(), 0.9); - let mut rate = RatePid::new(valid, valid, valid); + let mut rate = RatePid::new(valid, valid, valid).unwrap(); assert_eq!( rate.update( BodyRate::radians_per_second(0.0, 0.0, 0.0), @@ -1691,8 +2433,8 @@ mod tests { )); } let controller = TranslationalController::new(config).unwrap(); - let invalid_reference = KinematicReference { - position: Vector3::new(f32::NAN, 0.0, 0.0), + let zero_reference = KinematicReference { + position: Vector3::zero(), velocity: Vector3::zero(), acceleration: Vector3::zero(), }; @@ -1700,20 +2442,18 @@ mod tests { position: Vector3::zero(), velocity: Vector3::zero(), }; - assert_eq!( - controller.update(invalid_reference, estimate), - Err(TranslationalControlError::NonFiniteState) - ); for reference in [ KinematicReference { - position: Vector3::zero(), + position: Vector3::new(f32::NAN, 0.0, 0.0), + ..zero_reference + }, + KinematicReference { velocity: Vector3::new(f32::NAN, 0.0, 0.0), - acceleration: Vector3::zero(), + ..zero_reference }, KinematicReference { - position: Vector3::zero(), - velocity: Vector3::zero(), acceleration: Vector3::new(f32::NAN, 0.0, 0.0), + ..zero_reference }, ] { assert_eq!( @@ -1732,21 +2472,13 @@ mod tests { }, ] { assert_eq!( - controller.update( - KinematicReference { - position: Vector3::zero(), - velocity: Vector3::zero(), - acceleration: Vector3::zero() - }, - invalid_estimate - ), + controller.update(zero_reference, invalid_estimate), Err(TranslationalControlError::NonFiniteState) ); } let overflowing = KinematicReference { position: Vector3::new(f32::MAX, 0.0, 0.0), - velocity: Vector3::zero(), - acceleration: Vector3::zero(), + ..zero_reference }; let huge_gain = TranslationalController::new(TranslationalControlConfig { position_gain_per_second_squared: f32::MAX, @@ -1977,7 +2709,8 @@ mod tests { derivative: 0.0, output_limit: 1.0, integral_limit: 1.0, - }); + }) + .unwrap(); assert_eq!(pid.update(2.0, 0.0, 0.1), Err(PidError::NonFinite)); assert_eq!(pid.integral_contribution(), 0.0); @@ -1990,23 +2723,32 @@ mod tests { Err(TrajectoryError::NumericalOverflow) ); - let mut bounded_but_inconsistent = - Trajectory::new(Vector3::zero(), trajectory_limits()).unwrap(); - bounded_but_inconsistent.reference.velocity = Vector3::new(5.0, 0.0, 0.0); - assert_eq!( - bounded_but_inconsistent.update(0.01), - Err(TrajectoryError::NumericalOverflow) - ); + // An explicitly measured overspeed is now a supported transition seed. + // Preserve the physical bound by braking at the acceleration limit, + // rather than rejecting healthy fallback or clipping velocity instantly. + let mut braking = Trajectory::from_state( + Vector3::zero(), + Vector3::new(5.0, 0.0, 0.0), + trajectory_limits(), + ) + .unwrap(); + let step = braking.update(0.01).unwrap(); + assert!((step.velocity.x() - 4.97).abs() < 1e-6); + assert!((step.position.x() - 0.0497).abs() < 1e-7); let envelope = ThrustEnvelope { mass_kg: 1.0, maximum_collective_thrust_newtons: f32::MAX, maximum_tilt_tangent: 1.0, }; + let (force, saturated) = envelope + .force_for_acceleration(Vector3::new(f32::MAX, 0.0, 0.0), f32::MAX / 2.0) + .unwrap(); assert_eq!( - envelope.force_for_acceleration(Vector3::new(f32::MAX, 0.0, 0.0), f32::MAX / 2.0), - Err(ThrustEnvelopeError::InvalidAcceleration) + force.vector(), + Vector3::new(f32::MAX / 2.0, 0.0, -f32::MAX / 2.0) ); + assert!(saturated); } #[test] @@ -2021,18 +2763,20 @@ mod tests { let mut derivative_overflow = Pid::new(PidConfig { derivative: f32::MAX, ..valid - }); + }) + .unwrap(); assert_eq!( derivative_overflow.update(0.0, 2.0, 0.1), Err(PidError::NonFinite) ); - let mut integral_nan = Pid::new(valid); + let mut integral_nan = Pid::new(valid).unwrap(); integral_nan.integral_contribution = f32::NAN; assert_eq!(integral_nan.update(0.0, 0.0, 0.1), Err(PidError::NonFinite)); let mut output_overflow = Pid::new(PidConfig { proportional: f32::MAX, ..valid - }); + }) + .unwrap(); output_overflow.integral_contribution = f32::MAX; assert_eq!( output_overflow.update(1.0, 0.0, 0.1), @@ -2090,9 +2834,7 @@ mod tests { ); assert_eq!(trajectory.reference(), before); - // Each force component is finite and passes the tilt limiter, but the - // requested vector norm overflows. The collective limiter must reject - // it rather than manufacturing a finite-looking thrust command. + // Here the norm itself exceeds f32, even with scaled evaluation. let collective_norm_overflow = ThrustEnvelope { mass_kg: 1.0, maximum_collective_thrust_newtons: f32::MAX, @@ -2100,7 +2842,7 @@ mod tests { }; assert_eq!( collective_norm_overflow - .force_for_acceleration(Vector3::new(f32::MAX, 0.0, -f32::MAX / 2.0), 1.0), + .force_for_acceleration(Vector3::new(f32::MAX, 0.0, -f32::MAX), 1.0), Err(ThrustEnvelopeError::InvalidAcceleration) ); diff --git a/firmware/libs/kaptan-data-channel/Cargo.toml b/firmware/libs/kaptan-data-channel/Cargo.toml index b94e9e8..4ab708a 100644 --- a/firmware/libs/kaptan-data-channel/Cargo.toml +++ b/firmware/libs/kaptan-data-channel/Cargo.toml @@ -9,6 +9,7 @@ version.workspace = true workspace = true [dependencies] +critical-section = "1.2" kaptan-err-reportable = { path = "../kaptan-err-reportable" } kaptan-mavlink = { path = "../kaptan-mavlink", default-features = false, features = [ "sensor-telemetry", @@ -17,3 +18,6 @@ kaptan-sensor = { path = "../kaptan-sensor" } kaptan-time = { path = "../kaptan-time" } kaptan-tx = { path = "../kaptan-tx" } mavlink-core = { version = "0.18", default-features = false } + +[target.'cfg(not(target_os = "none"))'.dev-dependencies] +critical-section = { version = "1.2", features = ["std"] } diff --git a/firmware/libs/kaptan-data-channel/src/lib.rs b/firmware/libs/kaptan-data-channel/src/lib.rs index 494fd0a..cbe1947 100644 --- a/firmware/libs/kaptan-data-channel/src/lib.rs +++ b/firmware/libs/kaptan-data-channel/src/lib.rs @@ -8,19 +8,19 @@ use kaptan_err_reportable::KaptanError; use kaptan_mavlink::{MavlinkFrame, encoder::MavlinkEncoder, generated::kaptan::MavMessage}; -use kaptan_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot}; +use kaptan_sensor::{imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot}; +use kaptan_time::Timestamp; use kaptan_tx::{FrameClass, PendingFrame}; use mavlink_core::MessageData; mod mailbox; -pub use mailbox::{LatestImuMailbox, LatestOpticalFlowMailbox, LatestRangeMailbox}; +pub use mailbox::{ + Consumer, LatestImuMailbox, LatestMailbox, LatestOpticalFlowMailbox, LatestRangeMailbox, + Producer, +}; -/// Validated publication schedule consumed by the outbound data channel. -/// -/// This is a derived runtime input, not a persisted configuration root. Board -/// packages own the factory values and the platform constructs this view only -/// after validating the active user configuration against immutable hardware. +/// Publication rates in hertz. #[derive(Clone, Copy, Debug, Eq, PartialEq)] pub struct DataChannelRates { /// IMU publication rate. @@ -33,15 +33,55 @@ pub struct DataChannelRates { pub heartbeat_hz: usize, } -/// Mailboxes read by one data-channel worker. -#[derive(Clone, Copy)] +/// Mailboxes read by the single telemetry collector. pub struct DataSources<'a> { /// Latest IMU observation from the acquisition worker. - pub imu: &'a LatestImuMailbox, + pub imu: Consumer<'a, ImuSnapshot>, /// Latest range observation from either supported vertical route. - pub range: &'a LatestRangeMailbox, + pub range: Consumer<'a, RangeSnapshot>, /// Latest optical-flow observation from MTF-01. - pub optical_flow: &'a LatestOpticalFlowMailbox, + pub optical_flow: Consumer<'a, OpticalFlowSnapshot>, +} + +/// Latest collected observations shared by any number of outbound links. +#[derive(Clone, Copy, Debug, Default, Eq, PartialEq)] +pub struct TelemetrySamples { + /// Latest IMU observation. + pub imu: Option, + /// Latest range observation. + pub range: Option, + /// Latest optical-flow observation. + pub optical_flow: Option, +} + +/// Sole consumer of a set of SPSC mailboxes, retaining their latest values. +pub struct DataCollector<'a> { + sources: DataSources<'a>, + samples: TelemetrySamples, +} + +impl<'a> DataCollector<'a> { + /// Creates the single collector for these mailboxes. + pub fn new(sources: DataSources<'a>) -> Self { + Self { + sources, + samples: TelemetrySamples::default(), + } + } + + /// Collects each mailbox once, retaining values without a newer publication. + pub fn snapshot(&mut self) -> TelemetrySamples { + if let Some(imu) = self.sources.imu.take() { + self.samples.imu = Some(imu); + } + if let Some(range) = self.sources.range.take() { + self.samples.range = Some(range); + } + if let Some(flow) = self.sources.optical_flow.take() { + self.samples.optical_flow = Some(flow); + } + self.samples + } } /// Data-channel construction failure. @@ -59,80 +99,70 @@ pub enum DataChannelError { } /// Scheduled outbound MAVLink frame producer. -pub struct DataChannel<'a, const SERVICE_HZ: usize> { +pub struct DataChannel { encoder: MavlinkEncoder, schedule: Schedule, - sources: DataSources<'a>, - maximum_vertical_age_ticks: u8, - observed_imu: u32, - observed_range: u32, - last_range: Option, - range_age_ticks: u8, - observed_optical_flow: u32, - last_optical_flow: Option, - optical_flow_age_ticks: u8, + maximum_vertical_age_microseconds: u64, + last_imu_timestamp: Option, } -impl<'a, const SERVICE_HZ: usize> DataChannel<'a, SERVICE_HZ> { +impl DataChannel { /// Creates an outbound data flow from already-validated runtime inputs. pub fn new( system_id: u8, component_id: u8, rates: DataChannelRates, - maximum_vertical_age_ticks: u8, - sources: DataSources<'a>, + maximum_vertical_age_microseconds: u64, ) -> Result { let schedule = Schedule::from_rates(rates)?; Ok(Self { encoder: MavlinkEncoder::new(system_id, component_id), schedule, - sources, - maximum_vertical_age_ticks, - observed_imu: 0, - observed_range: 0, - last_range: None, - range_age_ticks: u8::MAX, - observed_optical_flow: 0, - last_optical_flow: None, - optical_flow_age_ticks: u8::MAX, + maximum_vertical_age_microseconds, + last_imu_timestamp: None, }) } /// Runs all outbound work due for one platform service tick. /// /// At most one classified frame is returned for transport admission. - pub fn on_service_tick(&mut self) -> Option> { - self.refresh_vertical_data(); - + /// IMU timestamps must increase; recreate the channel for a new clock epoch. + pub fn on_service_tick( + &mut self, + samples: &TelemetrySamples, + now: Timestamp, + ) -> Option> { match self.schedule.next_due() { Some(DueData::Imu) => { - self.sources - .imu - .take_newer(&mut self.observed_imu) - .map(|sample| PendingFrame { - class: FrameClass::BestEffort, - frame: self.encoder.imu_snapshot(&sample), - }) + let sample = samples.imu.filter(|sample| { + self.last_imu_timestamp + .is_none_or(|last| sample.timestamp > last) + })?; + self.last_imu_timestamp = Some(sample.timestamp); + Some(PendingFrame { + class: FrameClass::BestEffort, + frame: self.encoder.imu_snapshot(&sample), + }) } - Some(DueData::Distance) if self.range_age_ticks <= self.maximum_vertical_age_ticks => { - self.last_range.map(|range| PendingFrame { + Some(DueData::Distance) => samples + .range + .filter(|range| self.vertical_is_fresh(range.timestamp, now)) + .map(|range| PendingFrame { class: FrameClass::BestEffort, frame: self.encoder.range_snapshot(&range), - }) - } - Some(DueData::OpticalFlow) - if self.optical_flow_age_ticks <= self.maximum_vertical_age_ticks => - { - self.last_optical_flow.map(|flow| PendingFrame { + }), + Some(DueData::OpticalFlow) => samples + .optical_flow + .filter(|flow| self.vertical_is_fresh(flow.timestamp, now)) + .map(|flow| PendingFrame { class: FrameClass::BestEffort, frame: self.encoder.optical_flow_snapshot(&flow), - }) - } + }), Some(DueData::Heartbeat) => Some(PendingFrame { class: FrameClass::Vital, frame: self.encoder.heartbeat(), }), - Some(DueData::Distance | DueData::OpticalFlow) | None => None, + None => None, } } @@ -149,23 +179,10 @@ impl<'a, const SERVICE_HZ: usize> DataChannel<'a, SERVICE_HZ> { } } - fn refresh_vertical_data(&mut self) { - if let Some(range) = self.sources.range.take_newer(&mut self.observed_range) { - self.last_range = Some(range); - self.range_age_ticks = 0; - } else { - self.range_age_ticks = self.range_age_ticks.saturating_add(1); - } - if let Some(flow) = self - .sources - .optical_flow - .take_newer(&mut self.observed_optical_flow) - { - self.last_optical_flow = Some(flow); - self.optical_flow_age_ticks = 0; - } else { - self.optical_flow_age_ticks = self.optical_flow_age_ticks.saturating_add(1); - } + fn vertical_is_fresh(&self, timestamp: Timestamp, now: Timestamp) -> bool { + now.as_microseconds() + .checked_sub(timestamp.as_microseconds()) + .is_some_and(|age| age <= self.maximum_vertical_age_microseconds) } } @@ -245,70 +262,4 @@ impl Schedule { } #[cfg(test)] -mod tests { - use super::{ - DataChannel, DataChannelRates, DataSources, LatestImuMailbox, LatestOpticalFlowMailbox, - LatestRangeMailbox, - }; - use kaptan_mavlink::generated::kaptan::MavMessage; - use kaptan_sensor::{ - imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot, - }; - use kaptan_time::Timestamp; - use kaptan_tx::FrameClass; - - const RATES: DataChannelRates = DataChannelRates { - imu_hz: 20, - range_hz: Some(20), - optical_flow_hz: Some(20), - heartbeat_hz: 2, - }; - - #[test] - fn owns_rate_selection_encoding_and_classification() { - let imu = LatestImuMailbox::new(); - let range = LatestRangeMailbox::new(); - let flow = LatestOpticalFlowMailbox::new(); - let mut channel: DataChannel<'_, 100> = DataChannel::new( - 1, - 1, - RATES, - 10, - DataSources { - imu: &imu, - range: &range, - optical_flow: &flow, - }, - ) - .unwrap(); - let mut best_effort = 0; - let mut vital = 0; - - for tick in 0..100 { - imu.publish(ImuSnapshot::new( - Timestamp::from_microseconds(tick), - [0; 3], - [0; 3], - 0, - )); - range.publish(RangeSnapshot::new(Timestamp::from_microseconds(tick), 100)); - flow.publish(OpticalFlowSnapshot::new( - Timestamp::from_microseconds(tick), - [0; 2], - 100, - 100, - )); - if let Some(frame) = channel.on_service_tick() { - assert!(frame.frame.has_valid_crc::()); - assert_eq!(frame.frame.sequence(), best_effort + vital); - match frame.class { - FrameClass::Vital => vital += 1, - FrameClass::BestEffort => best_effort += 1, - FrameClass::Critical => unreachable!(), - } - } - } - assert_eq!(best_effort, 60); - assert_eq!(vital, 2); - } -} +mod tests; diff --git a/firmware/libs/kaptan-data-channel/src/mailbox.rs b/firmware/libs/kaptan-data-channel/src/mailbox.rs index 8f604cb..36af2b5 100644 --- a/firmware/libs/kaptan-data-channel/src/mailbox.rs +++ b/firmware/libs/kaptan-data-channel/src/mailbox.rs @@ -1,227 +1,220 @@ -//! Lock-free single-producer, single-consumer latest-value handoff. - -use core::{ - marker::PhantomData, - sync::atomic::{AtomicU32, Ordering}, -}; +//! Single-producer, single-consumer latest-value handoff. +use core::cell::Cell; +use critical_section::Mutex; use kaptan_sensor::{imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot}; -use kaptan_time::Timestamp; -/// Converts an observation to the atomic word representation used by a -/// [`LatestMailbox`]. +/// Fixed-size latest-value mailbox with one producer and one consumer. /// -/// This is a data-channel handoff contract, not a sensor-domain contract: -/// sensor crates own their fields and units, while this crate owns their -/// lock-free representation between an acquisition producer and consumer. -pub trait MailboxValue: Copy { - /// Converts this observation into its handoff representation. - fn into_words(self) -> [u32; WORDS]; - - /// Reconstructs an observation from its handoff representation. - fn from_words(words: [u32; WORDS]) -> Self; +/// A publication replaces any unread value. Each operation acquires the platform +/// critical section only to copy or take an `Option`. There are no callbacks, +/// allocation, retries, or waits inside that section. The platform must supply a +/// critical-section implementation that excludes every participating context. +/// On a host this requires the `critical-section` crate's `std` backend. +/// +/// Storage is borrowed by the endpoints and cannot move while they are in use. +/// Dropping or forgetting an endpoint does not leave a lock held. After both +/// borrows end the mailbox can be split again, retaining any unread value. +/// +/// ``` +/// use kaptan_data_channel::LatestMailbox; +/// let mut mailbox = LatestMailbox::new(); +/// let (mut producer, mut consumer) = mailbox.split(); +/// producer.publish([1, 2, 3]); +/// producer.publish([4, 5, 6]); +/// assert_eq!(consumer.take(), Some([4, 5, 6])); +/// assert_eq!(consumer.take(), None); +/// ``` +pub struct LatestMailbox { + slot: Mutex>>, } -/// Lock-free latest-value SPSC mailbox. -/// -/// Publishing a newer value intentionally replaces an unread older value: the -/// output path is best-effort and must never delay acquisition. The sequence -/// counter is a seqlock; odd values indicate a writer and even values identify -/// a stable snapshot. Exactly one producer and one consumer may use an -/// instance concurrently. Sequence operations are sequentially consistent so -/// a reader cannot validate an old even sequence around concurrently written -/// atomic payload words; the payload words themselves need only be relaxed. -pub struct LatestMailbox { - sequence: AtomicU32, - words: [AtomicU32; WORDS], - value: PhantomData, -} - -impl, const WORDS: usize> LatestMailbox { - /// Creates an empty mailbox. +impl LatestMailbox { + /// Creates an empty mailbox without acquiring a critical section. pub const fn new() -> Self { Self { - sequence: AtomicU32::new(0), - words: [const { AtomicU32::new(0) }; WORDS], - value: PhantomData, + slot: Mutex::new(Cell::new(None)), } } - /// Publishes a snapshot, replacing any unread older snapshot. - pub fn publish(&self, value: Value) { - let sequence = self.sequence.fetch_add(1, Ordering::SeqCst); - debug_assert_eq!(sequence & 1, 0, "SPSC producer must not reenter"); - - for (destination, word) in self.words.iter().zip(value.into_words()) { - destination.store(word, Ordering::Relaxed); - } - - self.sequence - .store(sequence.wrapping_add(2), Ordering::SeqCst); - } - - /// Returns the newest stable snapshot different from `observed_sequence`. - pub fn take_newer(&self, observed_sequence: &mut u32) -> Option { - let before = self.sequence.load(Ordering::SeqCst); - if before == *observed_sequence || before & 1 != 0 { - return None; - } - - let mut words = [0; WORDS]; - for (destination, source) in words.iter_mut().zip(&self.words) { - *destination = source.load(Ordering::Relaxed); - } - - if self.sequence.load(Ordering::SeqCst) != before { - return None; - } - - *observed_sequence = before; - Some(Value::from_words(words)) + /// Borrows the mailbox as exactly one producer and one consumer. + /// + /// A second split cannot overlap either endpoint's use: + /// + /// ```compile_fail,E0499 + /// use kaptan_data_channel::LatestMailbox; + /// let mut mailbox = LatestMailbox::::new(); + /// let (mut producer, _consumer) = mailbox.split(); + /// let (mut duplicate, _) = mailbox.split(); + /// producer.publish(1); + /// duplicate.publish(2); + /// ``` + pub fn split(&mut self) -> (Producer<'_, T>, Consumer<'_, T>) { + (Producer { mailbox: self }, Consumer { mailbox: self }) } } -impl, const WORDS: usize> Default for LatestMailbox { +impl Default for LatestMailbox { fn default() -> Self { Self::new() } } +/// Exclusive publication endpoint. Moving it transfers publication ownership. +/// +/// ```compile_fail,E0599 +/// use kaptan_data_channel::LatestMailbox; +/// let mut mailbox = LatestMailbox::::new(); +/// let (producer, _) = mailbox.split(); +/// let duplicate = producer.clone(); +/// ``` +pub struct Producer<'a, T: Copy> { + mailbox: &'a LatestMailbox, +} + +impl Producer<'_, T> { + /// Publishes a complete value, replacing any unread older value. + /// + /// ```compile_fail,E0596 + /// use kaptan_data_channel::LatestMailbox; + /// let mut mailbox = LatestMailbox::::new(); + /// let (producer, _) = mailbox.split(); + /// let shared = &producer; + /// shared.publish(1); + /// ``` + pub fn publish(&mut self, value: T) { + critical_section::with(|cs| self.mailbox.slot.borrow(cs).set(Some(value))); + } +} + +/// Exclusive consumption endpoint. Values are removed at most once. +/// +/// ```compile_fail,E0599 +/// use kaptan_data_channel::LatestMailbox; +/// let mut mailbox = LatestMailbox::::new(); +/// let (_, consumer) = mailbox.split(); +/// let duplicate = consumer.clone(); +/// ``` +pub struct Consumer<'a, T: Copy> { + mailbox: &'a LatestMailbox, +} + +impl Consumer<'_, T> { + /// Takes the latest unread value, or `None` if no publication is pending. + pub fn take(&mut self) -> Option { + critical_section::with(|cs| self.mailbox.slot.borrow(cs).take()) + } +} + /// Latest-value mailbox for IMU data-channel snapshots. -pub type LatestImuMailbox = LatestMailbox; +pub type LatestImuMailbox = LatestMailbox; /// Latest-value mailbox for range data-channel snapshots. -pub type LatestRangeMailbox = LatestMailbox; +pub type LatestRangeMailbox = LatestMailbox; /// Latest-value mailbox for optical-flow data-channel snapshots. -pub type LatestOpticalFlowMailbox = LatestMailbox; - -impl MailboxValue<9> for ImuSnapshot { - fn into_words(self) -> [u32; 9] { - let timestamp = self.timestamp.as_microseconds(); - [ - timestamp as u32, - (timestamp >> 32) as u32, - self.acceleration_milligravity[0] as u16 as u32, - self.acceleration_milligravity[1] as u16 as u32, - self.acceleration_milligravity[2] as u16 as u32, - self.angular_rate_milliradians_per_second[0] as u16 as u32, - self.angular_rate_milliradians_per_second[1] as u16 as u32, - self.angular_rate_milliradians_per_second[2] as u16 as u32, - self.temperature_centidegrees_celsius as u16 as u32, - ] - } - - fn from_words(words: [u32; 9]) -> Self { - Self::new( - timestamp_from_words(words[0], words[1]), - [ - words[2] as u16 as i16, - words[3] as u16 as i16, - words[4] as u16 as i16, - ], - [ - words[5] as u16 as i16, - words[6] as u16 as i16, - words[7] as u16 as i16, - ], - words[8] as u16 as i16, - ) - } -} - -impl MailboxValue<3> for RangeSnapshot { - fn into_words(self) -> [u32; 3] { - let timestamp = self.timestamp.as_microseconds(); - [ - timestamp as u32, - (timestamp >> 32) as u32, - self.distance_millimetres, - ] - } - - fn from_words(words: [u32; 3]) -> Self { - Self::new(timestamp_from_words(words[0], words[1]), words[2]) - } -} - -impl MailboxValue<6> for OpticalFlowSnapshot { - fn into_words(self) -> [u32; 6] { - let timestamp = self.timestamp.as_microseconds(); - [ - timestamp as u32, - (timestamp >> 32) as u32, - self.velocity_millimetres_per_second[0] as u32, - self.velocity_millimetres_per_second[1] as u32, - self.ground_distance_millimetres, - u32::from(self.quality), - ] - } - - fn from_words(words: [u32; 6]) -> Self { - Self::new( - timestamp_from_words(words[0], words[1]), - [words[2] as i32, words[3] as i32], - words[4], - words[5] as u8, - ) - } -} - -const fn timestamp_from_words(low: u32, high: u32) -> Timestamp { - Timestamp::from_microseconds((low as u64) | ((high as u64) << 32)) -} +pub type LatestOpticalFlowMailbox = LatestMailbox; #[cfg(test)] mod tests { - use super::{LatestImuMailbox, LatestOpticalFlowMailbox, LatestRangeMailbox}; - use kaptan_sensor::{ - imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot, - }; + extern crate std; + + use super::*; use kaptan_time::Timestamp; + use std::thread; #[test] - fn publishes_only_the_newest_unread_imu_sample() { - let mailbox = LatestImuMailbox::new(); - let mut observed = 0; - mailbox.publish(ImuSnapshot::new( - Timestamp::from_microseconds(1), - [1; 3], - [2; 3], - 3, - )); - mailbox.publish(ImuSnapshot::new( - Timestamp::from_microseconds(2), - [4; 3], - [5; 3], - 6, - )); - - let sample = mailbox.take_newer(&mut observed).unwrap(); - assert_eq!(sample.timestamp, Timestamp::from_microseconds(2)); - assert_eq!(sample.acceleration_milligravity, [4; 3]); - assert!(mailbox.take_newer(&mut observed).is_none()); - } - - #[test] - fn publishes_only_the_newest_unread_range_sample() { - let mailbox = LatestRangeMailbox::new(); - let mut observed = 0; - mailbox.publish(RangeSnapshot::new(Timestamp::from_microseconds(1), 100)); - mailbox.publish(RangeSnapshot::new(Timestamp::from_microseconds(2), 200)); - - assert_eq!( - mailbox.take_newer(&mut observed), - Some(RangeSnapshot::new(Timestamp::from_microseconds(2), 200)) + fn latest_publication_overwrites_and_is_taken_once() { + let mut mailbox = LatestImuMailbox::new(); + let (mut producer, mut consumer) = mailbox.split(); + assert_eq!(consumer.take(), None); + let first = ImuSnapshot::new(Timestamp::from_microseconds(1), [1; 3], [2; 3], 3); + let latest = ImuSnapshot::new( + Timestamp::from_microseconds(u64::MAX), + [-32_768, 0, 32_767], + [-34_906, 0, 34_906], + -2_501, ); - assert!(mailbox.take_newer(&mut observed).is_none()); + producer.publish(first); + producer.publish(latest); + assert_eq!(consumer.take(), Some(latest)); + assert_eq!(consumer.take(), None); + producer.publish(latest); + assert_eq!(consumer.take(), Some(latest)); } #[test] - fn preserves_optical_flow_fields() { - let mailbox = LatestOpticalFlowMailbox::new(); - let mut observed = 0; - let sample = OpticalFlowSnapshot::new(Timestamp::from_microseconds(1), [-6, 7], 8, 9); - mailbox.publish(sample); + fn vertical_snapshots_preserve_all_fields() { + let mut range = LatestRangeMailbox::new(); + let mut flow = LatestOpticalFlowMailbox::new(); + let (mut range_producer, mut range_consumer) = range.split(); + let (mut flow_producer, mut flow_consumer) = flow.split(); + let timestamp = Timestamp::from_microseconds(u64::MAX); + let range = RangeSnapshot::new(timestamp, u32::MAX); + let flow = OpticalFlowSnapshot::new(timestamp, [i32::MIN, i32::MAX], u32::MAX, 255); + range_producer.publish(range); + flow_producer.publish(flow); + assert_eq!(range_consumer.take(), Some(range)); + assert_eq!(flow_consumer.take(), Some(flow)); + } - assert_eq!(mailbox.take_newer(&mut observed), Some(sample)); + #[test] + // Forget is deliberate: endpoint safety must never depend on destruction. + #[allow(clippy::forget_non_drop)] + fn moving_dropping_and_forgetting_endpoints_needs_no_cleanup() { + let mut mailbox = LatestMailbox::new(); + { + let mut consumer = { + let (producer, consumer) = mailbox.split(); + let mut moved = producer; + moved.publish(1); + consumer + }; + // The producer left scope before its final publication is taken. + assert_eq!(consumer.take(), Some(1)); + assert_eq!(consumer.take(), None); + } + { + let (mut producer, consumer) = mailbox.split(); + core::mem::forget(consumer); + producer.publish(2); + producer.publish(3); + core::mem::forget(producer); + } + // Neither endpoint owns a guard or controls the backing storage lifetime. + let mut moved_mailbox = mailbox; + let (_, mut consumer) = moved_mailbox.split(); + assert_eq!(consumer.take(), Some(3)); + assert_eq!(consumer.take(), None); + } + + #[test] + fn owned_endpoints_publish_coherent_values_across_host_threads() { + const LAST: u64 = 50_000; + let mut mailbox = LatestMailbox::<[u64; 16]>::new(); + let (mut producer, mut consumer) = mailbox.split(); + thread::scope(|scope| { + let writer = scope.spawn(move || { + for value in 1..=LAST { + producer.publish([value; 16]); + } + }); + let mut last = 0; + let mut observe = |value: [u64; 16]| { + assert_eq!(value, [value[0]; 16], "torn publication"); + assert!(value[0] > last, "duplicate or reordered publication"); + last = value[0]; + }; + while !writer.is_finished() { + if let Some(value) = consumer.take() { + observe(value); + } + thread::yield_now(); + } + writer.join().unwrap(); + if let Some(value) = consumer.take() { + observe(value); + } + assert_eq!(last, LAST); + assert_eq!(consumer.take(), None); + }); } } diff --git a/firmware/libs/kaptan-data-channel/src/tests.rs b/firmware/libs/kaptan-data-channel/src/tests.rs new file mode 100644 index 0000000..fce97e0 --- /dev/null +++ b/firmware/libs/kaptan-data-channel/src/tests.rs @@ -0,0 +1,442 @@ +use super::{ + DataChannel, DataChannelError, DataChannelRates, DataCollector, DataSources, LatestImuMailbox, + LatestOpticalFlowMailbox, LatestRangeMailbox, Producer, TelemetrySamples, +}; +use kaptan_mavlink::{ + MavlinkFrame, + generated::kaptan::{HEARTBEAT_DATA, HeartbeatState, MavMessage}, + receiver::IntegrityMavlinkReceiver, +}; +use kaptan_sensor::{imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot}; +use kaptan_time::Timestamp; +use kaptan_tx::{EnqueueResult, FrameClass, PendingFrame, TransmitQueue}; + +const RATES: DataChannelRates = DataChannelRates { + imu_hz: 20, + range_hz: Some(20), + optical_flow_hz: Some(20), + heartbeat_hz: 2, +}; + +const ALL_STREAMS: DataChannelRates = DataChannelRates { + imu_hz: 1, + range_hz: Some(1), + optical_flow_hz: Some(1), + heartbeat_hz: 1, +}; + +const HEARTBEAT: HEARTBEAT_DATA = HEARTBEAT_DATA { + state: HeartbeatState::HEARTBEAT_STATE_INITIALIZED_OUTPUTS_INHIBITED, +}; + +#[derive(Default)] +struct Mailboxes { + imu: LatestImuMailbox, + range: LatestRangeMailbox, + optical_flow: LatestOpticalFlowMailbox, +} + +impl Mailboxes { + fn split(&mut self) -> (Publishers<'_>, DataCollector<'_>) { + let (imu_producer, imu) = self.imu.split(); + let (range_producer, range) = self.range.split(); + let (flow_producer, optical_flow) = self.optical_flow.split(); + ( + Publishers { + imu: imu_producer, + range: range_producer, + optical_flow: flow_producer, + }, + DataCollector::new(DataSources { + imu, + range, + optical_flow, + }), + ) + } +} + +struct Publishers<'a> { + imu: Producer<'a, ImuSnapshot>, + range: Producer<'a, RangeSnapshot>, + optical_flow: Producer<'a, OpticalFlowSnapshot>, +} + +impl Publishers<'_> { + fn publish(&mut self, samples: TelemetrySamples) { + if let Some(imu) = samples.imu { + self.imu.publish(imu); + } + if let Some(range) = samples.range { + self.range.publish(range); + } + if let Some(flow) = samples.optical_flow { + self.optical_flow.publish(flow); + } + } +} + +fn samples_at(microseconds: u64) -> TelemetrySamples { + let timestamp = Timestamp::from_microseconds(microseconds); + TelemetrySamples { + imu: Some(ImuSnapshot::new( + timestamp, + [-1_001, 2, 3], + [4, -5_006, 7], + 2_501, + )), + range: Some(RangeSnapshot::new(timestamp, 2_913)), + optical_flow: Some(OpticalFlowSnapshot::new(timestamp, [-123, 456], 2_917, 201)), + } +} + +fn decode(pending: &PendingFrame) -> MavMessage { + assert!(pending.frame.has_valid_crc::()); + let mut receiver = IntegrityMavlinkReceiver::<1>::new(); + for byte in pending.frame.raw_bytes() { + receiver.receive_byte(*byte); + } + receiver.take_received().unwrap().decode().unwrap() +} + +fn assert_sample_frame(pending: &PendingFrame, samples: &TelemetrySamples) -> usize { + let index = match decode(pending) { + MavMessage::IMU(data) => { + let imu = samples.imu.unwrap(); + assert_eq!(data.time_us, imu.timestamp.as_microseconds()); + assert_eq!( + data.acceleration_milligravity, + imu.acceleration_milligravity + ); + assert_eq!( + data.angular_rate_milliradians_per_second, + imu.angular_rate_milliradians_per_second + ); + assert_eq!( + data.temperature_centidegrees_celsius, + imu.temperature_centidegrees_celsius + ); + 0 + } + MavMessage::RANGE(data) => { + let range = samples.range.unwrap(); + assert_eq!(data.time_us, range.timestamp.as_microseconds()); + assert_eq!(data.distance_millimetres, range.distance_millimetres); + 1 + } + MavMessage::OPTICAL_FLOW(data) => { + let flow = samples.optical_flow.unwrap(); + assert_eq!(data.time_us, flow.timestamp.as_microseconds()); + assert_eq!( + data.velocity_millimetres_per_second, + flow.velocity_millimetres_per_second + ); + assert_eq!( + data.ground_distance_millimetres, + flow.ground_distance_millimetres + ); + assert_eq!(data.quality, flow.quality); + 2 + } + MavMessage::HEARTBEAT(data) => { + assert_eq!(data.state, HEARTBEAT.state); + 3 + } + _ => panic!("unexpected telemetry message"), + }; + assert_eq!( + pending.class, + if index == 3 { + FrameClass::Vital + } else { + FrameClass::BestEffort + } + ); + index +} + +#[test] +fn collector_retains_latest_samples_between_publications() { + let mut mailboxes = Mailboxes::default(); + let (mut publishers, mut collector) = mailboxes.split(); + assert_eq!(collector.snapshot(), TelemetrySamples::default()); + + publishers.publish(samples_at(1)); + publishers.publish(samples_at(2)); + let first = collector.snapshot(); + assert_eq!(first, samples_at(2)); + assert_eq!(collector.snapshot(), first); + + let range = RangeSnapshot::new(Timestamp::from_microseconds(3), 4_567); + publishers.range.publish(range); + let updated = TelemetrySamples { + range: Some(range), + ..first + }; + assert_eq!(collector.snapshot(), updated); + assert_eq!(collector.snapshot(), updated); + assert_eq!(first, samples_at(2)); +} + +#[test] +fn owns_rate_selection_encoding_and_classification() { + let mut mailboxes = Mailboxes::default(); + let (mut publishers, mut collector) = mailboxes.split(); + let mut channel = DataChannel::<100>::new(1, 1, RATES, 100_000).unwrap(); + let mut counts = [0; 4]; + let mut sequence = 0; + + for tick in 0..100 { + let now = Timestamp::from_microseconds(tick * 10_000); + publishers.publish(samples_at(now.as_microseconds())); + let samples = collector.snapshot(); + if let Some(frame) = channel.on_service_tick(&samples, now) { + assert_eq!(frame.frame.sequence(), sequence); + sequence += 1; + counts[assert_sample_frame(&frame, &samples)] += 1; + } + } + assert_eq!(counts, [20, 20, 20, 2]); +} + +#[test] +fn links_share_one_snapshot_with_independent_rates_and_sequences() { + let mut mailboxes = Mailboxes::default(); + let (mut publishers, mut collector) = mailboxes.split(); + let mut fast = DataChannel::<100>::new(1, 2, RATES, 100_000).unwrap(); + let mut slow = DataChannel::<100>::new( + 3, + 4, + DataChannelRates { + imu_hz: 10, + range_hz: Some(5), + optical_flow_hz: None, + heartbeat_hz: 1, + }, + 100_000, + ) + .unwrap(); + let external = slow.encode_message(FrameClass::Critical, &HEARTBEAT); + assert_eq!(external.class, FrameClass::Critical); + assert_eq!(external.frame.sequence(), 0); + assert!(matches!(decode(&external), MavMessage::HEARTBEAT(_))); + let mut sequences = [0, 1]; + let mut counts = [[0; 4]; 2]; + + for tick in 0..100 { + let now = Timestamp::from_microseconds(tick * 10_000); + publishers.publish(samples_at(now.as_microseconds())); + let samples = collector.snapshot(); + for (link, channel) in [&mut fast, &mut slow].into_iter().enumerate() { + if let Some(frame) = channel.on_service_tick(&samples, now) { + assert_eq!(frame.frame.sequence(), sequences[link]); + assert_eq!( + &frame.frame.raw_bytes()[5..7], + if link == 0 { &[1, 2] } else { &[3, 4] } + ); + sequences[link] += 1; + counts[link][assert_sample_frame(&frame, &samples)] += 1; + } + } + } + assert_eq!(counts, [[20, 20, 20, 2], [10, 5, 0, 1]]); + assert_eq!(sequences, [62, 17]); +} + +#[test] +fn stalled_and_backpressured_link_does_not_consume_another_links_data() { + let mut mailboxes = Mailboxes::default(); + let (mut publishers, mut collector) = mailboxes.split(); + let mut healthy = DataChannel::<4>::new(1, 1, ALL_STREAMS, 100_000).unwrap(); + let mut blocked = DataChannel::<4>::new(1, 2, ALL_STREAMS, 100_000).unwrap(); + let mut queue = TransmitQueue::<_, 1>::new(); + assert_eq!( + queue.enqueue(blocked.encode_message(FrameClass::Vital, &HEARTBEAT)), + EnqueueResult::Enqueued + ); + let mut counts = [[0; 4]; 2]; + for tick in 0..8 { + let now = Timestamp::from_microseconds(tick * 250_000); + publishers.publish(samples_at(now.as_microseconds())); + let samples = collector.snapshot(); + if tick >= 4 { + let frame = blocked.on_service_tick(&samples, now).unwrap(); + assert_eq!(frame.frame.sequence(), (tick - 3) as u8); + counts[1][assert_sample_frame(&frame, &samples)] += 1; + assert_eq!( + queue.enqueue(frame), + if frame.class == FrameClass::Vital { + EnqueueResult::VitalQueueFull + } else { + EnqueueResult::BestEffortDropped + } + ); + } + let frame = healthy.on_service_tick(&samples, now).unwrap(); + assert_eq!(frame.frame.sequence(), tick as u8); + counts[0][assert_sample_frame(&frame, &samples)] += 1; + } + assert_eq!(counts, [[2; 4], [1; 4]]); +} + +#[test] +fn imu_requires_a_strictly_newer_timestamp_even_after_clock_regression() { + let mut channel = DataChannel::<2>::new( + 1, + 1, + DataChannelRates { + range_hz: None, + optical_flow_hz: None, + ..ALL_STREAMS + }, + 100_000, + ) + .unwrap(); + let mut sequence = 0; + for (timestamp, expected) in [ + (Some(0), true), + (Some(0), false), + (None, false), + (Some(0), false), + (Some(1), true), + (Some(0), false), + (Some(2), true), + ] { + let now = Timestamp::from_microseconds(timestamp.unwrap_or(0)); + let mut samples = samples_at(now.as_microseconds()); + samples.imu = timestamp.map(|_| ImuSnapshot::new(now, [sequence as i16; 3], [0; 3], 0)); + let frame = channel.on_service_tick(&samples, now); + assert_eq!(frame.is_some(), expected); + if let Some(frame) = frame { + assert_eq!(frame.frame.sequence(), sequence); + sequence += 1; + assert_eq!(assert_sample_frame(&frame, &samples), 0); + } + let heartbeat = channel.on_service_tick(&samples, now).unwrap(); + assert_eq!(heartbeat.frame.sequence(), sequence); + sequence += 1; + assert_eq!(assert_sample_frame(&heartbeat, &samples), 3); + } +} + +fn assert_vertical_cycle( + channel: &mut DataChannel<4>, + samples: &TelemetrySamples, + now: Timestamp, + expected: [bool; 2], +) { + channel.on_service_tick(samples, now); + let heartbeat = channel.on_service_tick(samples, now).unwrap(); + assert_eq!(assert_sample_frame(&heartbeat, samples), 3); + for (index, expected) in expected.into_iter().enumerate() { + let frame = channel.on_service_tick(samples, now); + assert_eq!(frame.is_some(), expected); + if let Some(frame) = frame { + assert_eq!(assert_sample_frame(&frame, samples), index + 1); + } + } +} + +#[test] +fn vertical_freshness_uses_sample_time_on_first_collection() { + for (sample_time, now, maximum_age, expected) in [ + (0, 100_001, 100_000, false), + (100_001, 100_000, 100_000, false), + (u64::MAX, 0, u64::MAX, false), + (0, 100_000, 100_000, true), + (42, 42, 0, true), + (42, 43, 0, false), + ] { + let mut mailboxes = Mailboxes::default(); + let (mut publishers, mut collector) = mailboxes.split(); + publishers.publish(samples_at(sample_time)); + let samples = collector.snapshot(); + let mut channel = DataChannel::<4>::new(1, 1, ALL_STREAMS, maximum_age).unwrap(); + assert_vertical_cycle( + &mut channel, + &samples, + Timestamp::from_microseconds(now), + [expected; 2], + ); + } +} + +#[test] +fn repeated_ticks_and_republication_do_not_change_sample_age() { + let mut mailboxes = Mailboxes::default(); + let (mut publishers, mut collector) = mailboxes.split(); + let mut channel = DataChannel::<4>::new(1, 1, ALL_STREAMS, 100_000).unwrap(); + for (now, expected, repetitions) in [ + (110_000, true, 64), + (110_001, false, 1), + (1_000_000, false, 1), + ] { + for _ in 0..repetitions { + publishers.publish(samples_at(10_000)); + let samples = collector.snapshot(); + assert_vertical_cycle( + &mut channel, + &samples, + Timestamp::from_microseconds(now), + [expected; 2], + ); + } + } +} + +#[test] +fn vertical_streams_use_their_own_sample_timestamps() { + for (range_time, flow_time, expected) in [ + (99_999, 100_000, [false, true]), + (200_000, 200_001, [true, false]), + ] { + let samples = TelemetrySamples { + range: samples_at(range_time).range, + optical_flow: samples_at(flow_time).optical_flow, + ..TelemetrySamples::default() + }; + let mut channel = DataChannel::<4>::new(1, 1, ALL_STREAMS, 100_000).unwrap(); + assert_vertical_cycle( + &mut channel, + &samples, + Timestamp::from_microseconds(200_000), + expected, + ); + } +} + +#[test] +fn invalid_rates_keep_existing_errors() { + for (rates, expected) in [ + ( + DataChannelRates { imu_hz: 0, ..RATES }, + DataChannelError::ZeroRate, + ), + ( + DataChannelRates { + range_hz: Some(0), + ..RATES + }, + DataChannelError::ZeroRate, + ), + ( + DataChannelRates { + imu_hz: 101, + ..RATES + }, + DataChannelError::RateExceedsServiceFrequency, + ), + ( + DataChannelRates { + imu_hz: 100, + ..RATES + }, + DataChannelError::ScheduleFull, + ), + ] { + assert_eq!( + DataChannel::<100>::new(1, 1, rates, 100_000).err(), + Some(expected) + ); + } +} diff --git a/firmware/libs/kaptan-diagnostics-mavlink/src/lib.rs b/firmware/libs/kaptan-diagnostics-mavlink/src/lib.rs index 1ed9683..4cd4467 100644 --- a/firmware/libs/kaptan-diagnostics-mavlink/src/lib.rs +++ b/firmware/libs/kaptan-diagnostics-mavlink/src/lib.rs @@ -1,8 +1,6 @@ //! MAVLink projection for Kaptan logs and reportable firmware errors. //! -//! This adapter creates generated payload data only. A platform-owned link is -//! solely responsible for MAVLink framing, bounded admission, DMA, and retry -//! policy. +//! Produces payloads. The caller handles framing and transmission. #![no_std] @@ -15,7 +13,7 @@ use kaptan_mavlink::generated::kaptan::{ERROR_DATA, LOG_DATA, LogFlags, LogLevel const ERROR_RAISED_AT_BYTES: usize = 96; -/// Encodes one ordinary log record as its sole wire message. +/// Encodes one log record. pub fn encode_log(record: &LogRecord) -> LOG_DATA { LOG_DATA { level: wire_level(record.level()), @@ -29,7 +27,7 @@ pub fn encode_log(record: &LogRecord) -> LOG_DATA { } } -/// Encodes one reportable firmware error as its sole wire message. +/// Encodes one reportable firmware error. pub fn encode_error(record: &ErrorRecord) -> ERROR_DATA { let source = record.raised_at().as_bytes(); let mut length = source.len().min(ERROR_RAISED_AT_BYTES); diff --git a/firmware/libs/kaptan-dma/src/lib.rs b/firmware/libs/kaptan-dma/src/lib.rs deleted file mode 100644 index 80361e9..0000000 --- a/firmware/libs/kaptan-dma/src/lib.rs +++ /dev/null @@ -1,73 +0,0 @@ -//! DMA memory ownership and non-blocking transfer contracts. -//! -//! Board/HAL code proves the memory region, cache policy, and peripheral route. -//! Runtime code sees only prepared immutable TX ranges and terminal outcomes. - -#![no_std] - -/// A borrowed byte range prepared for a device-to-read DMA transfer. -/// -/// This is a capability rather than a general byte-slice wrapper. It exists -/// only after a platform-selected policy proves DMA reachability and CPU-to-device -/// visibility for the selected controller. -pub struct DmaReadableBuffer<'a> { - bytes: &'a [u8], -} - -impl<'a> DmaReadableBuffer<'a> { - /// Creates a DMA-read capability after platform-specific validation. - /// - /// # Safety - /// - /// - `bytes` is reachable by the selected DMA engine. - /// - CPU writes to `bytes` are visible before transfer begins. - /// - `bytes` remains addressable, immovable, and immutable until a terminal - /// completion or a documented route-halt outcome releases ownership. - pub const unsafe fn new_unchecked(bytes: &'a [u8]) -> Self { - Self { bytes } - } - - /// Returns the DMA-visible byte range. - pub const fn bytes(&self) -> &'a [u8] { - self.bytes - } -} - -/// Prepares CPU-owned bytes for a DMA transmit handoff. -/// -/// A cache-disabled board validates the RAM range. A cached board must clean -/// the exact range and issue the required ordering barrier before returning. -pub trait DmaTxBufferFactory { - /// Platform-specific preparation failure. - type Error; - - /// Validates and prepares a frame for device DMA reads. - fn prepare<'a>(&mut self, bytes: &'a [u8]) -> Result, Self::Error>; -} - -/// Terminal result reported by a DMA transmit interrupt handler. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub enum TxCompletion { - /// The selected engine has no terminal event pending. - NotPending, - /// DMA reached the peripheral data-register endpoint. - /// - /// This does not prove that the final UART stop bit left the wire. - Complete, - /// The engine reported a terminal transfer fault. - TransferFault, - /// The route entered a halted state and requires explicit reinitialization. - Halted, -} - -/// Owns one non-blocking DMA-backed transmit route. -/// -/// The caller retains the prepared range until a terminal completion or fault. -/// Implementations must not poll for a hardware state transition from this API. -pub trait DmaTxEngine { - /// Start-operation failure. - type StartError; - - /// Starts one DMA transfer from a prepared readable buffer. - fn start(&mut self, buffer: DmaReadableBuffer<'_>) -> Result<(), Self::StartError>; -} diff --git a/firmware/libs/kaptan-drivers/src/icm42688/error.rs b/firmware/libs/kaptan-drivers/src/icm42688/error.rs index 5f15a17..31b3a61 100644 --- a/firmware/libs/kaptan-drivers/src/icm42688/error.rs +++ b/firmware/libs/kaptan-drivers/src/icm42688/error.rs @@ -18,4 +18,16 @@ pub enum Error { /// The selected SPI route reported a terminal transfer fault. #[kaptan_error(id = 5104)] TransferFault, + /// The bus could not safely accept and copy out the completed transfer. + #[kaptan_error(id = 5105)] + Completion(kaptan_spi::CompletionError), + /// An accepted SPI transfer exceeded the 10 ms service deadline. + #[kaptan_error(id = 5106)] + TransferTimeout, + /// The bus stayed busy for the pending operation's 20 ms admission budget. + #[kaptan_error(id = 5107)] + AdmissionTimeout, + /// No acquisition was requested within two output periods plus 10 ms. + #[kaptan_error(id = 5108)] + DataReadyTimeout, } diff --git a/firmware/libs/kaptan-drivers/src/icm42688/mod.rs b/firmware/libs/kaptan-drivers/src/icm42688/mod.rs index 48b32ce..3aeeb2d 100644 --- a/firmware/libs/kaptan-drivers/src/icm42688/mod.rs +++ b/firmware/libs/kaptan-drivers/src/icm42688/mod.rs @@ -5,7 +5,7 @@ mod sample; pub mod service; use kaptan_sensor::imu::ImuSnapshot; -use kaptan_spi::{SpiCompletion, SpiDevice}; +use kaptan_spi::{CompletionError, SpiCompletion, SpiDevice}; use kaptan_time::Timestamp; pub use error::Error; @@ -27,6 +27,19 @@ const INTF_CONFIG1: u8 = 0x4d; const TEMP_DATA1: u8 = 0x1d; const SAMPLE_BYTES: usize = 14; const TRANSFER_CAPACITY: usize = SAMPLE_BYTES + 1; +// TDK DS-000347 rev 1.5 §14.1 p.64: soft reset requires 1 ms. §3.3.2 p.14 +// also requires up to 1 ms before power-on register access. §14.36 p.76 +// requires 200 us without writes after OFF -> ON and at least 45 ms gyro ON +// time. Use 45 ms as a conservative start-up allowance over §3.1/3.2's +// TYPICAL 30/10 ms gyro/accel start-up, not a physical settling guarantee. +const RESET_SETTLE_US: u64 = 1_000; +const SENSOR_SETTLE_US: u64 = 45_000; +const SAMPLE_PERIOD_US: u64 = 1_000; +// Host service budget, not a datasheet delay. Supported SPI3 bursts take less +// than 0.3 ms. Periodic service must continue independently of DMA and DRDY. +const TRANSFER_TIMEOUT_US: u64 = 10_000; +// Allows a retry on the F722's 10 ms periodic service before admission expires. +const ADMISSION_TIMEOUT_US: u64 = 20_000; // SPI mode 0, active-high push-pull, pulsed INT1. Data-ready is cleared by // the SPI burst read and its source is explicitly enabled on INT1. @@ -47,19 +60,40 @@ const CONFIGURATION: [(u8, u8); 9] = [ #[derive(Clone, Copy, Debug, Eq, PartialEq)] enum State { Dormant, - Resetting, - WaitingForReset, - ReadingIdentity, - WritingConfiguration(usize), - WaitingForSensorStart, - Ready, - ReadingSample { timestamp: Timestamp }, + Pending { + operation: Operation, + since: Timestamp, + }, + Active { + operation: Operation, + since: Timestamp, + }, + Waiting { + next: Operation, + since: Timestamp, + delay_us: u64, + }, + Settling { + since: Timestamp, + }, + Ready { + since: Timestamp, + }, + Faulted, } -/// Observable outcome of one ICM-42688-P SPI terminal event. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +enum Operation { + Reset, + Identity, + Configure(usize), + Sample, +} + +/// Observable outcome of one timer or ICM-42688-P SPI terminal event. #[derive(Debug, Eq, PartialEq)] pub enum Icm42688Event { - /// The selected SPI interrupt had no terminal transfer event. + /// No sample, initialization completion or terminal fault was produced. NotPending, /// One initialization transfer completed; initialization continues. Initializing, @@ -73,9 +107,8 @@ pub enum Icm42688Event { /// Configured ICM-42688-P driver. /// -/// The driver owns fixed transfer buffers and progresses exclusively from -/// timer, data-ready and SPI-DMA completion events. It never waits for a -/// peripheral status flag. +/// The driver owns ordinary movable staging buffers and progresses from timer, +/// data-ready and completion events. Hardware never addresses these buffers. pub struct Icm42688 { bus: Bus, state: State, @@ -96,125 +129,139 @@ impl Icm42688 { } impl Icm42688 { - /// Starts the sensor reset. A subsequent periodic tick provides the - /// datasheet-required reset-settling interval before identification. - pub fn begin_initialization(&mut self) -> Result<(), Error> { + /// Accepts initialization once. Busy admission is retried by periodic + /// service. All supplied instants must use one non-regressing boot clock. + /// Power must be stable by this call. Register access waits another 1 ms. + pub fn begin_initialization(&mut self, now: Timestamp) -> Result<(), Error> { if self.state != State::Dormant { return Err(Error::NotReady); } - self.prepare_write(DEVICE_CONFIG, SOFT_RESET); - self.submit(2)?; - self.state = State::Resetting; + self.wait(Operation::Reset, now, RESET_SETTLE_US); Ok(()) } - /// Advances reset/wake delays from the board periodic event. - pub fn advance_initialization(&mut self) -> Result<(), Error> { + /// Services elapsed settling, admission and acquisition deadlines. Call + /// periodically even with an active transfer. No unsolicited reads occur. + pub fn on_periodic_tick(&mut self, now: Timestamp) -> Icm42688Event { + if let Some(error) = self.check_deadline(now) { + return Icm42688Event::Fault(error); + } match self.state { - State::WaitingForReset => { - self.prepare_read(WHO_AM_I, 2); - self.submit(2)?; - self.state = State::ReadingIdentity; + State::Waiting { + next, + since, + delay_us, + } if elapsed(now, since, delay_us) => { + self.queue(next, now); } - State::WaitingForSensorStart => { - self.start_configuration(7)?; + State::Settling { since } if elapsed(now, since, SAMPLE_PERIOD_US) => { + self.state = State::Ready { since: now }; + return Icm42688Event::Initialized; } _ => {} } - Ok(()) + match self.submit_pending(now) { + Ok(()) => Icm42688Event::NotPending, + Err(error) => Icm42688Event::Fault(error), + } } - /// Starts one temperature, accelerometer and gyroscope burst read. - pub fn request_sample(&mut self, timestamp: Timestamp) -> Result<(), Error> { - if self.state != State::Ready { + /// Accepts a burst-read request. Busy admission remains pending for periodic + /// retry. The eventual accepted submission time becomes the snapshot + /// timestamp, not the ISR time or a physical sampling epoch. + pub fn request_sample(&mut self, now: Timestamp) -> Result<(), Error> { + if let Some(error) = self.check_deadline(now) { + return Err(error); + } + if !self.is_ready() { return Err(Error::NotReady); } - self.prepare_read(TEMP_DATA1, TRANSFER_CAPACITY); - self.submit(TRANSFER_CAPACITY)?; - self.state = State::ReadingSample { timestamp }; - Ok(()) + self.queue(Operation::Sample, now); + self.submit_pending(now) } /// Advances the state machine from an already-acknowledged SPI completion. pub fn on_transfer_complete( &mut self, completion: SpiCompletion, + now: Timestamp, ) -> Icm42688Event { - self.bus.complete_transfer(completion); - match completion { - SpiCompletion::NotPending => Icm42688Event::NotPending, - SpiCompletion::TransferFault => { - self.state = State::Dormant; + if completion == SpiCompletion::NotPending { + return Icm42688Event::NotPending; + } + if let Some(error) = self.check_deadline(now) { + return Icm42688Event::Fault(error); + } + let State::Active { operation, since } = self.state else { + return Icm42688Event::NotPending; + }; + let length = if operation == Operation::Sample { + TRANSFER_CAPACITY + } else { + 2 + }; + match self + .bus + .complete_transfer(completion, &mut self.receive[..length]) + { + Ok(()) => self.advance_after_completion(operation, since, now), + Err(CompletionError::NotPending) => Icm42688Event::NotPending, + Err(CompletionError::TransferFault) => { + self.state = State::Faulted; Icm42688Event::Fault(Error::TransferFault) } - SpiCompletion::Complete => self.advance_after_completion(), + Err(error) => { + self.state = State::Faulted; + Icm42688Event::Fault(Error::Completion(error)) + } } } /// Returns whether an IMU data-ready event can start a sensor transfer. pub fn is_ready(&self) -> bool { - self.state == State::Ready + matches!(self.state, State::Ready { .. }) } - fn advance_after_completion(&mut self) -> Icm42688Event { - match self.state { - State::Dormant => Icm42688Event::NotPending, - State::Resetting => { - self.state = State::WaitingForReset; - Icm42688Event::Initializing - } - State::WaitingForReset | State::WaitingForSensorStart => Icm42688Event::NotPending, - State::ReadingIdentity => { + fn advance_after_completion( + &mut self, + operation: Operation, + requested: Timestamp, + now: Timestamp, + ) -> Icm42688Event { + match operation { + Operation::Reset => self.wait(Operation::Identity, now, RESET_SETTLE_US), + Operation::Identity => { let identity = self.receive[1]; if identity != EXPECTED_WHO_AM_I { - self.state = State::Dormant; + self.state = State::Faulted; return Icm42688Event::Fault(Error::UnexpectedIdentity(identity)); } - self.start_configuration_event(0) + self.queue(Operation::Configure(0), now); } - State::WritingConfiguration(index) => { + Operation::Configure(index) => { let next = index + 1; if next == 7 { - // PWR_MGMT0 starts both sensor paths. The following - // timer tick gives them more than the required 200 µs - // before their output-rate configuration is written. - self.state = State::WaitingForSensorStart; - Icm42688Event::Initializing + // Includes the 200 us no-write interval after PWR_MGMT0. + self.wait(Operation::Configure(next), now, SENSOR_SETTLE_US); } else if next == CONFIGURATION.len() { - self.state = State::Ready; - Icm42688Event::Initialized + self.state = State::Settling { since: now }; } else { - self.start_configuration_event(next) + self.queue(Operation::Configure(next), now); } } - State::Ready => Icm42688Event::NotPending, - State::ReadingSample { timestamp } => { + Operation::Sample => { let mut bytes = [0; SAMPLE_BYTES]; bytes.copy_from_slice(&self.receive[1..]); - self.state = State::Ready; - Icm42688Event::Sample(sample::decode(&bytes, timestamp)) + self.state = State::Ready { since: now }; + return Icm42688Event::Sample(sample::decode(&bytes, requested)); } } - } - - fn start_configuration_event(&mut self, index: usize) -> Icm42688Event { - match self.start_configuration(index) { + match self.submit_pending(now) { Ok(()) => Icm42688Event::Initializing, Err(error) => Icm42688Event::Fault(error), } } - fn start_configuration(&mut self, index: usize) -> Result<(), Error> { - let (register, value) = CONFIGURATION[index]; - self.prepare_write(register, value); - if let Err(error) = self.submit(2) { - self.state = State::Dormant; - return Err(error); - } - self.state = State::WritingConfiguration(index); - Ok(()) - } - fn prepare_read(&mut self, register: u8, length: usize) { self.transmit = [0; TRANSFER_CAPACITY]; self.receive = [0; TRANSFER_CAPACITY]; @@ -228,83 +275,337 @@ impl Icm42688 { self.transmit[..2].copy_from_slice(&[register, value]); } - fn submit(&mut self, length: usize) -> Result<(), Error> { - let (bus, transmit, receive) = (&mut self.bus, &self.transmit, &mut self.receive); - bus.start_transfer(&transmit[..length], &mut receive[..length]) - .map_err(Error::Start) + fn queue(&mut self, operation: Operation, now: Timestamp) { + self.state = State::Pending { + operation, + since: now, + }; } + + fn wait(&mut self, next: Operation, now: Timestamp, delay_us: u64) { + self.state = State::Waiting { + next, + since: now, + delay_us, + }; + } + + fn submit_pending(&mut self, now: Timestamp) -> Result<(), Error> { + let State::Pending { operation, .. } = self.state else { + return Ok(()); + }; + match operation { + Operation::Reset => self.prepare_write(DEVICE_CONFIG, SOFT_RESET), + Operation::Identity => self.prepare_read(WHO_AM_I, 2), + Operation::Configure(index) => { + let (register, value) = CONFIGURATION[index]; + self.prepare_write(register, value); + } + Operation::Sample => self.prepare_read(TEMP_DATA1, TRANSFER_CAPACITY), + } + let length = if operation == Operation::Sample { + TRANSFER_CAPACITY + } else { + 2 + }; + match self.bus.start_transfer(&self.transmit[..length]) { + Ok(()) => { + self.state = State::Active { + operation, + since: now, + }; + Ok(()) + } + Err(error) if Bus::is_busy(&error) => Ok(()), + Err(error) => { + self.state = State::Faulted; + Err(Error::Start(error)) + } + } + } + + fn check_deadline(&mut self, now: Timestamp) -> Option> { + let error = match self.state { + State::Active { since, .. } if elapsed(now, since, TRANSFER_TIMEOUT_US) => { + // This terminal fault proves quiescence or quarantines permanent + // DMA storage. A timeout alone never releases that storage. + match self + .bus + .complete_transfer(SpiCompletion::TransferFault, &mut []) + { + Err(CompletionError::TransferFault | CompletionError::NotActive) | Ok(()) => { + Error::TransferTimeout + } + Err(error) => Error::Completion(error), + } + } + State::Pending { since, .. } if elapsed(now, since, ADMISSION_TIMEOUT_US) => { + Error::AdmissionTimeout + } + State::Ready { since } + if elapsed(now, since, 2 * SAMPLE_PERIOD_US + TRANSFER_TIMEOUT_US) => + { + Error::DataReadyTimeout + } + _ => return None, + }; + self.state = State::Faulted; + Some(error) + } +} + +fn elapsed(now: Timestamp, since: Timestamp, duration_us: u64) -> bool { + now.as_microseconds() + .checked_sub(since.as_microseconds()) + .is_some_and(|elapsed| elapsed >= duration_us) } #[cfg(test)] mod tests { - use super::{Error, Icm42688, Icm42688Event, TRANSFER_CAPACITY}; - use kaptan_spi::{SpiCompletion, SpiDevice}; + use super::*; + use crate::mpu6000::test_bus::{FakeBus, FakeError}; + use kaptan_spi::{CompletionError, SpiCompletion}; use kaptan_time::Timestamp; - struct Bus { - response: [u8; TRANSFER_CAPACITY], + fn t(us: u64) -> Timestamp { + Timestamp::from_microseconds(us + 1_000) } - impl SpiDevice for Bus { - type StartError = (); - - fn start_transfer(&mut self, _: &[u8], receive: &mut [u8]) -> Result<(), Self::StartError> { - receive.copy_from_slice(&self.response[..receive.len()]); - Ok(()) - } - - fn complete_transfer(&mut self, _: SpiCompletion) {} + fn start(driver: &mut Icm42688) { + driver + .begin_initialization(Timestamp::from_microseconds(0)) + .unwrap(); + driver.on_periodic_tick(Timestamp::from_microseconds(999)); + assert_eq!(driver.bus.starts, 0); + driver.on_periodic_tick(t(0)); } - #[test] - fn accepts_a_sample_only_after_identity_and_wake_delays() { - let mut response = [0; TRANSFER_CAPACITY]; - response[1] = 0x47; - let mut imu = Icm42688::new(Bus { response }); - imu.begin_initialization().unwrap(); + fn initialized() -> Icm42688 { + let mut bus = FakeBus::ready(); + bus.response[1] = 0x47; + let mut imu = Icm42688::new(bus); + start(&mut imu); assert!(matches!( - imu.on_transfer_complete(SpiCompletion::Complete), + imu.on_transfer_complete(SpiCompletion::Complete, t(100)), Icm42688Event::Initializing )); - assert!(matches!(imu.advance_initialization(), Ok(()))); + imu.on_periodic_tick(t(1_099)); + assert_eq!(imu.bus.starts, 1); + imu.on_periodic_tick(t(1_100)); assert!(matches!( - imu.on_transfer_complete(SpiCompletion::Complete), + imu.on_transfer_complete(SpiCompletion::Complete, t(1_200)), Icm42688Event::Initializing )); - for _ in 0..7 { + for index in 0..7 { assert!(matches!( - imu.on_transfer_complete(SpiCompletion::Complete), + imu.on_transfer_complete(SpiCompletion::Complete, t(1_300 + index * 100)), Icm42688Event::Initializing )); } - assert!(matches!(imu.advance_initialization(), Ok(()))); + for now in [1_900, 2_099, 2_100, 31_900, 46_899] { + imu.on_periodic_tick(t(now)); + assert_eq!(imu.bus.starts, 9); + } + imu.on_periodic_tick(t(46_900)); assert!(matches!( - imu.on_transfer_complete(SpiCompletion::Complete), + imu.on_transfer_complete(SpiCompletion::Complete, t(47_000)), Icm42688Event::Initializing )); - assert!(matches!( - imu.on_transfer_complete(SpiCompletion::Complete), - Icm42688Event::Initialized - )); + imu.on_transfer_complete(SpiCompletion::Complete, t(47_100)); + imu.on_periodic_tick(t(48_099)); + assert!(!imu.is_ready()); + assert_eq!(imu.request_sample(t(48_099)), Err(Error::NotReady)); + assert_eq!(imu.on_periodic_tick(t(48_100)), Icm42688Event::Initialized); assert!(imu.is_ready()); - imu.request_sample(Timestamp::from_microseconds(1)).unwrap(); + imu + } + + #[test] + fn accepts_a_sample_only_after_elapsed_reset_wake_and_data_settling() { + let mut imu = initialized(); + imu.request_sample(t(48_101)).unwrap(); + assert_eq!(imu.receive, [0; TRANSFER_CAPACITY]); + assert_eq!( + imu.on_transfer_complete(SpiCompletion::NotPending, t(48_102)), + Icm42688Event::NotPending + ); + assert_eq!(imu.receive, [0; TRANSFER_CAPACITY]); + imu.transmit.fill(0xff); + imu.bus.response.fill(0xff); + let mut imu = core::mem::replace(&mut imu, Icm42688::new(FakeBus::ready())); assert!(matches!( - imu.on_transfer_complete(SpiCompletion::Complete), - Icm42688Event::Sample(_) + imu.on_transfer_complete(SpiCompletion::Complete, t(48_300)), + Icm42688Event::Sample(sample) if sample.timestamp == t(48_101) )); + assert_eq!(imu.receive[1], 0x47); + assert_eq!( + imu.on_transfer_complete(SpiCompletion::Complete, t(48_301)), + Icm42688Event::NotPending + ); } #[test] fn rejects_a_wrong_identity() { - let mut imu = Icm42688::new(Bus { - response: [0; TRANSFER_CAPACITY], - }); - imu.begin_initialization().unwrap(); - let _ = imu.on_transfer_complete(SpiCompletion::Complete); - imu.advance_initialization().unwrap(); + let mut imu = Icm42688::new(FakeBus::ready()); + start(&mut imu); + let _ = imu.on_transfer_complete(SpiCompletion::Complete, t(0)); + imu.on_periodic_tick(t(1_000)); assert!(matches!( - imu.on_transfer_complete(SpiCompletion::Complete), + imu.on_transfer_complete(SpiCompletion::Complete, t(1_100)), Icm42688Event::Fault(Error::UnexpectedIdentity(0)) )); } + + #[test] + fn completion_errors_never_republish_a_previous_sample() { + for (event, error) in [ + ( + SpiCompletion::Complete, + Some(CompletionError::InvalidLength), + ), + (SpiCompletion::Complete, Some(CompletionError::Quarantined)), + (SpiCompletion::Complete, Some(CompletionError::NotActive)), + ( + SpiCompletion::Complete, + Some(CompletionError::TransferFault), + ), + (SpiCompletion::TransferFault, None), + ] { + let mut driver = Icm42688::new(FakeBus::ready()); + driver.state = State::Ready { since: t(0) }; + driver.request_sample(t(1)).unwrap(); + assert!(matches!( + driver.on_transfer_complete(SpiCompletion::Complete, t(1)), + Icm42688Event::Sample(_) + )); + driver.request_sample(t(2)).unwrap(); + driver.receive.fill(0xa5); + driver.bus.fail_completion = error; + assert!(matches!( + driver.on_transfer_complete(event, t(2)), + Icm42688Event::Fault(_) + )); + assert_eq!(driver.receive, [0xa5; TRANSFER_CAPACITY]); + assert!(!driver.is_ready()); + assert!(!matches!( + driver.on_transfer_complete(SpiCompletion::Complete, t(3)), + Icm42688Event::Sample(_) + )); + if error == Some(CompletionError::Quarantined) { + assert_eq!(driver.begin_initialization(t(4)), Err(Error::NotReady)); + } + } + } + + #[test] + fn rejected_submission_cannot_complete_a_stale_sample() { + let mut driver = Icm42688::new(FakeBus::ready()); + driver.state = State::Ready { since: t(0) }; + driver.receive.fill(0xa5); + driver.bus.fail_start = true; + assert_eq!( + driver.request_sample(t(1)), + Err(Error::Start(FakeError::Start)) + ); + assert_eq!( + driver.on_transfer_complete(SpiCompletion::Complete, t(2)), + Icm42688Event::NotPending + ); + } + + #[test] + fn every_phase_times_out_and_cancels_once_without_rearming() { + for operation in [Operation::Reset, Operation::Identity, Operation::Sample] + .into_iter() + .chain((0..9).map(Operation::Configure)) + { + let mut driver = Icm42688::new(FakeBus::ready()); + driver.queue(operation, t(100)); + driver.submit_pending(t(100)).unwrap(); + assert_eq!( + driver.on_periodic_tick(t(10_099)), + Icm42688Event::NotPending + ); + assert_eq!(driver.bus.cancellations, 0); + assert_eq!( + driver.on_periodic_tick(t(10_100)), + Icm42688Event::Fault(Error::TransferTimeout) + ); + assert_eq!(driver.bus.cancellations, 1); + assert_eq!( + driver.on_transfer_complete(SpiCompletion::Complete, t(10_101)), + Icm42688Event::NotPending + ); + driver.on_periodic_tick(t(1_000_000)); + assert_eq!(driver.bus.starts, 1); + assert_eq!( + driver.begin_initialization(t(1_000_000)), + Err(Error::NotReady) + ); + } + } + + #[test] + fn every_busy_phase_preserves_pending_operation_and_has_an_admission_deadline() { + for operation in [Operation::Reset, Operation::Identity, Operation::Sample] + .into_iter() + .chain((0..9).map(Operation::Configure)) + { + let mut driver = Icm42688::new(FakeBus::ready()); + driver.bus.busy = true; + driver.queue(operation, t(100)); + driver.submit_pending(t(100)).unwrap(); + assert_eq!( + driver.state, + State::Pending { + operation, + since: t(100) + } + ); + driver.on_periodic_tick(t(200)); + driver.bus.busy = false; + driver.on_periodic_tick(t(300)); + assert_eq!( + driver.state, + State::Active { + operation, + since: t(300) + } + ); + assert_eq!(driver.bus.starts, 1); + } + let mut driver = Icm42688::new(FakeBus::ready()); + driver.bus.busy = true; + start(&mut driver); + assert_eq!( + driver.on_periodic_tick(t(20_000)), + Icm42688Event::Fault(Error::AdmissionTimeout) + ); + assert_eq!(driver.bus.cancellations, 0); + } + + #[test] + fn missed_drdy_late_completion_and_failed_cancellation_never_publish() { + let mut driver = initialized(); + driver.on_periodic_tick(t(60_099)); + assert_eq!(driver.bus.starts, 11); + assert_eq!( + driver.on_periodic_tick(t(60_100)), + Icm42688Event::Fault(Error::DataReadyTimeout) + ); + let mut driver = initialized(); + driver.request_sample(t(49_000)).unwrap(); + assert_eq!( + driver.on_transfer_complete(SpiCompletion::Complete, t(59_000)), + Icm42688Event::Fault(Error::TransferTimeout) + ); + let mut driver = initialized(); + driver.request_sample(t(49_000)).unwrap(); + driver.bus.fail_completion = Some(CompletionError::Quarantined); + assert_eq!( + driver.on_periodic_tick(t(59_000)), + Icm42688Event::Fault(Error::Completion(CompletionError::Quarantined)) + ); + assert_eq!(driver.bus.start_transfer(&[1]), Err(FakeError::Quarantined)); + } } diff --git a/firmware/libs/kaptan-drivers/src/icm42688/sample.rs b/firmware/libs/kaptan-drivers/src/icm42688/sample.rs index 7b49ae5..50c1fba 100644 --- a/firmware/libs/kaptan-drivers/src/icm42688/sample.rs +++ b/firmware/libs/kaptan-drivers/src/icm42688/sample.rs @@ -1,4 +1,9 @@ //! ICM-42688-P burst-sample decoding. +//! +//! Integer division rounds toward zero before adding temperature's offset. +//! Over the full signed raw domain, the configured ranges decode to +//! -4,000..=3,999 mg, -8,731..=8,731 mrad/s and -22,234..=27,233 cdegC. +//! Products fit i32 and the acceleration and temperature results fit i16. use kaptan_sensor::imu::ImuSnapshot; use kaptan_time::Timestamp; @@ -32,18 +37,18 @@ const fn scale_acceleration(raw: i16) -> i16 { } // The configured ±500°/s range has 65.5 counts/(°/s). -const fn scale_angular_rate(raw: i16) -> i16 { - ((raw as i32 * 17_453) / 65_500) as i16 +const fn scale_angular_rate(raw: i16) -> i32 { + (raw as i32 * 17_453) / 65_500 } // ICM-42688-P temperature is 132.48 counts/°C with a 25°C offset. const fn scale_temperature(raw: i16) -> i16 { - ((raw as i32 * 100) / 13_248 + 2_500) as i16 + ((raw as i32 * 10_000) / 13_248 + 2_500) as i16 } #[cfg(test)] mod tests { - use super::decode; + use super::{decode, scale_temperature}; use kaptan_time::Timestamp; #[test] @@ -54,4 +59,61 @@ mod tests { assert_eq!(sample.angular_rate_milliradians_per_second[0], 8_725); assert_eq!(sample.temperature_centidegrees_celsius, 2_500); } + + #[test] + fn signed_raw_endpoints_fit_the_configured_ranges() { + for (temperature_raw, temperature) in [(i16::MIN, -22_234), (i16::MAX, 27_233)] { + let mut bytes = [0; 14]; + for (index, raw) in [ + temperature_raw, + i16::MIN, + 0, + i16::MAX, + i16::MIN, + 0, + i16::MAX, + ] + .into_iter() + .enumerate() + { + bytes[index * 2..index * 2 + 2].copy_from_slice(&raw.to_be_bytes()); + } + let timestamp = Timestamp::from_microseconds(123); + let sample = decode(&bytes, timestamp); + assert_eq!(sample.timestamp, timestamp); + assert_eq!(sample.acceleration_milligravity, [-4_000, 0, 3_999]); + assert_eq!( + sample.angular_rate_milliradians_per_second, + [-8_731, 0, 8_731] + ); + assert_eq!(sample.temperature_centidegrees_celsius, temperature); + } + } + + #[test] + fn temperature_matches_datasheet_scale_and_rounding() { + // ±13,248 counts is ±100°C relative to the 25°C offset. + for (raw, expected) in [ + (-13_248, -7_500), + (-132, 2_401), + (-1, 2_500), + (0, 2_500), + (1, 2_500), + (132, 2_599), + (13_248, 12_500), + ] { + assert_eq!(scale_temperature(raw), expected); + } + + // Independent floating-point datasheet reference over all 65,536 raw + // codes. Truncation may lose less than one centidegree, never wrap. + let mut previous = i16::MIN; + for raw in i16::MIN..=i16::MAX { + let actual = scale_temperature(raw); + let reference = (f64::from(raw) / 132.48 + 25.0) * 100.0; + assert!((f64::from(actual) - reference).abs() < 1.0); + assert!(actual >= previous); + previous = actual; + } + } } diff --git a/firmware/libs/kaptan-drivers/src/icm42688/service.rs b/firmware/libs/kaptan-drivers/src/icm42688/service.rs index 857ec3e..8bdfd3a 100644 --- a/firmware/libs/kaptan-drivers/src/icm42688/service.rs +++ b/firmware/libs/kaptan-drivers/src/icm42688/service.rs @@ -36,27 +36,18 @@ where Clock: MonotonicClock, Bus: SpiDevice, { - /// Submits the asynchronous reset transaction. + /// Accepts initialization and starts the power-on register-access delay. pub fn begin(&mut self) -> Result<(), Error> { - self.imu.begin_initialization() + self.imu.begin_initialization(self.clock.now()) } - /// Advances reset and sensor-start settling states on a periodic tick. + /// Services settling and missing-IRQ deadlines even with an active transfer. pub fn on_periodic_tick(&mut self) -> Icm42688ServiceEvent { - if self.imu.is_ready() { - return match self.imu.request_sample(self.clock.now()) { - Ok(()) | Err(Error::NotReady) => Icm42688ServiceEvent::Idle, - Err(error) => Icm42688ServiceEvent::Fault(error), - }; - } - let _ = self.clock.now(); - match self.imu.advance_initialization() { - Ok(()) => Icm42688ServiceEvent::Idle, - Err(error) => Icm42688ServiceEvent::Fault(error), - } + service_event(self.imu.on_periodic_tick(self.clock.now())) } - /// Handles one coalesced data-ready event. + /// Handles one coalesced data-ready notification. Time is captured here in + /// the worker, at read submission, not at IRQ entry or physical sampling. pub fn on_data_ready(&mut self) -> Icm42688ServiceEvent { if !self.imu.is_ready() { return Icm42688ServiceEvent::Idle; @@ -72,11 +63,110 @@ where &mut self, completion: SpiCompletion, ) -> Icm42688ServiceEvent { - match self.imu.on_transfer_complete(completion) { - Icm42688Event::NotPending | Icm42688Event::Initializing => Icm42688ServiceEvent::Idle, - Icm42688Event::Initialized => Icm42688ServiceEvent::Initialized, - Icm42688Event::Sample(sample) => Icm42688ServiceEvent::Sample(sample), - Icm42688Event::Fault(error) => Icm42688ServiceEvent::Fault(error), + service_event(self.imu.on_transfer_complete(completion, self.clock.now())) + } +} + +fn service_event(event: Icm42688Event) -> Icm42688ServiceEvent { + match event { + Icm42688Event::NotPending | Icm42688Event::Initializing => Icm42688ServiceEvent::Idle, + Icm42688Event::Initialized => Icm42688ServiceEvent::Initialized, + Icm42688Event::Sample(sample) => Icm42688ServiceEvent::Sample(sample), + Icm42688Event::Fault(error) => Icm42688ServiceEvent::Fault(error), + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::mpu6000::test_bus::FakeBus; + use kaptan_time::Timestamp; + + struct Clock(u64); + impl MonotonicClock for Clock { + fn now(&mut self) -> Timestamp { + Timestamp::from_microseconds(self.0) } + } + + fn initialized() -> Icm42688Service { + let mut bus = FakeBus::ready(); + bus.response[1] = 0x47; + let mut service = Icm42688Service::new(Clock(0), Icm42688::new(bus)); + service.begin().unwrap(); + service.clock.0 = 999; + service.on_periodic_tick(); + assert_eq!(service.imu.bus.starts, 0); + service.clock.0 = 1_000; + service.on_periodic_tick(); + service.on_transfer_complete(SpiCompletion::Complete); + service.clock.0 = 1_999; + service.on_periodic_tick(); + assert_eq!(service.imu.bus.starts, 1); + service.clock.0 = 2_000; + service.on_periodic_tick(); + for _ in 0..8 { + service.on_transfer_complete(SpiCompletion::Complete); + } + service.clock.0 = 46_999; + service.on_periodic_tick(); + assert_eq!(service.imu.bus.starts, 9); + service.clock.0 = 47_000; + service.on_periodic_tick(); + service.on_transfer_complete(SpiCompletion::Complete); + service.on_transfer_complete(SpiCompletion::Complete); + service.clock.0 = 48_000; + assert!(matches!( + service.on_periodic_tick(), + Icm42688ServiceEvent::Initialized + )); + service + } + + #[test] + fn fake_clock_labels_the_accepted_read_request_even_after_busy_admission() { + let mut service = initialized(); + service.clock.0 = 48_100; + service.imu.bus.busy = true; + assert!(matches!( + service.on_data_ready(), + Icm42688ServiceEvent::Idle + )); + service.clock.0 = 48_200; + service.imu.bus.busy = false; + service.on_periodic_tick(); + service.clock.0 = 48_500; + assert!( + matches!(service.on_transfer_complete(SpiCompletion::Complete), + Icm42688ServiceEvent::Sample(sample) if sample.timestamp == Timestamp::from_microseconds(48_200)) + ); + } + + #[test] + fn fake_clock_reports_missing_drdy_and_dma_without_polling_samples() { + let mut service = initialized(); + service.clock.0 = 59_999; + assert!(matches!( + service.on_periodic_tick(), + Icm42688ServiceEvent::Idle + )); + assert_eq!(service.imu.bus.starts, 11); + service.clock.0 = 60_000; + assert!(matches!( + service.on_periodic_tick(), + Icm42688ServiceEvent::Fault(Error::DataReadyTimeout) + )); + let mut service = initialized(); + service.on_data_ready(); + service.clock.0 = 58_000; + assert!(matches!( + service.on_periodic_tick(), + Icm42688ServiceEvent::Fault(Error::TransferTimeout) + )); + assert!(matches!( + service.on_transfer_complete(SpiCompletion::Complete), + Icm42688ServiceEvent::Idle + )); + assert_eq!(service.imu.bus.cancellations, 1); } } diff --git a/firmware/libs/kaptan-drivers/src/lib.rs b/firmware/libs/kaptan-drivers/src/lib.rs index 6a208e4..6320c26 100644 --- a/firmware/libs/kaptan-drivers/src/lib.rs +++ b/firmware/libs/kaptan-drivers/src/lib.rs @@ -1,8 +1,4 @@ //! Hardware-independent chip protocol drivers. -//! -//! Drivers express device register protocols and data conversion through small -//! bus traits. MCU, pin, DMA, interrupt, and RTIC mechanics belong below this -//! layer in a HAL route implementation and its platform composition. #![no_std] diff --git a/firmware/libs/kaptan-drivers/src/lidar_lite_v3/error.rs b/firmware/libs/kaptan-drivers/src/lidar_lite_v3/error.rs index 166ca27..4e813d3 100644 --- a/firmware/libs/kaptan-drivers/src/lidar_lite_v3/error.rs +++ b/firmware/libs/kaptan-drivers/src/lidar_lite_v3/error.rs @@ -29,6 +29,9 @@ pub enum Error { /// The device reported a documented no-signal sentinel distance of zero or one centimetre. #[kaptan_error(id = 5208)] NoSignal, + /// The controller rejected completion or could not prove physical STOP. + #[kaptan_error(id = 5209)] + Completion(kaptan_i2c::CompletionError), } /// LIDAR-Lite v3 startup lifecycle failure. diff --git a/firmware/libs/kaptan-drivers/src/lidar_lite_v3/mod.rs b/firmware/libs/kaptan-drivers/src/lidar_lite_v3/mod.rs index b00cf1d..c16183f 100644 --- a/firmware/libs/kaptan-drivers/src/lidar_lite_v3/mod.rs +++ b/firmware/libs/kaptan-drivers/src/lidar_lite_v3/mod.rs @@ -4,7 +4,7 @@ mod error; mod registers; pub mod service; -use kaptan_i2c::{I2cBus, I2cCompletion}; +use kaptan_i2c::{CompletionError, I2cBus, I2cCompletion}; use kaptan_sensor::range::RangeSnapshot; use kaptan_time::Timestamp; @@ -24,6 +24,8 @@ const DISTANCE_LENGTH: usize = 2; /// /// The extra margin preserves liveness if the optical engine remains busy /// while still accepting a documented slow acquisition. +/// Completion at exactly this elapsed time is accepted. Later completion is +/// discarded, regardless of whether the periodic timeout handler has run. const MEASUREMENT_TIMEOUT_MICROSECONDS: u64 = 200_000; /// Receiver-bias correction selection for one acquisition. @@ -50,6 +52,7 @@ impl AcquisitionMode { #[derive(Clone, Copy, Debug, Eq, PartialEq)] enum State { Settling, + Faulted, Idle, Starting { timestamp: Timestamp }, Waiting { timestamp: Timestamp }, @@ -174,6 +177,7 @@ impl LidarLiteV3 { timestamp: Timestamp, mode: AcquisitionMode, ) -> Result<(), Error> { + self.service_bus(timestamp)?; if self.state != State::Idle { return Err(Error::NotReady); } @@ -192,15 +196,16 @@ impl LidarLiteV3 { /// therefore only records phase completion; this periodic path begins the /// following transaction after the bus has been released. pub fn poll_measurement(&mut self, now: Timestamp) -> Result<(), Error> { + self.service_bus(now)?; let timestamp = match self.state { State::Waiting { timestamp } | State::StatusReadPending { timestamp } | State::DistanceRegisterPending { timestamp } | State::DistanceReadPending { timestamp } => timestamp, - State::Settling | State::Idle => return Err(Error::NotReady), + State::Settling | State::Idle | State::Faulted => return Err(Error::NotReady), _ => { if self.measurement_timed_out(now) { - self.bus.cancel_transaction(); + self.bus.cancel_transaction(now); self.state = State::Idle; return Err(Error::MeasurementTimeout); } @@ -208,7 +213,7 @@ impl LidarLiteV3 { } }; if self.measurement_timed_out(now) { - self.bus.cancel_transaction(); + self.bus.cancel_transaction(now); self.state = State::Idle; return Err(Error::MeasurementTimeout); } @@ -222,17 +227,36 @@ impl LidarLiteV3 { } /// Advances the state machine from an already-acknowledged I²C terminal event. + /// `now` is the actual completion-handler time in the acquisition clock domain. pub fn on_transaction_complete( &mut self, completion: I2cCompletion, + now: Timestamp, ) -> LidarLiteV3Event { if completion == I2cCompletion::NotPending || !self.has_active_transaction() { return LidarLiteV3Event::NotPending; } + if self.measurement_timed_out(now) { + // Discard a controller-owned completed read without copying it out. + // If STOP is still pending, cancellation keeps admission closed and + // periodic bus service must finish STOP or report StopUnproven. + self.bus.cancel_transaction(now); + self.state = State::Idle; + return LidarLiteV3Event::Fault(Error::MeasurementTimeout); + } + match completion { I2cCompletion::TransferFault => { - self.bus.complete_transaction(completion, None); + let result = self.bus.complete_transaction(completion, None); + match result { + Err(CompletionError::NotPending) => return LidarLiteV3Event::NotPending, + Err(CompletionError::TransferFault) | Ok(()) => {} + Err(error) => { + self.state = State::Faulted; + return LidarLiteV3Event::Fault(Error::Completion(error)); + } + } self.state = State::Idle; LidarLiteV3Event::Fault(Error::TransferFault) } @@ -242,7 +266,13 @@ impl LidarLiteV3 { State::ReadingDistance { .. } => Some(&mut self.receive[..DISTANCE_LENGTH]), _ => None, }; - self.bus.complete_transaction(completion, read); + if let Err(error) = self.bus.complete_transaction(completion, read) { + if error == CompletionError::NotPending { + return LidarLiteV3Event::NotPending; + } + self.state = State::Faulted; + return LidarLiteV3Event::Fault(Error::Completion(error)); + } self.advance_after_completion() } I2cCompletion::NotPending => unreachable!(), @@ -254,6 +284,14 @@ impl LidarLiteV3 { self.state == State::Idle } + fn service_bus(&mut self, now: Timestamp) -> Result<(), Error> { + if let Err(error) = self.bus.service(now) { + self.state = State::Faulted; + return Err(Error::Completion(error)); + } + Ok(()) + } + fn has_active_transaction(&self) -> bool { matches!( self.state, @@ -276,16 +314,16 @@ impl LidarLiteV3 { | State::WritingDistanceRegister { timestamp } | State::DistanceReadPending { timestamp } | State::ReadingDistance { timestamp } => timestamp, - State::Settling | State::Idle => return false, + State::Settling | State::Idle | State::Faulted => return false, }; now.as_microseconds() .saturating_sub(timestamp.as_microseconds()) - >= MEASUREMENT_TIMEOUT_MICROSECONDS + > MEASUREMENT_TIMEOUT_MICROSECONDS } fn advance_after_completion(&mut self) -> LidarLiteV3Event { match self.state { - State::Settling => LidarLiteV3Event::NotPending, + State::Settling | State::Faulted => LidarLiteV3Event::NotPending, State::Idle => LidarLiteV3Event::NotPending, State::Starting { timestamp } => { self.state = State::Waiting { timestamp }; @@ -380,7 +418,7 @@ mod tests { AcquisitionMode, Error, LidarLiteV3, LidarLiteV3Address, LidarLiteV3Event, MEASUREMENT_TIMEOUT_MICROSECONDS, StartupError, }; - use kaptan_i2c::{I2cBus, I2cCompletion}; + use kaptan_i2c::{CompletionError, I2cBus, I2cCompletion}; use kaptan_sensor::range::RangeSnapshot; use kaptan_time::Timestamp; @@ -408,7 +446,11 @@ mod tests { response: [u8; 2], fail_start: bool, completions: usize, + read_copies: usize, cancellations: usize, + completion_error: Option, + service_error: Option, + cancelled_at: Option, } impl FakeBus { @@ -418,7 +460,11 @@ mod tests { response: [0; 2], fail_start: false, completions: 0, + read_copies: 0, cancellations: 0, + completion_error: None, + service_error: None, + cancelled_at: None, } } } @@ -454,17 +500,36 @@ mod tests { Ok(()) } - fn complete_transaction(&mut self, completion: I2cCompletion, read: Option<&mut [u8]>) { + fn complete_transaction( + &mut self, + completion: I2cCompletion, + read: Option<&mut [u8]>, + ) -> Result<(), CompletionError> { + if completion == I2cCompletion::NotPending { + return Err(CompletionError::NotPending); + } + if let Some(error) = self.completion_error.take() { + return Err(error); + } if completion == I2cCompletion::Complete && let Some(read) = read { read.copy_from_slice(&self.response[..read.len()]); + self.read_copies += 1; } self.completions += 1; + self.transaction = Transaction::None; + Ok(()) } - fn cancel_transaction(&mut self) { + fn cancel_transaction(&mut self, now: Timestamp) { self.cancellations += 1; + self.cancelled_at = Some(now); + self.transaction = Transaction::None; + } + + fn service(&mut self, _: Timestamp) -> Result<(), CompletionError> { + self.service_error.take().map_or(Ok(()), Err) } } @@ -473,12 +538,18 @@ mod tests { let mut driver = LidarLiteV3::new(FakeBus::ready()); assert!(!driver.is_ready()); assert_eq!( - driver.on_transaction_complete(I2cCompletion::NotPending), + driver.on_transaction_complete( + I2cCompletion::NotPending, + Timestamp::from_microseconds(0) + ), LidarLiteV3Event::NotPending ); assert_eq!(driver.bus.completions, 0); assert_eq!( - driver.on_transaction_complete(I2cCompletion::TransferFault), + driver.on_transaction_complete( + I2cCompletion::TransferFault, + Timestamp::from_microseconds(0) + ), LidarLiteV3Event::NotPending ); assert_eq!(driver.bus.completions, 0); @@ -514,7 +585,7 @@ mod tests { } ); assert_eq!( - driver.on_transaction_complete(I2cCompletion::Complete), + driver.on_transaction_complete(I2cCompletion::Complete, timestamp), LidarLiteV3Event::MeasurementStarted ); driver.poll_measurement(timestamp).unwrap(); @@ -528,7 +599,7 @@ mod tests { ); driver.bus.response = [STATUS_HEALTHY, 0]; assert_eq!( - driver.on_transaction_complete(I2cCompletion::Complete), + driver.on_transaction_complete(I2cCompletion::Complete, timestamp), LidarLiteV3Event::ReadingStatus ); driver.poll_measurement(timestamp).unwrap(); @@ -540,7 +611,7 @@ mod tests { } ); assert_eq!( - driver.on_transaction_complete(I2cCompletion::Complete), + driver.on_transaction_complete(I2cCompletion::Complete, timestamp), LidarLiteV3Event::SelectingDistanceRegister ); driver.poll_measurement(timestamp).unwrap(); @@ -553,7 +624,7 @@ mod tests { } ); assert_eq!( - driver.on_transaction_complete(I2cCompletion::Complete), + driver.on_transaction_complete(I2cCompletion::Complete, timestamp), LidarLiteV3Event::ReadingMeasurement ); driver.bus.response = 291_u16.to_be_bytes(); @@ -566,7 +637,7 @@ mod tests { } ); assert_eq!( - driver.on_transaction_complete(I2cCompletion::Complete), + driver.on_transaction_complete(I2cCompletion::Complete, timestamp), LidarLiteV3Event::Measurement(RangeSnapshot::new(timestamp, 2_910)) ); assert!(driver.is_ready()); @@ -581,17 +652,20 @@ mod tests { AcquisitionMode::WithoutReceiverBiasCorrection, ) .unwrap(); - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); driver .poll_measurement(Timestamp::from_microseconds(0)) .unwrap(); driver.bus.response = [STATUS_HEALTHY | 1, 0]; - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); driver .poll_measurement(Timestamp::from_microseconds(0)) .unwrap(); assert_eq!( - driver.on_transaction_complete(I2cCompletion::Complete), + driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)), LidarLiteV3Event::MeasurementBusy ); assert!(!driver.is_ready()); @@ -611,11 +685,12 @@ mod tests { AcquisitionMode::WithReceiverBiasCorrection, ) .unwrap(); - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); assert_eq!( driver.poll_measurement(Timestamp::from_microseconds( - MEASUREMENT_TIMEOUT_MICROSECONDS, + MEASUREMENT_TIMEOUT_MICROSECONDS + 1, )), Err(Error::MeasurementTimeout) ); @@ -632,17 +707,20 @@ mod tests { AcquisitionMode::WithReceiverBiasCorrection, ) .unwrap(); - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); driver .poll_measurement(Timestamp::from_microseconds(0)) .unwrap(); driver.bus.response = [STATUS_HEALTHY | STATUS_PROCESS_ERROR, 0]; - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); driver .poll_measurement(Timestamp::from_microseconds(0)) .unwrap(); assert_eq!( - driver.on_transaction_complete(I2cCompletion::Complete), + driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)), LidarLiteV3Event::Fault(Error::ProcessError) ); @@ -652,17 +730,20 @@ mod tests { AcquisitionMode::WithReceiverBiasCorrection, ) .unwrap(); - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); driver .poll_measurement(Timestamp::from_microseconds(0)) .unwrap(); driver.bus.response = [0, 0]; - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); driver .poll_measurement(Timestamp::from_microseconds(0)) .unwrap(); assert_eq!( - driver.on_transaction_complete(I2cCompletion::Complete), + driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)), LidarLiteV3Event::Fault(Error::Unhealthy) ); @@ -672,17 +753,20 @@ mod tests { AcquisitionMode::WithReceiverBiasCorrection, ) .unwrap(); - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); driver .poll_measurement(Timestamp::from_microseconds(0)) .unwrap(); driver.bus.response = [STATUS_HEALTHY | STATUS_INVALID_SIGNAL, 0]; - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); driver .poll_measurement(Timestamp::from_microseconds(0)) .unwrap(); assert_eq!( - driver.on_transaction_complete(I2cCompletion::Complete), + driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)), LidarLiteV3Event::Fault(Error::InvalidSignal) ); } @@ -696,20 +780,25 @@ mod tests { AcquisitionMode::WithReceiverBiasCorrection, ) .unwrap(); - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); driver .poll_measurement(Timestamp::from_microseconds(0)) .unwrap(); driver.bus.response = [STATUS_HEALTHY, 0]; - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); driver.bus.response = 1_u16.to_be_bytes(); let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); assert_eq!( - driver.on_transaction_complete(I2cCompletion::Complete), + driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)), LidarLiteV3Event::Fault(Error::NoSignal) ); @@ -719,20 +808,25 @@ mod tests { AcquisitionMode::WithReceiverBiasCorrection, ) .unwrap(); - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); driver .poll_measurement(Timestamp::from_microseconds(0)) .unwrap(); driver.bus.response = [STATUS_HEALTHY, 0]; - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); driver.bus.response = 0_u16.to_be_bytes(); let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); assert_eq!( - driver.on_transaction_complete(I2cCompletion::Complete), + driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)), LidarLiteV3Event::Fault(Error::NoSignal) ); @@ -742,20 +836,25 @@ mod tests { AcquisitionMode::WithReceiverBiasCorrection, ) .unwrap(); - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); driver .poll_measurement(Timestamp::from_microseconds(0)) .unwrap(); driver.bus.response = [STATUS_HEALTHY, 0]; - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); - let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)); driver.bus.response = u16::MAX.to_be_bytes(); let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); assert_eq!( - driver.on_transaction_complete(I2cCompletion::Complete), + driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)), LidarLiteV3Event::Measurement(RangeSnapshot::new( Timestamp::from_microseconds(0), 655_350, @@ -773,9 +872,147 @@ mod tests { ) .unwrap(); assert_eq!( - driver.on_transaction_complete(I2cCompletion::TransferFault), + driver.on_transaction_complete( + I2cCompletion::TransferFault, + Timestamp::from_microseconds(0) + ), LidarLiteV3Event::Fault(Error::TransferFault) ); assert!(driver.is_ready()); } + + #[test] + fn cancellation_is_serviced_after_acquisition_timeout_and_stop_failure_latches_fault() { + let mut driver = initialized(); + driver + .request_measurement( + Timestamp::from_microseconds(123), + AcquisitionMode::WithReceiverBiasCorrection, + ) + .unwrap(); + let timeout = Timestamp::from_microseconds(200_124); + assert_eq!( + driver.poll_measurement(timeout), + Err(Error::MeasurementTimeout) + ); + assert_eq!(driver.bus.cancelled_at, Some(timeout)); + driver.bus.service_error = Some(CompletionError::StopUnproven); + assert_eq!( + driver.request_measurement( + Timestamp::from_microseconds(210_123), + AcquisitionMode::WithReceiverBiasCorrection + ), + Err(Error::Completion(CompletionError::StopUnproven)) + ); + assert!(!driver.is_ready()); + assert_eq!( + driver.on_transaction_complete( + I2cCompletion::Complete, + Timestamp::from_microseconds(210_123) + ), + LidarLiteV3Event::NotPending + ); + assert_eq!( + driver.poll_measurement(Timestamp::from_microseconds(220_123)), + Err(Error::NotReady) + ); + assert_eq!(driver.bus.cancellations, 1); + } + + #[test] + fn unproven_completion_cannot_advance_a_phase_or_publish_stale_bytes() { + let mut driver = initialized(); + driver + .request_measurement( + Timestamp::from_microseconds(0), + AcquisitionMode::WithReceiverBiasCorrection, + ) + .unwrap(); + driver.bus.completion_error = Some(CompletionError::NotPending); + assert_eq!( + driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)), + LidarLiteV3Event::NotPending + ); + assert_eq!( + driver.state, + super::State::Starting { + timestamp: Timestamp::from_microseconds(0) + } + ); + driver.bus.completion_error = Some(CompletionError::StopUnproven); + assert_eq!( + driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)), + LidarLiteV3Event::Fault(Error::Completion(CompletionError::StopUnproven)) + ); + assert!(!driver.is_ready()); + assert_eq!( + driver + .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)), + LidarLiteV3Event::NotPending + ); + } + + #[test] + fn late_terminal_events_discard_every_active_phase_without_copying() { + for phase in 0..5 { + for completion in [I2cCompletion::Complete, I2cCompletion::TransferFault] { + let mut driver = initialized(); + let start = Timestamp::from_microseconds(123); + driver + .request_measurement(start, AcquisitionMode::WithReceiverBiasCorrection) + .unwrap(); + driver.bus.response = [STATUS_HEALTHY, 0]; + for step in 0..phase { + let now = 124 + step * 2; + driver.on_transaction_complete( + I2cCompletion::Complete, + Timestamp::from_microseconds(now), + ); + driver + .poll_measurement(Timestamp::from_microseconds(now + 1)) + .unwrap(); + } + let before = driver.state; + let copies = driver.bus.read_copies; + let completions = driver.bus.completions; + driver.receive = [0xa5; 2]; + let late = Timestamp::from_microseconds(200_124); + + // A nonterminal notification is not permission to cancel, even + // when the timer has been delayed past the acquisition limit. + assert_eq!( + driver.on_transaction_complete(I2cCompletion::NotPending, late), + LidarLiteV3Event::NotPending + ); + assert_eq!(driver.state, before); + assert_eq!(driver.bus.cancellations, 0); + assert_eq!(driver.bus.completions, completions); + assert_eq!(driver.receive, [0xa5; 2]); + + assert_eq!( + driver.on_transaction_complete(completion, late), + LidarLiteV3Event::Fault(Error::MeasurementTimeout) + ); + assert_eq!(driver.bus.cancelled_at, Some(late)); + assert_eq!(driver.bus.cancellations, 1); + assert_eq!(driver.bus.transaction, Transaction::None); + assert_eq!(driver.bus.read_copies, copies); + assert_eq!(driver.receive, [0xa5; 2]); + assert_eq!( + driver.on_transaction_complete(completion, late), + LidarLiteV3Event::NotPending + ); + + // Cancellation is still serviced if physical STOP was unproven. + driver.bus.service_error = Some(CompletionError::StopUnproven); + assert_eq!( + driver.poll_measurement(Timestamp::from_microseconds(210_124)), + Err(Error::Completion(CompletionError::StopUnproven)) + ); + assert!(!driver.is_ready()); + } + } + } } diff --git a/firmware/libs/kaptan-drivers/src/lidar_lite_v3/service.rs b/firmware/libs/kaptan-drivers/src/lidar_lite_v3/service.rs index 15d8073..e65eebf 100644 --- a/firmware/libs/kaptan-drivers/src/lidar_lite_v3/service.rs +++ b/firmware/libs/kaptan-drivers/src/lidar_lite_v3/service.rs @@ -3,16 +3,16 @@ use super::{AcquisitionMode, Error, LidarLiteV3, LidarLiteV3Event, StartupError}; use kaptan_i2c::{I2cBus, I2cCompletion}; use kaptan_sensor::range::RangeSnapshot; -use kaptan_time::MonotonicClock; +use kaptan_time::{MonotonicClock, Timestamp}; /// One observable result from a LIDAR-Lite v3 acquisition service. #[derive(Debug)] pub enum LidarLiteV3ServiceEvent { - /// No observation or terminal fault was produced. + /// No observation or reportable error was produced. Idle, /// One coherent range observation is ready for publication. Measurement(RangeSnapshot), - /// The LIDAR-Lite v3 path entered a terminal fault state. + /// Acquisition or controller failed. Fault(Error), } @@ -55,22 +55,21 @@ impl LidarLiteV3Service { &mut self, completion: I2cCompletion, ) -> LidarLiteV3ServiceEvent { - service_event(self.lidar.on_transaction_complete(completion)) + let now = self.clock.now(); + service_event(self.lidar.on_transaction_complete(completion, now)) } - /// Acknowledges a board-controller interrupt and advances the acquisition. - /// - /// The closure keeps controller-specific interrupt handling at the board - /// boundary while preserving this service as the sole owner of LIDAR state. + /// Acknowledges a controller interrupt and advances the acquisition. pub fn on_controller_interrupt( &mut self, acknowledge: F, ) -> LidarLiteV3ServiceEvent where - F: FnOnce(&mut Bus) -> I2cCompletion, + F: FnOnce(&mut Bus, Timestamp) -> I2cCompletion, { - let completion = self.lidar.acknowledge_with(acknowledge); - self.on_transaction_complete(completion) + let now = self.clock.now(); + let completion = self.lidar.acknowledge_with(|bus| acknowledge(bus, now)); + service_event(self.lidar.on_transaction_complete(completion, now)) } } @@ -89,19 +88,22 @@ fn service_event(event: LidarLiteV3Event) -> LidarLiteV3Serv mod tests { use super::{LidarLiteV3Service, LidarLiteV3ServiceEvent, service_event}; use crate::lidar_lite_v3::{AcquisitionMode, Error, LidarLiteV3, LidarLiteV3Event}; - use kaptan_i2c::{I2cBus, I2cCompletion}; + use kaptan_i2c::{CompletionError, I2cBus, I2cCompletion}; use kaptan_time::{MonotonicClock, Timestamp}; - struct Clock; + struct Clock(u64); impl MonotonicClock for Clock { fn now(&mut self) -> Timestamp { - Timestamp::from_microseconds(123) + Timestamp::from_microseconds(self.0) } } + #[derive(Default)] struct Bus { response: [u8; 2], + cancelled_at: Option, + read_copies: usize, } impl I2cBus for Bus { @@ -115,22 +117,37 @@ mod tests { Ok(()) } - fn complete_transaction(&mut self, completion: I2cCompletion, read: Option<&mut [u8]>) { + fn complete_transaction( + &mut self, + completion: I2cCompletion, + read: Option<&mut [u8]>, + ) -> Result<(), CompletionError> { + if completion == I2cCompletion::NotPending { + return Err(CompletionError::NotPending); + } if completion == I2cCompletion::Complete && let Some(read) = read { read.copy_from_slice(&self.response[..read.len()]); + self.read_copies += 1; } + Ok(()) } - fn cancel_transaction(&mut self) {} + fn cancel_transaction(&mut self, now: Timestamp) { + self.cancelled_at = Some(now); + } + + fn service(&mut self, _: Timestamp) -> Result<(), CompletionError> { + Ok(()) + } } #[test] fn produces_a_measurement_after_stop_separated_i2c_transactions() { let mut service = LidarLiteV3Service::new( - Clock, - LidarLiteV3::new(Bus { response: [0; 2] }), + Clock(123), + LidarLiteV3::new(Bus::default()), AcquisitionMode::WithReceiverBiasCorrection, ); service.finish_startup().unwrap(); @@ -148,7 +165,7 @@ mod tests { LidarLiteV3ServiceEvent::Idle )); assert!(matches!( - service.on_controller_interrupt(|bus| { + service.on_controller_interrupt(|bus, _| { bus.response = [1 << 5, 0]; I2cCompletion::Complete }), @@ -159,7 +176,7 @@ mod tests { LidarLiteV3ServiceEvent::Idle )); assert!(matches!( - service.on_controller_interrupt(|_| I2cCompletion::Complete), + service.on_controller_interrupt(|_, _| I2cCompletion::Complete), LidarLiteV3ServiceEvent::Idle )); assert!(matches!( @@ -167,7 +184,7 @@ mod tests { LidarLiteV3ServiceEvent::Idle )); assert!(matches!( - service.on_controller_interrupt(|bus| { + service.on_controller_interrupt(|bus, _| { bus.response = [0x01, 0x23]; I2cCompletion::Complete }), @@ -178,7 +195,7 @@ mod tests { LidarLiteV3ServiceEvent::Idle )); assert!(matches!( - service.on_controller_interrupt(|_| I2cCompletion::Complete), + service.on_controller_interrupt(|_, _| I2cCompletion::Complete), LidarLiteV3ServiceEvent::Measurement(sample) if sample.timestamp == Timestamp::from_microseconds(123) && sample.distance_millimetres == 2_910 @@ -200,4 +217,102 @@ mod tests { LidarLiteV3ServiceEvent::Fault(Error::Unhealthy) )); } + + fn distance_pending() -> LidarLiteV3Service { + let mut service = LidarLiteV3Service::new( + Clock(123), + LidarLiteV3::new(Bus::default()), + AcquisitionMode::WithReceiverBiasCorrection, + ); + service.finish_startup().unwrap(); + assert!(matches!( + service.on_periodic_tick(), + LidarLiteV3ServiceEvent::Idle + )); + for _ in 0..4 { + service.clock.0 += 1_000; + assert!(matches!( + service.on_controller_interrupt(|bus, _| { + bus.response = [1 << 5, 0]; + I2cCompletion::Complete + }), + LidarLiteV3ServiceEvent::Idle + )); + service.clock.0 += 1_000; + assert!(matches!( + service.on_periodic_tick(), + LidarLiteV3ServiceEvent::Idle + )); + } + service.lidar.bus.response = [0x01, 0x23]; + service + } + + #[test] + fn completion_accepts_exact_acquisition_limit_and_discards_one_microsecond_late() { + for controller_irq in [false, true] { + for elapsed in [200_000, 200_001] { + let mut service = distance_pending(); + service.clock.0 = 123 + elapsed; + let event = if controller_irq { + service.on_controller_interrupt(|_, now| { + assert_eq!(now, Timestamp::from_microseconds(123 + elapsed)); + I2cCompletion::Complete + }) + } else { + service.on_transaction_complete(I2cCompletion::Complete) + }; + if elapsed == 200_000 { + assert!(matches!(event, LidarLiteV3ServiceEvent::Measurement(sample) + if sample.timestamp == Timestamp::from_microseconds(123) + && sample.distance_millimetres == 2_910)); + assert_eq!(service.lidar.bus.cancelled_at, None); + assert_eq!(service.lidar.bus.read_copies, 2); + } else { + assert!(matches!( + event, + LidarLiteV3ServiceEvent::Fault(Error::MeasurementTimeout) + )); + assert_eq!( + service.lidar.bus.cancelled_at, + Some(Timestamp::from_microseconds(200_124)) + ); + assert_eq!(service.lidar.bus.read_copies, 1); + } + } + } + } + + #[test] + fn delayed_timer_and_final_irq_order_cannot_publish_an_expired_measurement() { + for irq_first in [true, false] { + let mut service = distance_pending(); + // Model a flash blackout ending with both priority-11 I2C and + // priority-10 periodic service pending. The clock has advanced. + service.clock.0 = 250_123; + let irq = |service: &mut LidarLiteV3Service| { + service.on_controller_interrupt(|_, _| I2cCompletion::Complete) + }; + let first = if irq_first { + irq(&mut service) + } else { + service.on_periodic_tick() + }; + assert!(matches!( + first, + LidarLiteV3ServiceEvent::Fault(Error::MeasurementTimeout) + )); + let second = if irq_first { + service.on_periodic_tick() + } else { + irq(&mut service) + }; + assert!(matches!(second, LidarLiteV3ServiceEvent::Idle)); + assert_eq!( + service.lidar.bus.cancelled_at, + Some(Timestamp::from_microseconds(250_123)) + ); + assert_eq!(service.lidar.bus.read_copies, 1); + } + } } diff --git a/firmware/libs/kaptan-drivers/src/mpu6000/error.rs b/firmware/libs/kaptan-drivers/src/mpu6000/error.rs index 6516954..2703567 100644 --- a/firmware/libs/kaptan-drivers/src/mpu6000/error.rs +++ b/firmware/libs/kaptan-drivers/src/mpu6000/error.rs @@ -18,4 +18,16 @@ pub enum Error { /// The selected SPI route reported a terminal transfer fault. #[kaptan_error(id = 5004)] TransferFault, + /// The bus could not safely accept and copy out the completed transfer. + #[kaptan_error(id = 5005)] + Completion(kaptan_spi::CompletionError), + /// An accepted SPI transfer exceeded the 10 ms service deadline. + #[kaptan_error(id = 5006)] + TransferTimeout, + /// The bus stayed busy for the pending operation's 20 ms admission budget. + #[kaptan_error(id = 5007)] + AdmissionTimeout, + /// No acquisition was requested within two output periods plus 10 ms. + #[kaptan_error(id = 5008)] + DataReadyTimeout, } diff --git a/firmware/libs/kaptan-drivers/src/mpu6000/mod.rs b/firmware/libs/kaptan-drivers/src/mpu6000/mod.rs index 6e38f7d..119d9f5 100644 --- a/firmware/libs/kaptan-drivers/src/mpu6000/mod.rs +++ b/firmware/libs/kaptan-drivers/src/mpu6000/mod.rs @@ -6,8 +6,11 @@ mod registers; mod sample; pub mod service; +#[cfg(test)] +pub(crate) mod test_bus; + use kaptan_sensor::imu::ImuSnapshot; -use kaptan_spi::{SpiCompletion, SpiDevice}; +use kaptan_spi::{CompletionError, SpiCompletion, SpiDevice}; use kaptan_time::Timestamp; pub use config::{AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000Config}; @@ -22,6 +25,21 @@ use registers::{ }; const TRANSFER_CAPACITY: usize = SAMPLE_BYTES + 1; +// RM-MPU-6000A-00 rev 4.2, §4.28 p.41 (bundled datasheet.pdf): +// DEVICE_RESET, 100 ms, SIGNAL_PATH_RESET=7, 100 ms. +const RESET_SETTLE_US: u64 = 100_000; +const SIGNAL_PATH_RESET: u8 = 0x68; +// PS-MPU-6000A-00 rev 3.1 §6.3 p.14: register access needs up to 100 ms +// after power-up. §6.1 p.12 lists 30 ms TYPICAL gyro start-up and §6.6 p.17 +// gives a 10 ms maximum PLL settling time. Use a conservative 100 ms wake +// allowance, not a guaranteed physical settling bound. +const WAKE_SETTLE_US: u64 = 100_000; +// Host service budgets, not sensor timing specifications. A 15-byte burst on +// the supported F722 SPI route takes less than 0.3 ms. Periodic service must +// continue even if no DMA/DRDY interrupt arrives. +const TRANSFER_TIMEOUT_US: u64 = 10_000; +// Allows a retry on the F722's 10 ms periodic service before admission expires. +const ADMISSION_TIMEOUT_US: u64 = 20_000; const CONFIGURATION: [(u8, u8); 9] = [ (PWR_MGMT_1, CLOCK_SOURCE_PLL_X_GYRO), (USER_CTRL, I2C_INTERFACE_DISABLE), @@ -37,18 +55,41 @@ const CONFIGURATION: [(u8, u8); 9] = [ #[derive(Clone, Copy, Debug, Eq, PartialEq)] enum State { Dormant, - Resetting, - WaitingForReset, - ReadingIdentity, - WritingConfiguration(usize), - Ready, - ReadingSample { timestamp: Timestamp }, + Pending { + operation: Operation, + since: Timestamp, + }, + Active { + operation: Operation, + since: Timestamp, + }, + Waiting { + next: Operation, + since: Timestamp, + delay_us: u64, + }, + Settling { + since: Timestamp, + }, + Ready { + since: Timestamp, + }, + Faulted, } -/// Observable outcome of one SPI terminal event. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +enum Operation { + Reset, + ResetSignalPaths, + Identity, + Configure(usize), + Sample, +} + +/// Observable outcome of one timer or SPI terminal event. #[derive(Debug, Eq, PartialEq)] pub enum Mpu6000Event { - /// The selected SPI interrupt had no terminal transfer event. + /// No sample, initialization completion or terminal fault was produced. NotPending, /// One initialization transfer completed; initialization continues. Initializing, @@ -62,8 +103,8 @@ pub enum Mpu6000Event { /// Configured MPU6000 driver. /// -/// The driver owns fixed transfer buffers and advances only from external -/// interrupt events. It never waits for SPI status flags. +/// The driver owns ordinary movable staging buffers and advances only from +/// external events. The bus copies these bytes; hardware never addresses them. pub struct Mpu6000 { bus: Bus, config: Mpu6000Config, @@ -86,109 +127,144 @@ impl Mpu6000 { } impl Mpu6000 { - /// Starts a device reset without waiting for SPI. - /// - /// The caller must subsequently call [`Self::advance_initialization`] only - /// after the platform-selected reset-settling interval has elapsed. - pub fn begin_initialization(&mut self) -> Result<(), Error> { + /// Accepts initialization once. Busy admission is retried by periodic + /// service. All supplied instants must use one non-regressing boot clock. + /// Power must be stable by this call. Register access waits another 100 ms. + pub fn begin_initialization(&mut self, now: Timestamp) -> Result<(), Error> { if self.state != State::Dormant { return Err(Error::NotReady); } - - self.prepare_write(PWR_MGMT_1, DEVICE_RESET); - self.submit(2)?; - self.state = State::Resetting; + self.wait(Operation::Reset, now, RESET_SETTLE_US); Ok(()) } - /// Starts identity verification after the required reset-settling interval. - /// - /// This operation only submits the next asynchronous SPI transfer; it - /// never waits for a peripheral or sensor status flag. - pub fn advance_initialization(&mut self) -> Result<(), Error> { - if self.state != State::WaitingForReset { - return Ok(()); + /// Services elapsed settling, admission and acquisition deadlines. This must + /// be called periodically throughout the driver's lifetime, including while + /// a transfer is active. It never starts an unsolicited sample read. + pub fn on_periodic_tick(&mut self, now: Timestamp) -> Mpu6000Event { + if let Some(error) = self.check_deadline(now) { + return Mpu6000Event::Fault(error); } + match self.state { + State::Waiting { + next, + since, + delay_us, + } if elapsed(now, since, delay_us) => { + self.queue(next, now); + } + State::Settling { since } if elapsed(now, since, self.sample_period_us()) => { + self.state = State::Ready { since: now }; + return Mpu6000Event::Initialized; + } + _ => {} + } + match self.submit_pending(now) { + Ok(()) => Mpu6000Event::NotPending, + Err(error) => Mpu6000Event::Fault(error), + } + } - self.prepare_read(WHO_AM_I, 2); - if let Err(error) = self.submit(2) { - self.state = State::Dormant; + /// Accepts a burst-read request. Busy admission remains pending for periodic + /// retry. The eventual accepted submission time becomes the snapshot + /// timestamp, not the ISR time or a physical sampling epoch. + pub fn request_sample(&mut self, now: Timestamp) -> Result<(), Error> { + if let Some(error) = self.check_deadline(now) { return Err(error); } - self.state = State::ReadingIdentity; - Ok(()) - } - - /// Starts one fourteen-byte sensor read when initialization is complete. - pub fn request_sample(&mut self, timestamp: Timestamp) -> Result<(), Error> { - if self.state != State::Ready { + if !self.is_ready() { return Err(Error::NotReady); } - - self.prepare_read(ACCEL_XOUT_H, TRANSFER_CAPACITY); - self.submit(TRANSFER_CAPACITY)?; - self.state = State::ReadingSample { timestamp }; - Ok(()) + self.queue(Operation::Sample, now); + self.submit_pending(now) } /// Advances the state machine from an already-acknowledged DMA terminal event. pub fn on_transfer_complete( &mut self, completion: SpiCompletion, + now: Timestamp, ) -> Mpu6000Event { - self.bus.complete_transfer(completion); - match completion { - SpiCompletion::NotPending => Mpu6000Event::NotPending, - SpiCompletion::TransferFault => { - self.state = State::Dormant; + if completion == SpiCompletion::NotPending { + return Mpu6000Event::NotPending; + } + if let Some(error) = self.check_deadline(now) { + return Mpu6000Event::Fault(error); + } + let State::Active { operation, since } = self.state else { + return Mpu6000Event::NotPending; + }; + let length = if operation == Operation::Sample { + TRANSFER_CAPACITY + } else { + 2 + }; + match self + .bus + .complete_transfer(completion, &mut self.receive[..length]) + { + Ok(()) => self.advance_after_completion(operation, since, now), + Err(CompletionError::NotPending) => Mpu6000Event::NotPending, + Err(CompletionError::TransferFault) => { + self.state = State::Faulted; Mpu6000Event::Fault(Error::TransferFault) } - SpiCompletion::Complete => self.advance_after_completion(), + Err(error) => { + self.state = State::Faulted; + Mpu6000Event::Fault(Error::Completion(error)) + } } } /// Returns whether an IMU data-ready event can start a sensor transfer. pub fn is_ready(&self) -> bool { - self.state == State::Ready + matches!(self.state, State::Ready { .. }) } - fn advance_after_completion(&mut self) -> Mpu6000Event { - match self.state { - State::Dormant => Mpu6000Event::NotPending, - State::Resetting => { - self.state = State::WaitingForReset; - Mpu6000Event::Initializing - } - State::WaitingForReset => Mpu6000Event::NotPending, - State::ReadingIdentity => { + fn advance_after_completion( + &mut self, + operation: Operation, + requested: Timestamp, + now: Timestamp, + ) -> Mpu6000Event { + match operation { + Operation::Reset => self.wait(Operation::ResetSignalPaths, now, RESET_SETTLE_US), + Operation::ResetSignalPaths => self.wait(Operation::Identity, now, RESET_SETTLE_US), + Operation::Identity => { let identity = self.receive[1]; if identity != EXPECTED_WHO_AM_I { - self.state = State::Dormant; + self.state = State::Faulted; return Mpu6000Event::Fault(Error::UnexpectedIdentity(identity)); } - self.start_configuration(0) + self.queue(Operation::Configure(0), now); } - State::WritingConfiguration(index) => { + Operation::Configure(index) => { let next = index + 1; - if next == CONFIGURATION.len() { - self.state = State::Ready; - Mpu6000Event::Initialized + if index == 0 { + self.wait(Operation::Configure(next), now, WAKE_SETTLE_US); + } else if next == CONFIGURATION.len() { + // Allow a complete output period after the last write before + // accepting DRDY. RM §4.2 defines the configured sample rate. + self.state = State::Settling { since: now }; } else { - self.start_configuration(next) + self.queue(Operation::Configure(next), now); } } - State::Ready => Mpu6000Event::NotPending, - State::ReadingSample { timestamp } => { + Operation::Sample => { let mut bytes = [0; SAMPLE_BYTES]; bytes.copy_from_slice(&self.receive[1..]); - self.state = State::Ready; - Mpu6000Event::Sample(sample::decode(&bytes, timestamp, self.config)) + self.state = State::Ready { since: now }; + return Mpu6000Event::Sample(sample::decode(&bytes, requested, self.config)); } } + match self.submit_pending(now) { + Ok(()) => Mpu6000Event::Initializing, + Err(error) => Mpu6000Event::Fault(error), + } } - fn start_configuration(&mut self, index: usize) -> Mpu6000Event { + fn prepare_configuration(&mut self, index: usize) { let (register, value) = CONFIGURATION[index]; let value = match register { CONFIG => self.config.digital_low_pass_filter.register_value(), @@ -198,16 +274,6 @@ impl Mpu6000 { _ => value, }; self.prepare_write(register, value); - match self.submit(2) { - Ok(()) => { - self.state = State::WritingConfiguration(index); - Mpu6000Event::Initializing - } - Err(error) => { - self.state = State::Dormant; - Mpu6000Event::Fault(error) - } - } } fn prepare_read(&mut self, register: u8, length: usize) { @@ -223,21 +289,106 @@ impl Mpu6000 { self.transmit[..2].copy_from_slice(&[register, value]); } - fn submit(&mut self, length: usize) -> Result<(), Error> { - let (bus, transmit, receive) = (&mut self.bus, &self.transmit, &mut self.receive); - bus.start_transfer(&transmit[..length], &mut receive[..length]) - .map_err(Error::Start) + fn queue(&mut self, operation: Operation, now: Timestamp) { + self.state = State::Pending { + operation, + since: now, + }; } + + fn wait(&mut self, next: Operation, now: Timestamp, delay_us: u64) { + self.state = State::Waiting { + next, + since: now, + delay_us, + }; + } + + fn submit_pending(&mut self, now: Timestamp) -> Result<(), Error> { + let State::Pending { operation, .. } = self.state else { + return Ok(()); + }; + match operation { + Operation::Reset => self.prepare_write(PWR_MGMT_1, DEVICE_RESET), + Operation::ResetSignalPaths => self.prepare_write(SIGNAL_PATH_RESET, 0x07), + Operation::Identity => self.prepare_read(WHO_AM_I, 2), + Operation::Configure(index) => self.prepare_configuration(index), + Operation::Sample => self.prepare_read(ACCEL_XOUT_H, TRANSFER_CAPACITY), + } + let length = if operation == Operation::Sample { + TRANSFER_CAPACITY + } else { + 2 + }; + match self.bus.start_transfer(&self.transmit[..length]) { + Ok(()) => { + self.state = State::Active { + operation, + since: now, + }; + Ok(()) + } + Err(error) if Bus::is_busy(&error) => Ok(()), + Err(error) => { + self.state = State::Faulted; + Err(Error::Start(error)) + } + } + } + + fn sample_period_us(&self) -> u64 { + let base = if self.config.digital_low_pass_filter == DigitalLowPassFilter::Bandwidth260Hz { + 125 + } else { + 1_000 + }; + base * (u64::from(self.config.sample_rate_divider) + 1) + } + + fn check_deadline(&mut self, now: Timestamp) -> Option> { + let error = match self.state { + State::Active { since, .. } if elapsed(now, since, TRANSFER_TIMEOUT_US) => { + // Terminal fault completion is also the bus cancellation contract. + // It proves quiescence or quarantines permanent DMA storage. + match self + .bus + .complete_transfer(SpiCompletion::TransferFault, &mut []) + { + Err(CompletionError::TransferFault | CompletionError::NotActive) | Ok(()) => { + Error::TransferTimeout + } + Err(error) => Error::Completion(error), + } + } + State::Pending { since, .. } if elapsed(now, since, ADMISSION_TIMEOUT_US) => { + Error::AdmissionTimeout + } + State::Ready { since } + if elapsed( + now, + since, + 2 * self.sample_period_us() + TRANSFER_TIMEOUT_US, + ) => + { + Error::DataReadyTimeout + } + _ => return None, + }; + self.state = State::Faulted; + Some(error) + } +} + +fn elapsed(now: Timestamp, since: Timestamp, duration_us: u64) -> bool { + now.as_microseconds() + .checked_sub(since.as_microseconds()) + .is_some_and(|elapsed| elapsed >= duration_us) } #[cfg(test)] mod tests { - use super::{ - AccelerometerRange, DigitalLowPassFilter, Error, GyroscopeRange, Mpu6000, Mpu6000Config, - Mpu6000Event, TRANSFER_CAPACITY, - }; - use kaptan_spi::{SpiCompletion, SpiDevice}; - use kaptan_time::Timestamp; + use super::test_bus::{FakeBus, FakeError}; + use super::*; const TEST_CONFIG: Mpu6000Config = Mpu6000Config { accelerometer: AccelerometerRange::PlusMinus4G, @@ -246,79 +397,87 @@ mod tests { sample_rate_divider: 9, }; - #[derive(Debug, Eq, PartialEq)] - enum FakeError { - Start, + fn t(us: u64) -> Timestamp { + Timestamp::from_microseconds(us + 100_000) } - struct FakeBus { - receive: [u8; TRANSFER_CAPACITY], - length: usize, - fail_start: bool, + fn start(driver: &mut Mpu6000) { + driver + .begin_initialization(Timestamp::from_microseconds(0)) + .unwrap(); + driver.on_periodic_tick(Timestamp::from_microseconds(99_999)); + assert_eq!(driver.bus.starts, 0); + driver.on_periodic_tick(t(0)); } - impl FakeBus { - fn ready() -> Self { - Self { - receive: [0; TRANSFER_CAPACITY], - length: 0, - fail_start: false, - } - } - } - - impl SpiDevice for FakeBus { - type StartError = FakeError; - - fn start_transfer(&mut self, _: &[u8], receive: &mut [u8]) -> Result<(), Self::StartError> { - if self.fail_start { - return Err(FakeError::Start); - } - - self.length = receive.len(); - receive.copy_from_slice(&self.receive[..self.length]); - Ok(()) - } - - fn complete_transfer(&mut self, _: SpiCompletion) {} - } - - #[test] - fn configures_a_verified_device_without_waiting() { + fn initialized() -> Mpu6000 { let mut bus = FakeBus::ready(); - bus.receive[1] = 0x68; + bus.response[1] = 0x68; let mut driver = Mpu6000::new(bus, TEST_CONFIG); - - driver.begin_initialization().unwrap(); - assert_eq!( - driver.on_transfer_complete(SpiCompletion::Complete), - Mpu6000Event::Initializing - ); - driver.advance_initialization().unwrap(); - for _ in 0..9 { - assert_eq!( - driver.on_transfer_complete(SpiCompletion::Complete), - Mpu6000Event::Initializing - ); + start(&mut driver); + driver.on_transfer_complete(SpiCompletion::Complete, t(100)); + driver.on_periodic_tick(t(100_100)); + assert_eq!(&driver.bus.transmitted[..2], &[0x68, 7]); + driver.on_transfer_complete(SpiCompletion::Complete, t(100_200)); + driver.on_periodic_tick(t(200_200)); + driver.on_transfer_complete(SpiCompletion::Complete, t(200_300)); + driver.on_transfer_complete(SpiCompletion::Complete, t(200_400)); + driver.on_periodic_tick(t(300_400)); + for index in 1..9 { + driver.on_transfer_complete(SpiCompletion::Complete, t(300_400 + index * 100)); } assert_eq!( - driver.on_transfer_complete(SpiCompletion::Complete), + driver.on_periodic_tick(t(311_200)), Mpu6000Event::Initialized ); assert!(driver.is_ready()); + driver + } + + #[test] + fn reset_signal_reset_and_wake_wait_elapsed_time_from_completion() { + let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG); + driver.bus.response[1] = 0x68; + start(&mut driver); + driver.on_transfer_complete(SpiCompletion::Complete, t(100)); + for now in [100, 100, 50_000, 100_099] { + driver.on_periodic_tick(t(now)); + assert_eq!(driver.bus.starts, 1); + assert!(!driver.is_ready()); + } + driver.on_periodic_tick(t(100_100)); + assert_eq!(driver.bus.starts, 2); + driver.on_transfer_complete(SpiCompletion::Complete, t(100_200)); + driver.on_periodic_tick(t(200_199)); + assert_eq!(driver.bus.starts, 2); + driver.on_periodic_tick(t(200_200)); + driver.on_transfer_complete(SpiCompletion::Complete, t(200_300)); + driver.on_transfer_complete(SpiCompletion::Complete, t(200_400)); + driver.on_periodic_tick(t(300_399)); + assert_eq!(driver.bus.starts, 4); + driver.on_periodic_tick(t(300_400)); + for index in 1..9 { + driver.on_transfer_complete(SpiCompletion::Complete, t(300_400 + index * 100)); + } + driver.on_periodic_tick(t(311_199)); + assert!(!driver.is_ready()); + assert_eq!(driver.request_sample(t(311_199)), Err(Error::NotReady)); + assert_eq!( + driver.on_periodic_tick(t(311_200)), + Mpu6000Event::Initialized + ); } #[test] fn rejects_an_unexpected_identity() { let mut bus = FakeBus::ready(); - bus.receive[1] = 0x70; + bus.response[1] = 0x70; let mut driver = Mpu6000::new(bus, TEST_CONFIG); - driver.begin_initialization().unwrap(); - let _ = driver.on_transfer_complete(SpiCompletion::Complete); - driver.advance_initialization().unwrap(); + driver.queue(Operation::Identity, t(0)); + driver.submit_pending(t(0)).unwrap(); assert!(matches!( - driver.on_transfer_complete(SpiCompletion::Complete), + driver.on_transfer_complete(SpiCompletion::Complete, t(100)), Mpu6000Event::Fault(Error::UnexpectedIdentity(0x70)) )); } @@ -330,22 +489,23 @@ mod tests { 0x00, 0x00, // 36.53 C 0x40, 0x10, 0xBF, 0xF0, 0x00, 0x00, // gyro: 500, -500, 0 dps ]; - let mut bus = FakeBus::ready(); - bus.receive[1] = 0x68; - let mut driver = Mpu6000::new(bus, TEST_CONFIG); - - driver.begin_initialization().unwrap(); - let _ = driver.on_transfer_complete(SpiCompletion::Complete); - driver.advance_initialization().unwrap(); - for _ in 0..9 { - let _ = driver.on_transfer_complete(SpiCompletion::Complete); - } - let _ = driver.on_transfer_complete(SpiCompletion::Complete); - driver.bus.receive[1..].copy_from_slice(&burst); - driver - .request_sample(Timestamp::from_microseconds(42)) - .unwrap(); - let Mpu6000Event::Sample(sample) = driver.on_transfer_complete(SpiCompletion::Complete) + let mut driver = initialized(); + driver.bus.response[1..].copy_from_slice(&burst); + driver.request_sample(t(312_000)).unwrap(); + assert_eq!(driver.receive, [0; TRANSFER_CAPACITY]); + assert_eq!( + driver.on_transfer_complete(SpiCompletion::NotPending, t(312_100)), + Mpu6000Event::NotPending + ); + assert_eq!(driver.receive, [0; TRANSFER_CAPACITY]); + // Overwrite staging bytes and move the whole driver while work is pending. + // The fake bus, like the DMA route, owns the pending response itself. + driver.transmit.fill(0xff); + driver.bus.response.fill(0xff); + let mut driver = + core::mem::replace(&mut driver, Mpu6000::new(FakeBus::ready(), TEST_CONFIG)); + let Mpu6000Event::Sample(sample) = + driver.on_transfer_complete(SpiCompletion::Complete, t(312_200)) else { panic!("sample transfer must complete"); }; @@ -356,18 +516,184 @@ mod tests { [8_726, -8_726, 0] ); assert_eq!(sample.temperature_centidegrees_celsius, 3_653); + assert_eq!(sample.timestamp, t(312_000)); + assert_eq!( + driver.on_transfer_complete(SpiCompletion::Complete, t(312_300)), + Mpu6000Event::NotPending + ); } #[test] fn reports_a_route_transfer_fault() { let mut bus = FakeBus::ready(); - bus.receive[1] = 0x68; + bus.response[1] = 0x68; let mut driver = Mpu6000::new(bus, TEST_CONFIG); - driver.begin_initialization().unwrap(); + start(&mut driver); assert!(matches!( - driver.on_transfer_complete(SpiCompletion::TransferFault), + driver.on_transfer_complete(SpiCompletion::TransferFault, t(100)), Mpu6000Event::Fault(Error::TransferFault) )); } + + #[test] + fn completion_errors_never_republish_a_previous_sample() { + for (event, error) in [ + ( + SpiCompletion::Complete, + Some(CompletionError::InvalidLength), + ), + (SpiCompletion::Complete, Some(CompletionError::Quarantined)), + (SpiCompletion::Complete, Some(CompletionError::NotActive)), + ( + SpiCompletion::Complete, + Some(CompletionError::TransferFault), + ), + (SpiCompletion::TransferFault, None), + ] { + let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG); + driver.state = State::Ready { since: t(0) }; + driver.request_sample(t(1)).unwrap(); + assert!(matches!( + driver.on_transfer_complete(SpiCompletion::Complete, t(1)), + Mpu6000Event::Sample(_) + )); + driver.request_sample(t(2)).unwrap(); + driver.receive.fill(0xa5); + driver.bus.fail_completion = error; + assert!(matches!( + driver.on_transfer_complete(event, t(2)), + Mpu6000Event::Fault(_) + )); + assert_eq!(driver.receive, [0xa5; TRANSFER_CAPACITY]); + assert!(!driver.is_ready()); + assert!(!matches!( + driver.on_transfer_complete(SpiCompletion::Complete, t(3)), + Mpu6000Event::Sample(_) + )); + if error == Some(CompletionError::Quarantined) { + assert_eq!(driver.begin_initialization(t(4)), Err(Error::NotReady)); + } + } + } + + #[test] + fn rejected_submission_cannot_complete_a_stale_sample() { + let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG); + driver.state = State::Ready { since: t(0) }; + driver.receive.fill(0xa5); + driver.bus.fail_start = true; + assert_eq!( + driver.request_sample(t(1)), + Err(Error::Start(FakeError::Start)) + ); + assert_eq!( + driver.on_transfer_complete(SpiCompletion::Complete, t(2)), + Mpu6000Event::NotPending + ); + } + + #[test] + fn every_transfer_phase_times_out_and_cancels_once_without_rearming() { + let operations = [ + Operation::Reset, + Operation::ResetSignalPaths, + Operation::Identity, + Operation::Sample, + ]; + for operation in operations + .into_iter() + .chain((0..9).map(Operation::Configure)) + { + let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG); + driver.queue(operation, t(100)); + driver.submit_pending(t(100)).unwrap(); + assert_eq!(driver.on_periodic_tick(t(10_099)), Mpu6000Event::NotPending); + assert_eq!(driver.bus.cancellations, 0); + assert_eq!( + driver.on_periodic_tick(t(10_100)), + Mpu6000Event::Fault(Error::TransferTimeout) + ); + assert_eq!(driver.bus.cancellations, 1); + assert_eq!( + driver.on_transfer_complete(SpiCompletion::Complete, t(10_101)), + Mpu6000Event::NotPending + ); + driver.on_periodic_tick(t(1_000_000)); + assert_eq!(driver.bus.starts, 1); + assert_eq!( + driver.begin_initialization(t(1_000_000)), + Err(Error::NotReady) + ); + } + } + + #[test] + fn every_busy_phase_preserves_pending_operation_and_has_an_admission_deadline() { + for operation in [ + Operation::Reset, + Operation::ResetSignalPaths, + Operation::Identity, + Operation::Sample, + ] + .into_iter() + .chain((0..9).map(Operation::Configure)) + { + let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG); + driver.bus.busy = true; + driver.queue(operation, t(100)); + driver.submit_pending(t(100)).unwrap(); + assert_eq!( + driver.state, + State::Pending { + operation, + since: t(100) + } + ); + driver.on_periodic_tick(t(200)); + driver.bus.busy = false; + driver.on_periodic_tick(t(300)); + assert_eq!( + driver.state, + State::Active { + operation, + since: t(300) + } + ); + assert_eq!(driver.bus.starts, 1); + } + let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG); + driver.bus.busy = true; + start(&mut driver); + assert_eq!( + driver.on_periodic_tick(t(20_000)), + Mpu6000Event::Fault(Error::AdmissionTimeout) + ); + assert_eq!(driver.bus.cancellations, 0); + } + + #[test] + fn missed_drdy_and_late_completions_fault_instead_of_polling_or_publishing() { + let mut driver = initialized(); + driver.on_periodic_tick(t(341_199)); + assert_eq!(driver.bus.starts, 12); + assert_eq!( + driver.on_periodic_tick(t(341_200)), + Mpu6000Event::Fault(Error::DataReadyTimeout) + ); + let mut driver = initialized(); + driver.request_sample(t(312_000)).unwrap(); + assert_eq!( + driver.on_transfer_complete(SpiCompletion::Complete, t(322_000)), + Mpu6000Event::Fault(Error::TransferTimeout) + ); + let mut driver = initialized(); + driver.request_sample(t(312_000)).unwrap(); + driver.bus.fail_completion = Some(CompletionError::Quarantined); + assert_eq!( + driver.on_periodic_tick(t(322_000)), + Mpu6000Event::Fault(Error::Completion(CompletionError::Quarantined)) + ); + assert_eq!(driver.bus.start_transfer(&[1]), Err(FakeError::Quarantined)); + } } diff --git a/firmware/libs/kaptan-drivers/src/mpu6000/sample.rs b/firmware/libs/kaptan-drivers/src/mpu6000/sample.rs index cadaf30..eec10fe 100644 --- a/firmware/libs/kaptan-drivers/src/mpu6000/sample.rs +++ b/firmware/libs/kaptan-drivers/src/mpu6000/sample.rs @@ -1,4 +1,10 @@ //! MPU6000 burst-sample decoding. +//! +//! Integer division rounds toward zero before adding temperature's offset. +//! For the full signed raw domain, acceleration stays in -16,000..=15,999 mg +//! across all ranges and temperature in -5,984..=13,290 cdegC. Gyro conversion +//! uses 0.017453 rad/degree and reaches -34,871..=34,870 mrad/s at ±2000°/s. +//! Its largest intermediate magnitude is 571,899,904, which fits i32. use kaptan_sensor::imu::ImuSnapshot; use kaptan_time::Timestamp; @@ -34,10 +40,98 @@ const fn scale_acceleration(raw: i16, config: Mpu6000Config) -> i16 { ((raw as i32 * 1_000) / config.accelerometer.counts_per_g()) as i16 } -const fn scale_angular_rate(raw: i16, config: Mpu6000Config) -> i16 { - ((raw as i32 * 17_453) / config.gyroscope.milliradians_per_second_divisor()) as i16 +const fn scale_angular_rate(raw: i16, config: Mpu6000Config) -> i32 { + (raw as i32 * 17_453) / config.gyroscope.milliradians_per_second_divisor() } const fn scale_temperature(raw: i16) -> i16 { ((raw as i32 * 100) / 340 + 3_653) as i16 } + +#[cfg(test)] +mod tests { + use super::{decode, scale_temperature}; + use crate::mpu6000::config::{ + AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000Config, + }; + use kaptan_time::Timestamp; + + #[test] + fn signed_raw_endpoints_decode_in_every_full_scale_configuration() { + // Known answers from the datasheet sensitivities and the documented + // 0.017453 rad/degree conversion, truncated toward zero. + let accelerometers = [ + (AccelerometerRange::PlusMinus2G, [-2_000, 0, 1_999]), + (AccelerometerRange::PlusMinus4G, [-4_000, 0, 3_999]), + (AccelerometerRange::PlusMinus8G, [-8_000, 0, 7_999]), + (AccelerometerRange::PlusMinus16G, [-16_000, 0, 15_999]), + ]; + let gyroscopes = [ + ( + GyroscopeRange::PlusMinus250DegreesPerSecond, + [-4_365, 0, 4_365], + ), + ( + GyroscopeRange::PlusMinus500DegreesPerSecond, + [-8_731, 0, 8_731], + ), + ( + GyroscopeRange::PlusMinus1000DegreesPerSecond, + [-17_435, 0, 17_435], + ), + ( + GyroscopeRange::PlusMinus2000DegreesPerSecond, + [-34_871, 0, 34_870], + ), + ]; + let timestamp = Timestamp::from_microseconds(123); + for (accelerometer, acceleration) in accelerometers { + for (gyroscope, angular_rate) in gyroscopes { + for (temperature_raw, temperature) in [(i16::MIN, -5_984), (i16::MAX, 13_290)] { + let mut bytes = [0; 14]; + for (index, raw) in [ + i16::MIN, + 0, + i16::MAX, + temperature_raw, + i16::MIN, + 0, + i16::MAX, + ] + .into_iter() + .enumerate() + { + bytes[index * 2..index * 2 + 2].copy_from_slice(&raw.to_be_bytes()); + } + let sample = decode( + &bytes, + timestamp, + Mpu6000Config { + accelerometer, + gyroscope, + digital_low_pass_filter: DigitalLowPassFilter::Bandwidth184Hz, + sample_rate_divider: 0, + }, + ); + assert_eq!(sample.timestamp, timestamp); + assert_eq!(sample.acceleration_milligravity, acceleration); + assert_eq!(sample.angular_rate_milliradians_per_second, angular_rate); + assert_eq!(sample.temperature_centidegrees_celsius, temperature); + } + } + } + } + + #[test] + fn temperature_rounds_toward_zero_before_adding_offset() { + for (raw, expected) in [ + (-340, 3_553), + (-1, 3_653), + (0, 3_653), + (1, 3_653), + (340, 3_753), + ] { + assert_eq!(scale_temperature(raw), expected); + } + } +} diff --git a/firmware/libs/kaptan-drivers/src/mpu6000/service.rs b/firmware/libs/kaptan-drivers/src/mpu6000/service.rs index 9278f87..1005fee 100644 --- a/firmware/libs/kaptan-drivers/src/mpu6000/service.rs +++ b/firmware/libs/kaptan-drivers/src/mpu6000/service.rs @@ -39,28 +39,18 @@ where Clock: MonotonicClock, Bus: SpiDevice, { - /// Submits the asynchronous sensor reset transaction. + /// Accepts initialization and starts the power-on register-access delay. pub fn begin(&mut self) -> Result<(), Error> { - self.imu.begin_initialization() + self.imu.begin_initialization(self.clock.now()) } - /// Advances the reset-settling state on a board periodic event. + /// Services settling and missing-IRQ deadlines even with an active transfer. pub fn on_periodic_tick(&mut self) -> Mpu6000ServiceEvent { - if self.imu.is_ready() { - return match self.imu.request_sample(self.clock.now()) { - Ok(()) | Err(Error::NotReady) => Mpu6000ServiceEvent::Idle, - Err(error) => Mpu6000ServiceEvent::Fault(error), - }; - } - - let _ = self.clock.now(); - match self.imu.advance_initialization() { - Ok(()) => Mpu6000ServiceEvent::Idle, - Err(error) => Mpu6000ServiceEvent::Fault(error), - } + service_event(self.imu.on_periodic_tick(self.clock.now())) } - /// Handles one coalesced data-ready event. + /// Handles one coalesced data-ready notification. Time is captured here in + /// the worker, at read submission, not at IRQ entry or physical sampling. pub fn on_data_ready(&mut self) -> Mpu6000ServiceEvent { if !self.imu.is_ready() { return Mpu6000ServiceEvent::Idle; @@ -77,22 +67,27 @@ where &mut self, completion: SpiCompletion, ) -> Mpu6000ServiceEvent { - match self.imu.on_transfer_complete(completion) { - Mpu6000Event::NotPending | Mpu6000Event::Initializing => Mpu6000ServiceEvent::Idle, - Mpu6000Event::Initialized => Mpu6000ServiceEvent::Initialized, - Mpu6000Event::Sample(sample) => Mpu6000ServiceEvent::Sample(sample), - Mpu6000Event::Fault(error) => Mpu6000ServiceEvent::Fault(error), - } + service_event(self.imu.on_transfer_complete(completion, self.clock.now())) + } +} + +fn service_event(event: Mpu6000Event) -> Mpu6000ServiceEvent { + match event { + Mpu6000Event::NotPending | Mpu6000Event::Initializing => Mpu6000ServiceEvent::Idle, + Mpu6000Event::Initialized => Mpu6000ServiceEvent::Initialized, + Mpu6000Event::Sample(sample) => Mpu6000ServiceEvent::Sample(sample), + Mpu6000Event::Fault(error) => Mpu6000ServiceEvent::Fault(error), } } #[cfg(test)] mod tests { use super::{Mpu6000Service, Mpu6000ServiceEvent}; + use crate::mpu6000::test_bus::FakeBus; use crate::mpu6000::{ AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000, Mpu6000Config, }; - use kaptan_spi::{SpiCompletion, SpiDevice}; + use kaptan_spi::SpiCompletion; use kaptan_time::{MonotonicClock, Timestamp}; const TEST_CONFIG: Mpu6000Config = Mpu6000Config { @@ -102,62 +97,80 @@ mod tests { sample_rate_divider: 9, }; - struct Clock; + struct Clock(u64); impl MonotonicClock for Clock { fn now(&mut self) -> Timestamp { - Timestamp::from_microseconds(42) + Timestamp::from_microseconds(self.0) } } - struct Bus { - response: [u8; 15], - } - - impl SpiDevice for Bus { - type StartError = (); - - fn start_transfer(&mut self, _: &[u8], receive: &mut [u8]) -> Result<(), Self::StartError> { - receive.copy_from_slice(&self.response[..receive.len()]); - Ok(()) - } - - fn complete_transfer(&mut self, _: SpiCompletion) {} - } - #[test] fn accepts_a_sample_only_after_initialization() { - let mut response = [0; 15]; - response[1] = 0x68; - let mut service = Mpu6000Service::new(Clock, Mpu6000::new(Bus { response }, TEST_CONFIG)); + let mut bus = FakeBus::ready(); + bus.response[1] = 0x68; + let mut service = Mpu6000Service::new(Clock(0), Mpu6000::new(bus, TEST_CONFIG)); service.begin().unwrap(); assert!(matches!(service.on_data_ready(), Mpu6000ServiceEvent::Idle)); + service.clock.0 = 99_999; + service.on_periodic_tick(); + assert_eq!(service.imu.bus.starts, 0); + service.clock.0 = 100_000; + service.on_periodic_tick(); assert!(matches!( service.on_transfer_complete(SpiCompletion::Complete), Mpu6000ServiceEvent::Idle )); - assert!(matches!( - service.on_periodic_tick(), - Mpu6000ServiceEvent::Idle - )); - for _ in 0..9 { + service.clock.0 = 200_000; + service.on_periodic_tick(); + service.on_transfer_complete(SpiCompletion::Complete); + service.clock.0 = 300_000; + service.on_periodic_tick(); + service.on_transfer_complete(SpiCompletion::Complete); + service.on_transfer_complete(SpiCompletion::Complete); + service.clock.0 = 400_000; + service.on_periodic_tick(); + for _ in 1..9 { assert!(matches!( service.on_transfer_complete(SpiCompletion::Complete), Mpu6000ServiceEvent::Idle )); } + service.clock.0 = 410_000; assert!(matches!( - service.on_transfer_complete(SpiCompletion::Complete), + service.on_periodic_tick(), Mpu6000ServiceEvent::Initialized )); + service.clock.0 = 410_042; + assert!(matches!(service.on_data_ready(), Mpu6000ServiceEvent::Idle)); + service.clock.0 = 410_123; + assert!(matches!( + service.on_transfer_complete(SpiCompletion::Complete), + Mpu6000ServiceEvent::Sample(sample) if sample.timestamp == Timestamp::from_microseconds(410_042) + )); + } + + #[test] + fn fake_clock_expires_active_work_without_any_irq() { + let mut service = + Mpu6000Service::new(Clock(50), Mpu6000::new(FakeBus::ready(), TEST_CONFIG)); + service.begin().unwrap(); + service.clock.0 = 100_050; + service.on_periodic_tick(); + service.clock.0 = 110_049; assert!(matches!( service.on_periodic_tick(), Mpu6000ServiceEvent::Idle )); + service.clock.0 = 110_050; + assert!(matches!( + service.on_periodic_tick(), + Mpu6000ServiceEvent::Fault(super::Error::TransferTimeout) + )); assert!(matches!( service.on_transfer_complete(SpiCompletion::Complete), - Mpu6000ServiceEvent::Sample(_) + Mpu6000ServiceEvent::Idle )); } } diff --git a/firmware/libs/kaptan-drivers/src/mpu6000/test_bus.rs b/firmware/libs/kaptan-drivers/src/mpu6000/test_bus.rs new file mode 100644 index 0000000..c35ec31 --- /dev/null +++ b/firmware/libs/kaptan-drivers/src/mpu6000/test_bus.rs @@ -0,0 +1,97 @@ +//! Test transport with owned pending bytes, never retained caller pointers. + +use kaptan_spi::{CompletionError, SpiCompletion, SpiDevice}; + +#[derive(Debug, Eq, PartialEq)] +pub(crate) enum FakeError { + Start, + Busy, + InvalidLength, + Quarantined, +} + +pub(crate) struct FakeBus { + pub response: [u8; 15], + pub fail_start: bool, + pub fail_completion: Option, + pub busy: bool, + pub starts: usize, + pub cancellations: usize, + pub transmitted: [u8; 15], + pending: Option<([u8; 15], usize)>, + quarantined: bool, +} + +impl FakeBus { + pub fn ready() -> Self { + Self { + response: [0; 15], + fail_start: false, + fail_completion: None, + busy: false, + starts: 0, + cancellations: 0, + transmitted: [0; 15], + pending: None, + quarantined: false, + } + } +} + +impl SpiDevice for FakeBus { + type StartError = FakeError; + + fn is_busy(error: &Self::StartError) -> bool { + *error == FakeError::Busy + } + + fn start_transfer(&mut self, transmit: &[u8]) -> Result<(), Self::StartError> { + if self.quarantined { + return Err(FakeError::Quarantined); + } + if self.pending.is_some() || self.busy { + return Err(FakeError::Busy); + } + if self.fail_start { + return Err(FakeError::Start); + } + if transmit.is_empty() || transmit.len() > self.response.len() { + return Err(FakeError::InvalidLength); + } + // Simulated hardware computes its response at submission, but only the + // accepted completion is allowed to copy those owned bytes to a caller. + self.pending = Some((self.response, transmit.len())); + self.transmitted[..transmit.len()].copy_from_slice(transmit); + self.starts += 1; + Ok(()) + } + + fn complete_transfer( + &mut self, + completion: SpiCompletion, + receive: &mut [u8], + ) -> Result<(), CompletionError> { + if completion == SpiCompletion::NotPending { + return Err(CompletionError::NotPending); + } + if self.quarantined { + return Err(CompletionError::Quarantined); + } + if completion == SpiCompletion::TransferFault { + self.cancellations += 1; + } + let (pending, length) = self.pending.take().ok_or(CompletionError::NotActive)?; + if let Some(error) = self.fail_completion.take() { + self.quarantined = error == CompletionError::Quarantined; + return Err(error); + } + if completion == SpiCompletion::TransferFault { + return Err(CompletionError::TransferFault); + } + if receive.len() != length { + return Err(CompletionError::InvalidLength); + } + receive.copy_from_slice(&pending[..length]); + Ok(()) + } +} diff --git a/firmware/libs/kaptan-drivers/src/mtf01/mod.rs b/firmware/libs/kaptan-drivers/src/mtf01/mod.rs index f9cfbcf..2f6e774 100644 --- a/firmware/libs/kaptan-drivers/src/mtf01/mod.rs +++ b/firmware/libs/kaptan-drivers/src/mtf01/mod.rs @@ -1,14 +1,7 @@ //! MicoAir MTF-01 Microlink UART frame decoder. -//! -//! Device documentation belongs beside this module. -//! -//! The MTF-01 continuously transmits fixed-size Microlink range-sensor frames. -//! Feed each received UART byte to [`mtf01::Mtf01::receive_byte`]. The parser is -//! allocation-free, does not use packed structs, and resynchronizes on a new -//! frame header after malformed input. use kaptan_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot}; -use kaptan_time::{MonotonicClock, Timestamp}; +use kaptan_time::Timestamp; const FRAME_HEADER: u8 = 0xEF; const RANGE_SENSOR_MESSAGE_ID: u8 = 0x51; @@ -26,11 +19,7 @@ enum State { Checksum, } -/// One complete MTF-01 range and optical-flow report. -/// -/// This preserves the MTF-01's device-specific fields. Use -/// [`Self::range_snapshot`] or [`Self::optical_flow_snapshot`] to obtain a -/// device-independent observation for the rest of the flight stack. +/// MTF-01 range and optical-flow report in device units. #[derive(Clone, Copy, Debug, Eq, PartialEq)] pub struct Mtf01Report { /// Flight-controller monotonic time at frame reception. @@ -58,34 +47,6 @@ pub struct Mtf01Report { } impl Mtf01Report { - /// Creates an MTF-01 observation from one validated Microlink frame. - #[allow(clippy::too_many_arguments)] - pub const fn new( - timestamp: Timestamp, - sensor_time_milliseconds: u32, - distance_millimetres: u32, - distance_strength: u8, - distance_precision: u8, - distance_valid: bool, - flow_velocity_x_centimetres_per_second_at_one_metre: i16, - flow_velocity_y_centimetres_per_second_at_one_metre: i16, - flow_quality: u8, - flow_valid: bool, - ) -> Self { - Self { - timestamp, - sensor_time_milliseconds, - distance_millimetres, - distance_strength, - distance_precision, - distance_valid, - flow_velocity_x_centimetres_per_second_at_one_metre, - flow_velocity_y_centimetres_per_second_at_one_metre, - flow_quality, - flow_valid, - } - } - /// Returns the valid range portion of this report. pub const fn range_snapshot(self) -> Option { if self.distance_valid && self.distance_millimetres != 0 { @@ -129,13 +90,10 @@ impl Mtf01Report { } } -/// MTF-01 Microlink decoder. +/// Microlink decoder for one UART receive stream. /// -/// One decoder is owned by one UART receive path. It accepts the MTF-01's -/// documented range-sensor message ID and 20-byte payload length. Device and -/// system IDs are deliberately accepted as transmitted, matching MicoAir's -/// reference decoder and allowing sensor IDs configured outside factory -/// defaults. +/// Accepts message ID 0x51 with a 20-byte payload. Device and system IDs are +/// unrestricted, matching MicoAir's reference decoder. pub struct Mtf01 { state: State, checksum: u8, @@ -155,9 +113,8 @@ impl Mtf01 { /// Accepts one UART byte and returns a complete, checksum-validated MTF-01 /// report when available. /// - /// `timestamp` should be the controller monotonic time at receipt of this - /// byte. It is used only for a completed frame, so it records the arrival - /// time of the report rather than an arbitrary intermediate byte. + /// `timestamp` is the controller's monotonic time at byte receipt. + /// The report retains the checksum byte's timestamp. pub fn receive_byte(&mut self, timestamp: Timestamp, byte: u8) -> Option { match self.state { State::Header => { @@ -198,10 +155,7 @@ impl Mtf01 { } State::Checksum => { let valid = byte == self.checksum; - // The checksum terminates the frame even when its value is - // also the Microlink header. The reference parser returns to - // its initial state here rather than treating checksum data as - // the start of the next frame. + // A checksum of 0xEF ends the frame and is not a new header. self.reset(); if valid { return Some(self.snapshot(timestamp)); @@ -233,18 +187,22 @@ impl Mtf01 { } fn snapshot(&self, timestamp: Timestamp) -> Mtf01Report { - Mtf01Report::new( + Mtf01Report { timestamp, - u32::from_le_bytes(self.payload[0..4].try_into().unwrap()), - u32::from_le_bytes(self.payload[4..8].try_into().unwrap()), - self.payload[8], - self.payload[9], - self.payload[10] == 1, - i16::from_le_bytes(self.payload[12..14].try_into().unwrap()), - i16::from_le_bytes(self.payload[14..16].try_into().unwrap()), - self.payload[16], - self.payload[17] == 1, - ) + sensor_time_milliseconds: u32::from_le_bytes(self.payload[0..4].try_into().unwrap()), + distance_millimetres: u32::from_le_bytes(self.payload[4..8].try_into().unwrap()), + distance_strength: self.payload[8], + distance_precision: self.payload[9], + distance_valid: self.payload[10] == 1, + flow_velocity_x_centimetres_per_second_at_one_metre: i16::from_le_bytes( + self.payload[12..14].try_into().unwrap(), + ), + flow_velocity_y_centimetres_per_second_at_one_metre: i16::from_le_bytes( + self.payload[14..16].try_into().unwrap(), + ), + flow_quality: self.payload[16], + flow_valid: self.payload[17] == 1, + } } } @@ -254,42 +212,10 @@ impl Default for Mtf01 { } } -/// Timestamping MTF-01 UART receive service. -/// -/// Feed this service from the selected UART receive interrupt or DMA consumer. -/// The board retains ownership of pin muxing, buffering, and interrupt -/// acknowledgement. -pub struct Mtf01Service { - clock: Clock, - decoder: Mtf01, -} - -impl Mtf01Service { - /// Creates an MTF-01 receive service awaiting a Microlink frame header. - pub const fn new(clock: Clock) -> Self { - Self { - clock, - decoder: Mtf01::new(), - } - } - - /// Discards a partial frame after a UART transport fault. - pub fn reset(&mut self) { - self.decoder.reset(); - } -} - -impl Mtf01Service { - /// Consumes one UART byte and returns one validated MTF-01 report. - pub fn on_received_byte(&mut self, byte: u8) -> Option { - self.decoder.receive_byte(self.clock.now(), byte) - } -} - #[cfg(test)] mod tests { - use super::{Mtf01, Mtf01Report, Mtf01Service}; - use kaptan_time::{MonotonicClock, Timestamp}; + use super::{Mtf01, Mtf01Report}; + use kaptan_time::Timestamp; fn frame(payload: [u8; 20]) -> [u8; 27] { let mut frame = [0_u8; 27]; @@ -323,18 +249,18 @@ mod tests { assert_eq!( report, - Some(Mtf01Report::new( + Some(Mtf01Report { timestamp, - 0x1234_5678, - 1_234, - 200, - 7, - true, - -1_234, - 1_234, - 250, - true, - )) + sensor_time_milliseconds: 0x1234_5678, + distance_millimetres: 1_234, + distance_strength: 200, + distance_precision: 7, + distance_valid: true, + flow_velocity_x_centimetres_per_second_at_one_metre: -1_234, + flow_velocity_y_centimetres_per_second_at_one_metre: 1_234, + flow_quality: 250, + flow_valid: true, + }) ); } @@ -423,18 +349,18 @@ mod tests { #[test] fn exposes_only_valid_generic_observations() { - let mut report = Mtf01Report::new( - Timestamp::from_microseconds(123), - 7, - 2_000, - 8, - 9, - true, - 50, - -25, - 12, - true, - ); + let mut report = Mtf01Report { + timestamp: Timestamp::from_microseconds(123), + sensor_time_milliseconds: 7, + distance_millimetres: 2_000, + distance_strength: 8, + distance_precision: 9, + distance_valid: true, + flow_velocity_x_centimetres_per_second_at_one_metre: 50, + flow_velocity_y_centimetres_per_second_at_one_metre: -25, + flow_quality: 12, + flow_valid: true, + }; assert_eq!(report.range_snapshot().unwrap().distance_millimetres, 2_000); assert_eq!( report @@ -449,21 +375,15 @@ mod tests { assert_eq!(report.optical_flow_snapshot(), None); } - struct Clock(u64); - - impl MonotonicClock for Clock { - fn now(&mut self) -> Timestamp { - self.0 += 1; - Timestamp::from_microseconds(self.0) - } - } - #[test] fn timestamps_a_report_with_its_last_received_byte() { - let mut service = Mtf01Service::new(Clock(100)); + let mut decoder = Mtf01::new(); let report = sample_frame() .into_iter() - .find_map(|byte| service.on_received_byte(byte)) + .enumerate() + .find_map(|(index, byte)| { + decoder.receive_byte(Timestamp::from_microseconds(101 + index as u64), byte) + }) .unwrap(); assert_eq!(report.timestamp, Timestamp::from_microseconds(127)); diff --git a/firmware/libs/kaptan-file-mavlink/src/lib.rs b/firmware/libs/kaptan-file-mavlink/src/lib.rs index ad04161..6bd12a4 100644 --- a/firmware/libs/kaptan-file-mavlink/src/lib.rs +++ b/firmware/libs/kaptan-file-mavlink/src/lib.rs @@ -18,7 +18,7 @@ pub use protocol::{ Content, ControlRequest, ControlResponse, DataRequest, DataResponse, FileFailure, FileMetadata, FileOperation, FilePath, FileRequest, FileResponse, FileStatus, Principal, Target, }; -pub use service::{FileProvider, FileService, crc32c}; +pub use service::{DispatchResult, FileProvider, FileService, IngressId, TransferPeer, crc32c}; /// Bytes transported by one `FILE_DATA` record. pub const DATA_BYTES: usize = 246; diff --git a/firmware/libs/kaptan-file-mavlink/src/protocol.rs b/firmware/libs/kaptan-file-mavlink/src/protocol.rs index c3d6bef..cdc3160 100644 --- a/firmware/libs/kaptan-file-mavlink/src/protocol.rs +++ b/firmware/libs/kaptan-file-mavlink/src/protocol.rs @@ -36,7 +36,7 @@ impl Target { } } -/// Exact MAVLink source that owns an open transfer. +/// Claimed MAVLink source identity; these wire fields are not authentication. #[derive(Clone, Copy, Debug, Eq, PartialEq)] pub struct Principal { system: u8, @@ -104,9 +104,9 @@ pub enum FileOperation { pub enum FileStatus { /// The operation succeeded. Ok = 1, - /// Another principal owns the single transfer slot. + /// Another peer owns the single transfer slot. Busy = 2, - /// The transfer identity is absent, stale, or owned by another principal. + /// The transfer identity is absent, stale, or owned by another peer. InvalidTransfer = 3, /// The operation is not valid for the transfer's current mode or state. InvalidState = 4, diff --git a/firmware/libs/kaptan-file-mavlink/src/service.rs b/firmware/libs/kaptan-file-mavlink/src/service.rs index 4c83ac7..318ca2e 100644 --- a/firmware/libs/kaptan-file-mavlink/src/service.rs +++ b/firmware/libs/kaptan-file-mavlink/src/service.rs @@ -6,6 +6,46 @@ use crate::{ Principal, Target, }; +/// Caller-assigned ingress identity, supplied out of band rather than on the wire. +/// +/// Distinct receive paths must use distinct, stable IDs while a lease is retained. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct IngressId(pub u8); + +/// Transfer owner, including the receive path of the claimed MAVLink identity. +/// +/// This isolates ingresses; it does not authenticate the principal on an ingress. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct TransferPeer { + pub principal: Principal, + pub ingress: IngressId, +} + +/// Response and the service's authoritative lease-renewal decision. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct DispatchResult { + pub response: FileResponse, + /// Accepted owner activity or an exact accepted retry, including a cached + /// terminal failure. Rejections never renew the lease, regardless of status. + pub refresh_lease: bool, +} + +impl DispatchResult { + fn control(response: ControlResponse, refresh_lease: bool) -> Self { + Self { + response: FileResponse::Control(response), + refresh_lease, + } + } + + fn data(response: DataResponse) -> Self { + Self { + response: FileResponse::Data(response), + refresh_lease: true, + } + } +} + /// Format-agnostic owner of the one file currently pinned or staged. /// /// The service calls `abort` after any provider failure, explicit abort, lease @@ -69,7 +109,7 @@ struct LastChunk { } struct Session { - owner: Principal, + owner: TransferPeer, transfer_id: u16, path: FilePath, mode: Mode, @@ -81,7 +121,7 @@ struct Session { #[derive(Clone, Copy)] struct Completed { - owner: Principal, + owner: TransferPeer, transfer_id: u16, request: FileRequest, response: ControlResponse, @@ -108,17 +148,19 @@ impl FileService { } } - /// Handles one already integrity-checked request. + /// Handles one already integrity-checked request from an explicit ingress. /// /// Untargeted requests are silently discarded. The caller may retry a /// response whose transmission was not admitted: every state-changing - /// request has an exact bounded replay path. + /// request has an exact bounded replay path. Renew the open/replay lease only + /// when `refresh_lease` is true, independently of response delivery. The peer + /// must come from the received frame's identity and caller-known ingress. pub fn dispatch( &mut self, provider: &mut P, - principal: Principal, + peer: TransferPeer, request: FileRequest, - ) -> Option { + ) -> Option { let addressed = match request { FileRequest::Control(request) => request.target, FileRequest::Data(request) => request.target, @@ -128,16 +170,15 @@ impl FileService { } Some(match request { FileRequest::Control(request) if request.operation == FileOperation::Read => { - self.read(provider, principal, request) + self.read(provider, peer, request) } - FileRequest::Control(request) => { - FileResponse::Control(self.control(provider, principal, request)) - } - FileRequest::Data(request) => self.data(provider, principal, request), + FileRequest::Control(request) => self.control(provider, peer, request), + FileRequest::Data(request) => self.data(provider, peer, request), }) } - /// Aborts the open transfer, if any, and clears replay state. + /// Aborts the open transfer, if any, and clears owner and replay state. + /// Returns whether an open provider transfer was aborted. pub fn expire(&mut self, provider: &mut P) -> bool { let expired = matches!(self.state, TransferState::Open(_)); if expired { @@ -150,28 +191,32 @@ impl FileService { fn control( &mut self, provider: &mut P, - principal: Principal, + peer: TransferPeer, request: ControlRequest, - ) -> ControlResponse { + ) -> DispatchResult { match request.operation { - FileOperation::OpenRead => self.open(provider, principal, request, Mode::Read), - FileOperation::OpenWrite => self.open(provider, principal, request, Mode::Write), + FileOperation::OpenRead => self.open(provider, peer, request, Mode::Read), + FileOperation::OpenWrite => self.open(provider, peer, request, Mode::Write), FileOperation::Read => unreachable!("read requests are dispatched as data responses"), - FileOperation::Close => self.close(provider, principal, request), - FileOperation::Abort => self.abort(provider, principal, request), - FileOperation::WriteAck => { - self.failure(principal, request, FileStatus::InvalidState, Content::EMPTY) - } + FileOperation::Close => self.close(provider, peer, request), + FileOperation::Abort => self.abort(provider, peer, request), + FileOperation::WriteAck => self.failure( + peer.principal, + request, + FileStatus::InvalidState, + Content::EMPTY, + ), } } fn open( &mut self, provider: &mut P, - principal: Principal, + peer: TransferPeer, request: ControlRequest, mode: Mode, - ) -> ControlResponse { + ) -> DispatchResult { + let principal = peer.principal; if request.transfer_id == 0 || request.position != 0 { return self.failure( principal, @@ -188,7 +233,7 @@ impl FileService { }; let replace_open = if let TransferState::Open(session) = &self.state { - if session.owner != principal { + if session.owner != peer { return self.failure(principal, request, FileStatus::Busy, Content::EMPTY); } let same = session.transfer_id == request.transfer_id @@ -229,7 +274,7 @@ impl FileService { self.state = TransferState::Idle; } if let TransferState::Completed(completed) = &self.state { - if completed.owner != principal { + if completed.owner != peer { return self.failure(principal, request, FileStatus::Busy, Content::EMPTY); } if completed.transfer_id == request.transfer_id { @@ -258,7 +303,7 @@ impl FileService { } }; self.state = TransferState::Open(Session { - owner: principal, + owner: peer, transfer_id: request.transfer_id, path, mode, @@ -276,44 +321,37 @@ impl FileService { fn read( &mut self, provider: &mut P, - principal: Principal, + peer: TransferPeer, request: ControlRequest, - ) -> FileResponse { + ) -> DispatchResult { + let principal = peer.principal; if !canonical_non_open(&request) { - return FileResponse::Control(self.failure( - principal, - request, - FileStatus::InvalidState, - Content::EMPTY, - )); + return self.failure(principal, request, FileStatus::InvalidState, Content::EMPTY); } let TransferState::Open(session) = &mut self.state else { - return FileResponse::Control(self.replay_or_failure(principal, request)); + return self.replay_or_failure(peer, request); }; - if session.owner != principal || session.transfer_id != request.transfer_id { - return FileResponse::Control(response( + if session.owner != peer || session.transfer_id != request.transfer_id { + return self.failure( principal, - request.operation, - request.transfer_id, - request.position, - FileMetadata { - size: request.file_size, - checksum_crc32c: request.checksum_crc32c, - }, + request, FileStatus::InvalidTransfer, Content::EMPTY, - )); + ); } if session.mode != Mode::Read { - return FileResponse::Control(response( - principal, - request.operation, - request.transfer_id, - request.position, - session.metadata, - FileStatus::InvalidState, - Content::EMPTY, - )); + return DispatchResult::control( + response( + principal, + request.operation, + request.transfer_id, + request.position, + session.metadata, + FileStatus::InvalidState, + Content::EMPTY, + ), + false, + ); } if request.position != session.position { if let Some(last) = session @@ -321,44 +359,53 @@ impl FileService { .as_ref() .filter(|last| last.position == request.position) { - return FileResponse::Data(DataResponse { + return DispatchResult::data(DataResponse { target: principal.target(), position: request.position, transfer_id: request.transfer_id, data: last.data, }); } - return FileResponse::Control(response( - principal, - request.operation, - request.transfer_id, - request.position, - session.metadata, - FileStatus::InvalidPosition, - Content::EMPTY, - )); + return DispatchResult::control( + response( + principal, + request.operation, + request.transfer_id, + request.position, + session.metadata, + FileStatus::InvalidPosition, + Content::EMPTY, + ), + false, + ); } let Some(logical) = chunk_length(session.metadata.size, request.position) else { - return FileResponse::Control(response( - principal, - request.operation, - request.transfer_id, - request.position, - session.metadata, - FileStatus::InvalidPosition, - Content::EMPTY, - )); + return DispatchResult::control( + response( + principal, + request.operation, + request.transfer_id, + request.position, + session.metadata, + FileStatus::InvalidPosition, + Content::EMPTY, + ), + false, + ); }; if logical == 0 { - return FileResponse::Control(response( - principal, - request.operation, - request.transfer_id, - request.position, - session.metadata, - FileStatus::InvalidPosition, - Content::EMPTY, - )); + return DispatchResult::control( + response( + principal, + request.operation, + request.transfer_id, + request.position, + session.metadata, + FileStatus::InvalidPosition, + Content::EMPTY, + ), + false, + ); } let mut data = [0_u8; DATA_BYTES]; if let Err(error) = provider.read(request.position, &mut data[..logical]) { @@ -373,15 +420,14 @@ impl FileService { error.status(), error.content(), ); - self.remember(principal, FileRequest::Control(request), response); - return FileResponse::Control(response); + return self.remember(peer, FileRequest::Control(request), response); } session.last_chunk = Some(LastChunk { position: request.position, data, }); session.position += logical as u32; - FileResponse::Data(DataResponse { + DispatchResult::data(DataResponse { target: principal.target(), position: request.position, transfer_id: request.transfer_id, @@ -392,66 +438,64 @@ impl FileService { fn data( &mut self, provider: &mut P, - principal: Principal, + peer: TransferPeer, request: DataRequest, - ) -> FileResponse { + ) -> DispatchResult { + let principal = peer.principal; let TransferState::Open(session) = &mut self.state else { - return FileResponse::Control(self.replay_data_or_failure(principal, request)); + return self.replay_data_or_failure(peer, request); }; - if session.owner != principal || session.transfer_id != request.transfer_id { - return FileResponse::Control(data_failure( - principal, - request, - FileStatus::InvalidTransfer, - )); + if session.owner != peer || session.transfer_id != request.transfer_id { + return DispatchResult::control( + data_failure(principal, request, FileStatus::InvalidTransfer), + false, + ); } if session.mode != Mode::Write { - return FileResponse::Control(data_failure( - principal, - request, - FileStatus::InvalidState, - )); + return DispatchResult::control( + data_failure(principal, request, FileStatus::InvalidState), + false, + ); } let logical = match chunk_length(session.metadata.size, request.position) { Some(length) if length != 0 => length, _ => { - return FileResponse::Control(data_failure( - principal, - request, - FileStatus::InvalidPosition, - )); + return DispatchResult::control( + data_failure(principal, request, FileStatus::InvalidPosition), + false, + ); } }; if request.data[logical..].iter().any(|byte| *byte != 0) { - return FileResponse::Control(data_failure( - principal, - request, - FileStatus::RequestConflict, - )); + return DispatchResult::control( + data_failure(principal, request, FileStatus::RequestConflict), + false, + ); } if request.position < session.position { let replayed = session .last_chunk .as_ref() .is_some_and(|last| last.position == request.position && last.data == request.data); - return FileResponse::Control(if replayed { - write_ack(principal, session) - } else { - data_failure(principal, request, FileStatus::RequestConflict) - }); + return DispatchResult::control( + if replayed { + write_ack(principal, session) + } else { + data_failure(principal, request, FileStatus::RequestConflict) + }, + replayed, + ); } if request.position != session.position { - return FileResponse::Control(data_failure( - principal, - request, - FileStatus::InvalidPosition, - )); + return DispatchResult::control( + data_failure(principal, request, FileStatus::InvalidPosition), + false, + ); } if let Err(error) = provider.write(request.position, &request.data[..logical]) { provider.abort(); let response = data_provider_failure(principal, request, error); - self.remember(principal, FileRequest::Data(request), response); - return FileResponse::Control(response); + return self.remember(peer, FileRequest::Data(request), response); } session.checksum.update(&request.data[..logical]); session.position += logical as u32; @@ -459,22 +503,23 @@ impl FileService { position: request.position, data: request.data, }); - FileResponse::Control(write_ack(principal, session)) + DispatchResult::control(write_ack(principal, session), true) } fn close( &mut self, provider: &mut P, - principal: Principal, + peer: TransferPeer, request: ControlRequest, - ) -> ControlResponse { + ) -> DispatchResult { + let principal = peer.principal; if !canonical_non_open(&request) { return self.failure(principal, request, FileStatus::InvalidState, Content::EMPTY); } let TransferState::Open(session) = &self.state else { - return self.replay_or_failure(principal, request); + return self.replay_or_failure(peer, request); }; - if session.owner != principal || session.transfer_id != request.transfer_id { + if session.owner != peer || session.transfer_id != request.transfer_id { return self.failure( principal, request, @@ -494,14 +539,19 @@ impl FileService { && session.checksum.checksum() != session.metadata.checksum_crc32c { provider.abort(); - let response = self.failure( + let response = response( principal, - request, + request.operation, + request.transfer_id, + request.position, + FileMetadata { + size: request.file_size, + checksum_crc32c: request.checksum_crc32c, + }, FileStatus::ChecksumMismatch, Content::EMPTY, ); - self.remember(principal, FileRequest::Control(request), response); - return response; + return self.remember(peer, FileRequest::Control(request), response); } let completion = provider.close(); let response = match completion { @@ -527,23 +577,23 @@ impl FileService { ) } }; - self.remember(principal, FileRequest::Control(request), response); - response + self.remember(peer, FileRequest::Control(request), response) } fn abort( &mut self, provider: &mut P, - principal: Principal, + peer: TransferPeer, request: ControlRequest, - ) -> ControlResponse { + ) -> DispatchResult { + let principal = peer.principal; if !canonical_non_open(&request) { return self.failure(principal, request, FileStatus::InvalidState, Content::EMPTY); } let TransferState::Open(session) = &self.state else { - return self.replay_or_failure(principal, request); + return self.replay_or_failure(peer, request); }; - if session.owner != principal || session.transfer_id != request.transfer_id { + if session.owner != peer || session.transfer_id != request.transfer_id { return self.failure( principal, request, @@ -562,8 +612,7 @@ impl FileService { FileStatus::Ok, Content::EMPTY, ); - self.remember(principal, FileRequest::Control(request), response); - response + self.remember(peer, FileRequest::Control(request), response) } fn open_response( @@ -571,15 +620,18 @@ impl FileService { principal: Principal, operation: FileOperation, session: &Session, - ) -> ControlResponse { - response( - principal, - operation, - session.transfer_id, - session.position, - session.metadata, - FileStatus::Ok, - Content::EMPTY, + ) -> DispatchResult { + DispatchResult::control( + response( + principal, + operation, + session.transfer_id, + session.position, + session.metadata, + FileStatus::Ok, + Content::EMPTY, + ), + true, ) } @@ -589,32 +641,36 @@ impl FileService { request: ControlRequest, status: FileStatus, content: Content, - ) -> ControlResponse { - response( - principal, - request.operation, - request.transfer_id, - request.position, - FileMetadata { - size: request.file_size, - checksum_crc32c: request.checksum_crc32c, - }, - status, - content, + ) -> DispatchResult { + DispatchResult::control( + response( + principal, + request.operation, + request.transfer_id, + request.position, + FileMetadata { + size: request.file_size, + checksum_crc32c: request.checksum_crc32c, + }, + status, + content, + ), + false, ) } - fn replay_or_failure(&self, principal: Principal, request: ControlRequest) -> ControlResponse { + fn replay_or_failure(&self, peer: TransferPeer, request: ControlRequest) -> DispatchResult { + let principal = peer.principal; match &self.state { TransferState::Completed(completed) - if completed.owner == principal + if completed.owner == peer && completed.transfer_id == request.transfer_id && completed.request == FileRequest::Control(request) => { - completed.response + DispatchResult::control(completed.response, true) } TransferState::Completed(completed) - if completed.owner == principal && completed.transfer_id == request.transfer_id => + if completed.owner == peer && completed.transfer_id == request.transfer_id => { self.failure( principal, @@ -633,31 +689,39 @@ impl FileService { } } - fn replay_data_or_failure( - &self, - principal: Principal, - request: DataRequest, - ) -> ControlResponse { + fn replay_data_or_failure(&self, peer: TransferPeer, request: DataRequest) -> DispatchResult { + let principal = peer.principal; match &self.state { TransferState::Completed(completed) - if completed.owner == principal + if completed.owner == peer && completed.transfer_id == request.transfer_id && completed.request == FileRequest::Data(request) => { - completed.response + DispatchResult::control(completed.response, true) } TransferState::Completed(completed) - if completed.owner == principal && completed.transfer_id == request.transfer_id => + if completed.owner == peer && completed.transfer_id == request.transfer_id => { - data_failure(principal, request, FileStatus::RequestConflict) + DispatchResult::control( + data_failure(principal, request, FileStatus::RequestConflict), + false, + ) } TransferState::Idle | TransferState::Open(_) | TransferState::Completed(_) => { - data_failure(principal, request, FileStatus::InvalidTransfer) + DispatchResult::control( + data_failure(principal, request, FileStatus::InvalidTransfer), + false, + ) } } } - fn remember(&mut self, owner: Principal, request: FileRequest, response: ControlResponse) { + fn remember( + &mut self, + owner: TransferPeer, + request: FileRequest, + response: ControlResponse, + ) -> DispatchResult { let transfer_id = match request { FileRequest::Control(request) => request.transfer_id, FileRequest::Data(request) => request.transfer_id, @@ -668,6 +732,7 @@ impl FileService { request, response, }); + DispatchResult::control(response, true) } } @@ -753,7 +818,10 @@ mod tests { use super::*; const SERVICE: Target = Target::new(1, 1); - const CLIENT: Principal = Principal::new(42, 7); + const CLIENT: TransferPeer = TransferPeer { + principal: Principal::new(42, 7), + ingress: IngressId(0), + }; const PATH: &[u8] = b"/test/data.bin"; #[derive(Clone, Copy, Eq, PartialEq)] @@ -903,8 +971,12 @@ mod tests { }) } - fn control_response(response: Option) -> ControlResponse { - let Some(FileResponse::Control(response)) = response else { + fn control_response(response: Option) -> ControlResponse { + let Some(DispatchResult { + response: FileResponse::Control(response), + .. + }) = response + else { panic!("expected control response"); }; response @@ -960,20 +1032,28 @@ mod tests { assert_eq!(opened.file_size, 300); let first = service.dispatch(&mut provider, CLIENT, control(FileOperation::Read, 9, 0)); - let Some(FileResponse::Data(first)) = first else { + let Some(DispatchResult { + response: FileResponse::Data(first), + refresh_lease: true, + }) = first + else { panic!("expected data"); }; assert_eq!(&first.data, &[0x5a; DATA_BYTES]); let retry = service.dispatch(&mut provider, CLIENT, control(FileOperation::Read, 9, 0)); - assert_eq!(retry, Some(FileResponse::Data(first))); + assert_eq!(retry, Some(DispatchResult::data(first))); assert_eq!(provider.reads, 1); - let Some(FileResponse::Data(last)) = service.dispatch( + let Some(DispatchResult { + response: FileResponse::Data(last), + refresh_lease: true, + }) = service.dispatch( &mut provider, CLIENT, control(FileOperation::Read, 9, DATA_BYTES as u32), - ) else { + ) + else { panic!("expected final data"); }; assert_eq!(&last.data[..54], &source[DATA_BYTES..]); @@ -1001,7 +1081,10 @@ mod tests { ); assert!(matches!( service.dispatch(&mut provider, CLIENT, control(FileOperation::Read, 1, 0),), - Some(FileResponse::Data(_)) + Some(DispatchResult { + response: FileResponse::Data(_), + refresh_lease: true + }) )); assert_eq!( control_response(service.dispatch( @@ -1035,7 +1118,10 @@ mod tests { assert!(matches!( service.dispatch(&mut provider, CLIENT, control(FileOperation::Read, 2, 0)), - Some(FileResponse::Data(_)) + Some(DispatchResult { + response: FileResponse::Data(_), + refresh_lease: true + }) )); assert_eq!( control_response(service.dispatch( diff --git a/firmware/libs/kaptan-file-mavlink/tests/peer_lease.rs b/firmware/libs/kaptan-file-mavlink/tests/peer_lease.rs new file mode 100644 index 0000000..2798e92 --- /dev/null +++ b/firmware/libs/kaptan-file-mavlink/tests/peer_lease.rs @@ -0,0 +1,632 @@ +use kaptan_file_mavlink::{ + Content, ControlRequest, ControlResponse, DATA_BYTES, DataRequest, DispatchResult, FileFailure, + FileMetadata, FileOperation, FilePath, FileProvider, FileRequest, FileResponse, FileService, + FileStatus, IngressId, Principal, Target, TransferPeer, crc32c, wire, +}; + +const TARGET: Target = Target::new(1, 1); +const UART: TransferPeer = TransferPeer { + principal: Principal::new(42, 7), + ingress: IngressId(0), +}; +const USB: TransferPeer = TransferPeer { + ingress: IngressId(1), + ..UART +}; +const ID: u16 = 9; + +#[derive(Default)] +struct Provider { + bytes: Vec, + opens: usize, + reads: usize, + writes: usize, + closes: usize, + aborts: usize, + fail_open: bool, + fail_read: bool, + fail_write: bool, + fail_close: bool, +} + +fn failure(status: FileStatus) -> FileFailure { + FileFailure::new(status, Content::try_new(b"provider detail").unwrap()).unwrap() +} + +impl FileProvider for Provider { + fn open_read(&mut self, _: &FilePath) -> Result { + self.opens += 1; + if self.fail_open { + return Err(failure(FileStatus::NotFound)); + } + Ok(FileMetadata { + size: self.bytes.len() as u32, + checksum_crc32c: crc32c(&self.bytes), + }) + } + + fn open_write(&mut self, path: &FilePath, _: FileMetadata) -> Result<(), FileFailure> { + self.open_read(path)?; + self.bytes.clear(); + Ok(()) + } + + fn read(&mut self, position: u32, output: &mut [u8]) -> Result<(), FileFailure> { + self.reads += 1; + if self.fail_read { + return Err(failure(FileStatus::InvalidPosition)); + } + let start = position as usize; + output.copy_from_slice(&self.bytes[start..start + output.len()]); + Ok(()) + } + + fn write(&mut self, position: u32, data: &[u8]) -> Result<(), FileFailure> { + self.writes += 1; + if self.fail_write { + return Err(failure(FileStatus::InvalidTransfer)); + } + assert_eq!(position as usize, self.bytes.len()); + self.bytes.extend_from_slice(data); + Ok(()) + } + + fn close(&mut self) -> Result { + self.closes += 1; + if self.fail_close { + return Err(failure(FileStatus::StorageFailure)); + } + Ok(Content::try_new(b"committed").unwrap()) + } + + fn abort(&mut self) { + self.aborts += 1; + } +} + +fn control(operation: FileOperation, position: u32) -> FileRequest { + FileRequest::Control(ControlRequest { + operation, + target: TARGET, + position, + file_size: 0, + checksum_crc32c: 0, + transfer_id: ID, + content: Content::EMPTY, + }) +} + +fn alter(request: FileRequest, change: impl FnOnce(&mut ControlRequest)) -> FileRequest { + let FileRequest::Control(mut request) = request else { + panic!("expected control request"); + }; + change(&mut request); + FileRequest::Control(request) +} + +fn open(operation: FileOperation, bytes: &[u8]) -> FileRequest { + alter(control(operation, 0), |request| { + request.content = Content::try_new(b"/test/data.bin").unwrap(); + if operation == FileOperation::OpenWrite { + request.file_size = bytes.len() as u32; + request.checksum_crc32c = crc32c(bytes); + } + }) +} + +fn data(position: u32, bytes: &[u8]) -> FileRequest { + let mut data = [0; DATA_BYTES]; + data[..bytes.len()].copy_from_slice(bytes); + FileRequest::Data(DataRequest { + target: TARGET, + transfer_id: ID, + position, + data, + }) +} + +// Exercise the real message codecs while keeping ingress entirely out of band. +fn exchange( + service: &mut FileService, + provider: &mut Provider, + peer: TransferPeer, + request: FileRequest, +) -> DispatchResult { + let decoded = wire::decode_request(&wire::encode_request(request)).unwrap(); + let result = service.dispatch(provider, peer, decoded).unwrap(); + assert_eq!( + wire::decode_response(&wire::encode_response(result.response)), + Ok(result.response) + ); + let target = match result.response { + FileResponse::Control(response) => response.target, + FileResponse::Data(response) => response.target, + }; + assert_eq!( + target, + Target::new(peer.principal.system(), peer.principal.component()) + ); + result +} + +fn expect_control(result: DispatchResult, status: FileStatus, refresh: bool) -> ControlResponse { + assert_eq!(result.refresh_lease, refresh); + let FileResponse::Control(response) = result.response else { + panic!("expected control response"); + }; + assert_eq!(response.status, status); + response +} + +#[test] +fn equal_wire_identities_cannot_modify_or_replay_another_ingress_upload() { + for (owner, other_ingress) in [(UART, USB), (USB, UART)] { + let mut service = FileService::new(TARGET); + let mut provider = Provider::default(); + let start = open(FileOperation::OpenWrite, b"abc"); + let chunk = data(0, b"abc"); + let close = control(FileOperation::Close, 3); + let opened = exchange(&mut service, &mut provider, owner, start); + expect_control(opened, FileStatus::Ok, true); + assert_eq!(exchange(&mut service, &mut provider, owner, start), opened); + assert_eq!(provider.opens, 1); + + // Check both components of ownership, including identical transfer IDs. + for foreign in [ + other_ingress, + TransferPeer { + principal: Principal::new(43, 7), + ..owner + }, + ] { + for (request, status) in [ + (start, FileStatus::Busy), + (alter(start, |r| r.transfer_id += 1), FileStatus::Busy), + (chunk, FileStatus::InvalidTransfer), + (control(FileOperation::Read, 0), FileStatus::InvalidTransfer), + ( + control(FileOperation::Abort, 0), + FileStatus::InvalidTransfer, + ), + (close, FileStatus::InvalidTransfer), + ( + control(FileOperation::WriteAck, 0), + FileStatus::InvalidState, + ), + ] { + expect_control( + exchange(&mut service, &mut provider, foreign, request), + status, + false, + ); + } + } + assert_eq!( + ( + provider.opens, + provider.reads, + provider.writes, + provider.closes, + provider.aborts + ), + (1, 0, 0, 0, 0) + ); + + let ack = exchange(&mut service, &mut provider, owner, chunk); + expect_control(ack, FileStatus::Ok, true); + expect_control( + exchange(&mut service, &mut provider, other_ingress, chunk), + FileStatus::InvalidTransfer, + false, + ); + assert_eq!(exchange(&mut service, &mut provider, owner, chunk), ack); + assert_eq!(provider.writes, 1); + + // Lose the durable close reply, then try to steal, evict, or corrupt it. + let lost_reply = exchange(&mut service, &mut provider, owner, close); + expect_control(lost_reply, FileStatus::Ok, true); + for (request, status) in [ + (close, FileStatus::InvalidTransfer), + (chunk, FileStatus::InvalidTransfer), + (start, FileStatus::Busy), + (alter(start, |r| r.transfer_id += 1), FileStatus::Busy), + ( + control(FileOperation::Abort, 3), + FileStatus::InvalidTransfer, + ), + ( + control(FileOperation::WriteAck, 3), + FileStatus::InvalidState, + ), + ] { + expect_control( + exchange(&mut service, &mut provider, other_ingress, request), + status, + false, + ); + } + expect_control( + exchange( + &mut service, + &mut provider, + owner, + control(FileOperation::Close, 2), + ), + FileStatus::RequestConflict, + false, + ); + expect_control( + exchange(&mut service, &mut provider, owner, start), + FileStatus::RequestConflict, + false, + ); + for _ in 0..3 { + assert_eq!( + exchange(&mut service, &mut provider, owner, close), + lost_reply + ); + } + assert_eq!(provider.bytes, b"abc"); + assert_eq!( + ( + provider.opens, + provider.writes, + provider.closes, + provider.aborts + ), + (1, 1, 1, 0) + ); + + assert!(!service.expire(&mut provider)); + expect_control( + exchange(&mut service, &mut provider, owner, close), + FileStatus::InvalidTransfer, + false, + ); + expect_control( + exchange(&mut service, &mut provider, other_ingress, start), + FileStatus::Ok, + true, + ); + expect_control( + exchange(&mut service, &mut provider, owner, close), + FileStatus::InvalidTransfer, + false, + ); + assert_eq!(provider.closes, 1); + } +} + +#[test] +fn read_and_abort_replay_are_bound_to_the_full_peer() { + let mut service = FileService::new(TARGET); + let mut provider = Provider { + bytes: b"snapshot".to_vec(), + ..Provider::default() + }; + let start = open(FileOperation::OpenRead, &[]); + expect_control( + exchange(&mut service, &mut provider, UART, start), + FileStatus::Ok, + true, + ); + let read = control(FileOperation::Read, 0); + expect_control( + exchange(&mut service, &mut provider, USB, read), + FileStatus::InvalidTransfer, + false, + ); + let first = exchange(&mut service, &mut provider, UART, read); + assert!(first.refresh_lease); + let FileResponse::Data(response) = first.response else { + panic!("expected data") + }; + assert_eq!(&response.data[..8], b"snapshot"); + expect_control( + exchange(&mut service, &mut provider, USB, read), + FileStatus::InvalidTransfer, + false, + ); + expect_control( + exchange(&mut service, &mut provider, UART, data(0, b"changed")), + FileStatus::InvalidState, + false, + ); + expect_control( + exchange( + &mut service, + &mut provider, + UART, + control(FileOperation::Read, 8), + ), + FileStatus::InvalidPosition, + false, + ); + assert_eq!(exchange(&mut service, &mut provider, UART, read), first); + assert_eq!(provider.reads, 1); + + let abort = control(FileOperation::Abort, 8); + expect_control( + exchange(&mut service, &mut provider, USB, abort), + FileStatus::InvalidTransfer, + false, + ); + let aborted = exchange(&mut service, &mut provider, UART, abort); + expect_control(aborted, FileStatus::Ok, true); + expect_control( + exchange(&mut service, &mut provider, USB, abort), + FileStatus::InvalidTransfer, + false, + ); + assert_eq!(exchange(&mut service, &mut provider, UART, abort), aborted); + assert!(!service.expire(&mut provider)); + assert!(!service.expire(&mut provider)); + expect_control( + exchange(&mut service, &mut provider, UART, abort), + FileStatus::InvalidTransfer, + false, + ); + expect_control( + exchange(&mut service, &mut provider, USB, start), + FileStatus::Ok, + true, + ); + assert_eq!(provider.aborts, 1); +} + +#[test] +fn rejected_activity_cannot_retain_open_or_completed_lease() { + const LEASE: u64 = 10; + for completed in [false, true] { + let mut service = FileService::new(TARGET); + let mut provider = Provider::default(); + let start = open(FileOperation::OpenWrite, &[]); + expect_control( + exchange(&mut service, &mut provider, UART, start), + FileStatus::Ok, + true, + ); + if completed { + expect_control( + exchange( + &mut service, + &mut provider, + UART, + control(FileOperation::Close, 0), + ), + FileStatus::Ok, + true, + ); + } + let mut lease_until = LEASE; + for now in 1..LEASE { + for peer in [UART, USB] { + for request in [ + control(FileOperation::WriteAck, 0), + alter(start, |r| { + r.content = Content::try_new(b"bad path").unwrap() + }), + alter(start, |r| r.transfer_id = 0), + alter(control(FileOperation::Read, 0), |r| r.file_size = 1), + alter(control(FileOperation::Abort, 0), |r| r.checksum_crc32c = 1), + alter(control(FileOperation::Close, 0), |r| { + r.content = Content::try_new(b"bad").unwrap() + }), + data(1, b"x"), + ] { + let result = exchange(&mut service, &mut provider, peer, request); + if result.refresh_lease { + lease_until = now + LEASE; + } + assert_eq!( + lease_until, LEASE, + "rejection retained the lease: {result:?}" + ); + } + } + for request in [ + start, + control(FileOperation::Close, 0), + control(FileOperation::Abort, 0), + ] { + assert!(!exchange(&mut service, &mut provider, USB, request).refresh_lease); + } + } + assert_eq!(provider.opens, 1); + assert_eq!(provider.closes, usize::from(completed)); + assert_eq!(provider.aborts, 0); + + // A clock-owning caller expires the entire slot at the unchanged deadline. + assert_eq!(service.expire(&mut provider), !completed); + assert!(!service.expire(&mut provider)); + assert_eq!(provider.aborts, usize::from(!completed)); + expect_control( + exchange( + &mut service, + &mut provider, + UART, + control(FileOperation::Close, 0), + ), + FileStatus::InvalidTransfer, + false, + ); + expect_control( + exchange(&mut service, &mut provider, USB, start), + FileStatus::Ok, + true, + ); + } +} + +#[test] +fn invalid_owner_requests_do_not_mutate_a_live_upload_or_its_retry() { + let mut service = FileService::new(TARGET); + let mut provider = Provider::default(); + let start = open(FileOperation::OpenWrite, b"abc"); + expect_control( + exchange(&mut service, &mut provider, UART, start), + FileStatus::Ok, + true, + ); + for (request, status) in [ + (control(FileOperation::Read, 0), FileStatus::InvalidState), + ( + control(FileOperation::Close, 0), + FileStatus::InvalidPosition, + ), + (data(1, b"bc"), FileStatus::InvalidPosition), + (data(0, b"abc!"), FileStatus::RequestConflict), + ( + alter(start, |r| r.checksum_crc32c ^= 1), + FileStatus::RequestConflict, + ), + ( + alter(control(FileOperation::Close, 0), |r| r.transfer_id += 1), + FileStatus::InvalidTransfer, + ), + ] { + expect_control( + exchange(&mut service, &mut provider, UART, request), + status, + false, + ); + } + assert_eq!( + (provider.writes, provider.closes, provider.aborts), + (0, 0, 0) + ); + let accepted = exchange(&mut service, &mut provider, UART, data(0, b"abc")); + expect_control(accepted, FileStatus::Ok, true); + expect_control( + exchange(&mut service, &mut provider, UART, data(0, b"abd")), + FileStatus::RequestConflict, + false, + ); + expect_control( + exchange(&mut service, &mut provider, UART, start), + FileStatus::RequestConflict, + false, + ); + assert_eq!( + exchange(&mut service, &mut provider, UART, data(0, b"abc")), + accepted + ); + expect_control( + exchange( + &mut service, + &mut provider, + UART, + control(FileOperation::Close, 3), + ), + FileStatus::Ok, + true, + ); + assert_eq!(provider.bytes, b"abc"); + assert_eq!( + (provider.writes, provider.closes, provider.aborts), + (1, 1, 0) + ); +} + +#[test] +fn terminal_failures_refresh_and_replay_only_for_the_accepted_peer() { + for stage in 0..4 { + let mut service = FileService::new(TARGET); + let mut provider = Provider { + bytes: b"abc".to_vec(), + fail_read: stage == 0, + fail_write: stage == 1, + fail_close: stage == 2, + ..Provider::default() + }; + let operation = if stage == 0 { + FileOperation::OpenRead + } else { + FileOperation::OpenWrite + }; + let start = open(operation, b"abc"); + expect_control( + exchange(&mut service, &mut provider, UART, start), + FileStatus::Ok, + true, + ); + if stage >= 2 { + let bytes = if stage == 3 { b"abd" } else { b"abc" }; + expect_control( + exchange(&mut service, &mut provider, UART, data(0, bytes)), + FileStatus::Ok, + true, + ); + } + let (request, status) = match stage { + 0 => (control(FileOperation::Read, 0), FileStatus::InvalidPosition), + 1 => (data(0, b"abc"), FileStatus::InvalidTransfer), + 2 => (control(FileOperation::Close, 3), FileStatus::StorageFailure), + _ => ( + control(FileOperation::Close, 3), + FileStatus::ChecksumMismatch, + ), + }; + let lost_reply = exchange(&mut service, &mut provider, UART, request); + expect_control(lost_reply, status, true); + expect_control( + exchange(&mut service, &mut provider, USB, request), + FileStatus::InvalidTransfer, + false, + ); + let conflict = match request { + FileRequest::Control(_) => alter(request, |r| r.position += 1), + FileRequest::Data(_) => data(0, b"abd"), + }; + expect_control( + exchange(&mut service, &mut provider, UART, conflict), + FileStatus::RequestConflict, + false, + ); + assert_eq!( + exchange(&mut service, &mut provider, UART, request), + lost_reply + ); + assert_eq!(provider.reads, usize::from(stage == 0)); + assert_eq!(provider.writes, usize::from(stage != 0)); + assert_eq!(provider.closes, usize::from(stage == 2)); + assert_eq!(provider.aborts, 1); + } +} + +#[test] +fn failed_open_does_not_claim_or_renew_the_slot_or_evict_a_completed_reply() { + let mut service = FileService::new(TARGET); + let mut provider = Provider { + fail_open: true, + ..Provider::default() + }; + let start = open(FileOperation::OpenWrite, &[]); + expect_control( + exchange(&mut service, &mut provider, UART, start), + FileStatus::NotFound, + false, + ); + provider.fail_open = false; + expect_control( + exchange(&mut service, &mut provider, USB, start), + FileStatus::Ok, + true, + ); + let close = control(FileOperation::Close, 0); + let completed = exchange(&mut service, &mut provider, USB, close); + expect_control(completed, FileStatus::Ok, true); + provider.fail_open = true; + expect_control( + exchange( + &mut service, + &mut provider, + USB, + alter(start, |r| r.transfer_id += 1), + ), + FileStatus::NotFound, + false, + ); + assert_eq!(exchange(&mut service, &mut provider, USB, close), completed); + assert_eq!(provider.closes, 1); +} diff --git a/firmware/libs/kaptan-flight-config/Cargo.toml b/firmware/libs/kaptan-flight-config/Cargo.toml index c5a25b6..5044b8d 100644 --- a/firmware/libs/kaptan-flight-config/Cargo.toml +++ b/firmware/libs/kaptan-flight-config/Cargo.toml @@ -10,6 +10,8 @@ workspace = true [dependencies] kaptan-config = { path = "../kaptan-config" } -kaptan-err-reportable = { path = "../kaptan-err-reportable" } -kaptan-log = { path = "../kaptan-log" } -minicbor = { version = "0.25", features = ["derive"] } +kaptan-control = { path = "../kaptan-control" } +kaptan-navigation = { path = "../kaptan-navigation" } + +[dev-dependencies] +kaptan-math = { path = "../kaptan-math" } diff --git a/firmware/libs/kaptan-flight-config/src/cbor.rs b/firmware/libs/kaptan-flight-config/src/cbor.rs deleted file mode 100644 index ab5fd09..0000000 --- a/firmware/libs/kaptan-flight-config/src/cbor.rs +++ /dev/null @@ -1,382 +0,0 @@ -//! Bounded CBOR persistence codec for one explicit current record. - -use kaptan_config::{ - ConfigModel, ConfigValue, ConfigViolation, FieldWriteError, ValidationRule, ValueKind, -}; -use kaptan_err_reportable::KaptanError; -use minicbor::{Decoder, Encoder, encode::Write}; - -use crate::{CandidateProfile, FORMAT_VERSION, SCHEMA_ID, VehicleCapabilities}; - -/// Maximum complete candidate snapshot accepted by the current vehicle schema. -pub const MAXIMUM_CANDIDATE_BYTES: usize = 4096; -const FORMAT_KEY: u16 = 0; -const SCHEMA_KEY: u16 = u16::MAX; - -/// CBOR snapshot decoding failure. -#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)] -pub enum CborError { - /// The supplied payload exceeded the bounded configuration storage budget. - #[kaptan_error(id = 3001)] - TooLarge, - /// CBOR was malformed or did not match a supported versioned wire record. - #[kaptan_error(id = 3002)] - Malformed, - /// A second CBOR item followed the candidate snapshot. - #[kaptan_error(id = 3003)] - TrailingData, - /// The decoded candidate violated complete schema/capability validation. - #[kaptan_error(id = 3004)] - InvalidProfile(ConfigViolation), -} - -/// Failure while encoding one complete candidate snapshot. -/// -/// The candidate is validated before the first CBOR item is written, so a -/// caller can never export a locally valid but relationally invalid edit. -#[derive(Debug)] -pub enum CborEncodeError { - /// The complete candidate did not validate against immutable board - /// capabilities. - InvalidProfile(ConfigViolation), - /// The caller-owned CBOR writer rejected the encoded snapshot. - Encode(minicbor::encode::Error), -} - -/// Decodes exactly one bounded current record and validates it against immutable -/// board capabilities before returning a candidate to its caller. -pub fn decode_candidate( - bytes: &[u8], - capabilities: &VehicleCapabilities, -) -> Result { - if bytes.len() > MAXIMUM_CANDIDATE_BYTES { - return Err(CborError::TooLarge); - } - let candidate = decode_current(bytes)?; - candidate - .validate(capabilities) - .map_err(CborError::InvalidProfile)?; - Ok(candidate) -} - -/// Validates and encodes one complete current snapshot to a caller-owned CBOR -/// writer. -/// -/// Validation happens before creating the encoder, ensuring a failed -/// cross-field or capability check emits no partial CBOR record. -pub fn encode_candidate( - candidate: CandidateProfile, - capabilities: &VehicleCapabilities, - writer: W, -) -> Result<(), CborEncodeError> -where - W: Write, -{ - candidate - .validate(capabilities) - .map_err(CborEncodeError::InvalidProfile)?; - let mut encoder = Encoder::new(writer); - encoder - .map(CandidateProfile::FIELD_COUNT as u64 + 2) - .map_err(CborEncodeError::Encode)?; - encoder - .u16(FORMAT_KEY) - .and_then(|encoder| encoder.u16(FORMAT_VERSION)) - .map_err(CborEncodeError::Encode)?; - encoder - .u16(SCHEMA_KEY) - .and_then(|encoder| encoder.bytes(&SCHEMA_ID)) - .map_err(CborEncodeError::Encode)?; - for descriptor in CandidateProfile::descriptors() { - encoder - .u16(descriptor.id.get()) - .map_err(CborEncodeError::Encode)?; - encode_value( - &mut encoder, - candidate - .field_value(descriptor.id) - .expect("descriptor field"), - ) - .map_err(CborEncodeError::Encode)?; - } - Ok(()) -} - -fn decode_current(bytes: &[u8]) -> Result { - let mut decoder = Decoder::new(bytes); - let entry_count = CandidateProfile::FIELD_COUNT as u64 + 2; - if decoder.map().map_err(|_| CborError::Malformed)? != Some(entry_count) { - return Err(CborError::Malformed); - } - let mut format_version = None; - let mut schema_id = None; - let mut values = [None; CandidateProfile::FIELD_COUNT]; - for _ in 0..entry_count { - let key = decoder.u16().map_err(|_| CborError::Malformed)?; - match key { - FORMAT_KEY => { - if format_version - .replace(decoder.u16().map_err(|_| CborError::Malformed)?) - .is_some() - { - return Err(CborError::Malformed); - } - } - SCHEMA_KEY => { - let bytes = decoder.bytes().map_err(|_| CborError::Malformed)?; - let value: [u8; 16] = bytes.try_into().map_err(|_| CborError::Malformed)?; - if schema_id.replace(value).is_some() { - return Err(CborError::Malformed); - } - } - _ => { - let (index, descriptor) = CandidateProfile::descriptors() - .enumerate() - .find(|(_, descriptor)| descriptor.id.get() == key) - .ok_or(CborError::Malformed)?; - if values[index].is_some() { - return Err(CborError::Malformed); - } - values[index] = Some(decode_value(&mut decoder, descriptor.kind)?); - } - } - } - if decoder.position() != bytes.len() { - return Err(CborError::TrailingData); - } - if format_version != Some(FORMAT_VERSION) || schema_id != Some(SCHEMA_ID) { - return Err(CborError::InvalidProfile(ConfigViolation::global( - ValidationRule::UnsupportedFormat, - ))); - } - if values.iter().any(Option::is_none) { - return Err(CborError::Malformed); - } - let mut candidate = CandidateProfile::factory(); - for (index, value) in values.into_iter().enumerate() { - let descriptor = CandidateProfile::descriptors() - .nth(index) - .ok_or(CborError::Malformed)?; - candidate - .set_field(descriptor.id, value.ok_or(CborError::Malformed)?) - .map_err(field_write_error)?; - } - Ok(candidate) -} - -fn encode_value( - encoder: &mut Encoder, - value: ConfigValue, -) -> Result<(), minicbor::encode::Error> -where - W: Write, -{ - match value { - ConfigValue::Bool(value) => encoder.bool(value)?, - ConfigValue::U8(value) => encoder.u8(value)?, - ConfigValue::U16(value) => encoder.u16(value)?, - ConfigValue::I32(value) => encoder.i32(value)?, - ConfigValue::U32(value) => encoder.u32(value)?, - ConfigValue::U64(value) => encoder.u64(value)?, - ConfigValue::F32(value) => encoder.f32(value)?, - ConfigValue::F64(value) => encoder.f64(value)?, - }; - Ok(()) -} - -fn decode_value(decoder: &mut Decoder<'_>, kind: ValueKind) -> Result { - match kind { - ValueKind::Bool => decoder.bool().map(ConfigValue::Bool), - ValueKind::U8 => decoder.u8().map(ConfigValue::U8), - ValueKind::U16 => decoder.u16().map(ConfigValue::U16), - ValueKind::I32 => decoder.i32().map(ConfigValue::I32), - ValueKind::U32 => decoder.u32().map(ConfigValue::U32), - ValueKind::U64 => decoder.u64().map(ConfigValue::U64), - ValueKind::F32 => decoder.f32().map(ConfigValue::F32), - ValueKind::F64 => decoder.f64().map(ConfigValue::F64), - } - .map_err(|_| CborError::Malformed) -} - -fn field_write_error(error: FieldWriteError) -> CborError { - match error { - FieldWriteError::Violation(issue) => CborError::InvalidProfile(issue), - FieldWriteError::UnknownField(_) | FieldWriteError::WrongType { .. } => { - CborError::Malformed - } - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn round_trip_and_trailing_rejection_are_exact() { - let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]); - let capabilities = VehicleCapabilities::quad_x(); - encode_candidate(CandidateProfile::factory(), &capabilities, &mut writer).unwrap(); - let length = writer.position(); - let bytes = writer.into_inner(); - assert!(decode_candidate(&bytes[..length], &capabilities).is_ok()); - let mut trailing = bytes; - trailing[length] = 0xf6; - assert!(matches!( - decode_candidate(&trailing[..=length], &capabilities), - Err(CborError::TrailingData) - )); - } - - #[test] - fn encoding_rejects_a_cross_field_invalid_candidate_before_writing() { - let capabilities = VehicleCapabilities::quad_x(); - let mut candidate = CandidateProfile::factory(); - candidate - .set_field(kaptan_config::FieldId::new(2), ConfigValue::U64(50_001)) - .expect("the scalar is locally valid"); - let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]); - - assert!(matches!( - encode_candidate(candidate, &capabilities, &mut writer), - Err(CborEncodeError::InvalidProfile(violation)) - if violation.rule() == ValidationRule::Inconsistent - )); - assert_eq!(writer.position(), 0); - } - - #[test] - fn rejects_wrong_format_and_schema_before_candidate_conversion() { - let capabilities = VehicleCapabilities::quad_x(); - let (bytes, length) = encoded_test_record(FORMAT_VERSION + 1, SCHEMA_ID, |key| key); - assert!(matches!( - decode_candidate(&bytes[..length], &capabilities), - Err(CborError::InvalidProfile(issue)) - if issue.rule() == ValidationRule::UnsupportedFormat - )); - - let (bytes, length) = encoded_test_record(FORMAT_VERSION, [0; 16], |key| key); - assert!(matches!( - decode_candidate(&bytes[..length], &capabilities), - Err(CborError::InvalidProfile(issue)) - if issue.rule() == kaptan_config::ValidationRule::UnsupportedFormat - )); - } - - #[test] - fn exact_map_rejects_unknown_duplicate_and_missing_fields() { - let capabilities = VehicleCapabilities::quad_x(); - let (bytes, length) = encoded_test_record(FORMAT_VERSION, SCHEMA_ID, |key| key); - assert!(decode_candidate(&bytes[..length], &capabilities).is_ok()); - - let (unknown, length) = - encoded_test_record( - FORMAT_VERSION, - SCHEMA_ID, - |key| { - if key == 1 { 25 } else { key } - }, - ); - assert!(matches!( - decode_candidate(&unknown[..length], &capabilities), - Err(CborError::Malformed) - )); - - let (duplicate, length) = encoded_test_record(FORMAT_VERSION, SCHEMA_ID, |key| { - if key == 1 { FORMAT_KEY } else { key } - }); - assert!(matches!( - decode_candidate(&duplicate[..length], &capabilities), - Err(CborError::Malformed) - )); - - let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]); - let mut encoder = Encoder::new(&mut writer); - encoder - .map(CandidateProfile::FIELD_COUNT as u64 + 1) - .unwrap(); - encoder - .u16(FORMAT_KEY) - .unwrap() - .u16(FORMAT_VERSION) - .unwrap(); - encoder.u16(SCHEMA_KEY).unwrap().bytes(&SCHEMA_ID).unwrap(); - let candidate = CandidateProfile::factory(); - for descriptor in CandidateProfile::descriptors().skip(1) { - encoder.u16(descriptor.id.get()).unwrap(); - encode_value(&mut encoder, candidate.field_value(descriptor.id).unwrap()).unwrap(); - } - let length = writer.position(); - let missing = writer.into_inner(); - assert!(matches!( - decode_candidate(&missing[..length], &capabilities), - Err(CborError::Malformed) - )); - } - - #[test] - fn cbor_scalar_write_preserves_the_generated_field_and_rule() { - let capabilities = VehicleCapabilities::quad_x(); - let mass = CandidateProfile::descriptors() - .find(|descriptor| descriptor.display_key == "thrust.mass_kg") - .expect("declared mass descriptor"); - let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]); - let mut encoder = Encoder::new(&mut writer); - encoder - .map(CandidateProfile::FIELD_COUNT as u64 + 2) - .unwrap(); - encoder - .u16(FORMAT_KEY) - .unwrap() - .u16(FORMAT_VERSION) - .unwrap(); - encoder.u16(SCHEMA_KEY).unwrap().bytes(&SCHEMA_ID).unwrap(); - let candidate = CandidateProfile::factory(); - for descriptor in CandidateProfile::descriptors() { - encoder.u16(descriptor.id.get()).unwrap(); - let value = if descriptor.id == mass.id { - ConfigValue::F32(0.009) - } else { - candidate.field_value(descriptor.id).unwrap() - }; - encode_value(&mut encoder, value).unwrap(); - } - let length = writer.position(); - let bytes = writer.into_inner(); - - assert!(matches!( - decode_candidate(&bytes[..length], &capabilities), - Err(CborError::InvalidProfile(issue)) - if issue.primary() == Some(mass.id) - && issue.related_field().is_none() - && issue.rule() == ValidationRule::BelowMinimum - )); - } - - fn encoded_test_record( - format_version: u16, - schema_id: [u8; 16], - mut remap_key: F, - ) -> ([u8; MAXIMUM_CANDIDATE_BYTES], usize) - where - F: FnMut(u16) -> u16, - { - let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]); - let mut encoder = Encoder::new(&mut writer); - encoder - .map(CandidateProfile::FIELD_COUNT as u64 + 2) - .unwrap(); - encoder - .u16(FORMAT_KEY) - .unwrap() - .u16(format_version) - .unwrap(); - encoder.u16(SCHEMA_KEY).unwrap().bytes(&schema_id).unwrap(); - let candidate = CandidateProfile::factory(); - for descriptor in CandidateProfile::descriptors() { - encoder.u16(remap_key(descriptor.id.get())).unwrap(); - encode_value(&mut encoder, candidate.field_value(descriptor.id).unwrap()).unwrap(); - } - let length = writer.position(); - (writer.into_inner(), length) - } -} diff --git a/firmware/libs/kaptan-flight-config/src/lib.rs b/firmware/libs/kaptan-flight-config/src/lib.rs index 7e9f687..190462b 100644 --- a/firmware/libs/kaptan-flight-config/src/lib.rs +++ b/firmware/libs/kaptan-flight-config/src/lib.rs @@ -1,17 +1,19 @@ -//! Vehicle configuration schema and CBOR snapshot codec. +//! Validated configuration for the portable flight runtime. #![no_std] -pub mod cbor; pub mod profile; -pub use cbor::{ - CborEncodeError, CborError, MAXIMUM_CANDIDATE_BYTES, decode_candidate, encode_candidate, -}; -pub use profile::{CandidateProfile, FORMAT_VERSION, VehicleCapabilities, VehicleProfile}; +pub use profile::{CandidateProfile, VehicleCapabilities, VehicleProfile}; /// Generated fingerprint of the complete vehicle configuration schema. pub const SCHEMA_ID: [u8; 16] = ::SCHEMA_ID; #[cfg(test)] extern crate std; + +#[cfg(test)] +extern crate self as kaptan_flight_config; +#[cfg(test)] +#[path = "../tests/support/mod.rs"] +mod test_support; diff --git a/firmware/libs/kaptan-flight-config/src/profile/active.rs b/firmware/libs/kaptan-flight-config/src/profile/active.rs index 45c7d2d..725c169 100644 --- a/firmware/libs/kaptan-flight-config/src/profile/active.rs +++ b/firmware/libs/kaptan-flight-config/src/profile/active.rs @@ -1,30 +1,28 @@ //! Immutable configuration projections consumed by runtime owners. -/// Immutable compiled contract for the physical vehicle/board image. -/// -/// A runtime candidate can tune only values inside this contract. It cannot -/// create pins, buses, sensor drivers, or actuator channels. +use kaptan_config::{Config, ConfigValue, ConfigViolation, FieldId, ValidationRule}; +use kaptan_navigation::{ + ContactGeometry, NavigationConfig, NavigationInstallation, NoiseDensities, +}; + +use super::{ + ActuatorLimits, CandidateProfile, ControlSettings, PidSettings, SafetySettings, ThrustSettings, + TimingSettings, +}; + +/// Compiled vehicle capabilities. #[derive(Clone, Copy, Debug, PartialEq)] pub struct VehicleCapabilities { - /// Number of board-declared logical actuator channels available to the - /// selected vehicle model. - actuator_count: u8, - /// Reviewed physical allocation for this board/airframe image. - /// - /// This is capability data rather than a persisted setting: profile edits - /// must never alter motor order, thrust signs, or control authority. + /// Fixed allocation rows. Profile edits cannot change motor order or thrust signs. mixer: [[f32; 4]; 4], + installation: NavigationInstallation, } impl VehicleCapabilities { - /// Returns the reviewed four-actuator Quad-X allocation supported by P1. - /// - /// New allocation layouts require a dedicated board-bound airframe review - /// and constructor, rather than accepting an arbitrary coefficient array - /// through a configuration or capability initializer. - pub const fn quad_x() -> Self { + /// Four-actuator Quad-X allocation with an explicit navigation installation. + pub const fn quad_x(installation: NavigationInstallation) -> Self { Self { - actuator_count: 4, + installation, mixer: [ [-1.0, 1.0, 1.0, 1.0], [-1.0, -1.0, -1.0, 1.0], @@ -33,205 +31,91 @@ impl VehicleCapabilities { ], } } - - pub(crate) const fn actuator_count(self) -> u8 { - self.actuator_count - } - - pub(crate) const fn mixer(self) -> [[f32; 4]; 4] { - self.mixer - } -} - -/// Active prearm timing policy. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct SafetySettings { - pub(crate) prearm_dwell_microseconds: u64, -} - -impl SafetySettings { - pub const fn prearm_dwell_microseconds(self) -> u64 { - self.prearm_dwell_microseconds - } -} - -/// Active stream timing policy. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct TimingSettings { - pub(crate) maximum_imu_interval_microseconds: u64, - pub(crate) imu_freshness_microseconds: u64, - pub(crate) local_state_freshness_microseconds: u64, - pub(crate) command_link_freshness_microseconds: u64, -} - -impl TimingSettings { - pub const fn maximum_imu_interval_microseconds(self) -> u64 { - self.maximum_imu_interval_microseconds - } - pub const fn imu_freshness_microseconds(self) -> u64 { - self.imu_freshness_microseconds - } - pub const fn local_state_freshness_microseconds(self) -> u64 { - self.local_state_freshness_microseconds - } - pub const fn command_link_freshness_microseconds(self) -> u64 { - self.command_link_freshness_microseconds - } -} - -/// Active local-aiding uncertainty policy. -#[derive(Clone, Copy, Debug, PartialEq)] -pub struct AidingSettings { - pub(crate) position_variance_m2: f32, - pub(crate) velocity_variance_m2_per_second_squared: f32, -} - -impl AidingSettings { - pub const fn position_variance_m2(self) -> f32 { - self.position_variance_m2 - } - pub const fn velocity_variance_m2_per_second_squared(self) -> f32 { - self.velocity_variance_m2_per_second_squared - } -} - -/// Active outer-loop controller policy. -#[derive(Clone, Copy, Debug, PartialEq)] -pub struct ControlSettings { - pub(crate) position_gain_per_second_squared: f32, - pub(crate) velocity_gain_per_second: f32, - pub(crate) maximum_horizontal_acceleration_mps2: f32, - pub(crate) maximum_upward_acceleration_mps2: f32, - pub(crate) maximum_downward_acceleration_mps2: f32, -} - -impl ControlSettings { - pub const fn position_gain_per_second_squared(self) -> f32 { - self.position_gain_per_second_squared - } - pub const fn velocity_gain_per_second(self) -> f32 { - self.velocity_gain_per_second - } - pub const fn maximum_horizontal_acceleration_mps2(self) -> f32 { - self.maximum_horizontal_acceleration_mps2 - } - pub const fn maximum_upward_acceleration_mps2(self) -> f32 { - self.maximum_upward_acceleration_mps2 - } - pub const fn maximum_downward_acceleration_mps2(self) -> f32 { - self.maximum_downward_acceleration_mps2 - } -} - -/// Active physical thrust envelope. -#[derive(Clone, Copy, Debug, PartialEq)] -pub struct ThrustSettings { - pub(crate) mass_kg: f32, - pub(crate) maximum_collective_thrust_newtons: f32, - pub(crate) maximum_tilt_tangent: f32, -} - -impl ThrustSettings { - pub const fn mass_kg(self) -> f32 { - self.mass_kg - } - pub const fn maximum_collective_thrust_newtons(self) -> f32 { - self.maximum_collective_thrust_newtons - } - pub const fn maximum_tilt_tangent(self) -> f32 { - self.maximum_tilt_tangent - } -} - -/// Active scalar PID policy. -#[derive(Clone, Copy, Debug, PartialEq)] -pub struct PidSettings { - pub(crate) proportional: f32, - pub(crate) integral: f32, - pub(crate) derivative: f32, - pub(crate) output_limit: f32, - pub(crate) integral_limit: f32, -} - -impl PidSettings { - pub const fn proportional(self) -> f32 { - self.proportional - } - pub const fn integral(self) -> f32 { - self.integral - } - pub const fn derivative(self) -> f32 { - self.derivative - } - pub const fn output_limit(self) -> f32 { - self.output_limit - } - pub const fn integral_limit(self) -> f32 { - self.integral_limit - } -} - -/// Active actuator command bounds. -#[derive(Clone, Copy, Debug, PartialEq)] -pub struct ActuatorLimits { - pub(crate) minimum: f32, - pub(crate) maximum: f32, - pub(crate) slew_per_second: f32, -} - -impl ActuatorLimits { - pub const fn minimum(self) -> f32 { - self.minimum - } - pub const fn maximum(self) -> f32 { - self.maximum - } - pub const fn slew_per_second(self) -> f32 { - self.slew_per_second - } } /// Immutable complete configuration selected at boot. +/// +/// An editable candidate must pass validation before it can become active. +/// ```compile_fail,E0308 +/// use kaptan_flight_config::{CandidateProfile, VehicleProfile}; +/// fn activate(candidate: CandidateProfile) -> VehicleProfile { candidate } +/// ``` +/// Callers cannot replace the validated candidate inside an active profile. +/// ```compile_fail,E0616 +/// use kaptan_flight_config::{CandidateProfile, VehicleProfile}; +/// fn replace(mut active: VehicleProfile, candidate: CandidateProfile) { +/// active.candidate = candidate; +/// } +/// ``` #[derive(Clone, Copy, Debug, PartialEq)] pub struct VehicleProfile { - pub(crate) safety: SafetySettings, - pub(crate) timing: TimingSettings, - pub(crate) aiding: AidingSettings, - pub(crate) control: ControlSettings, - pub(crate) thrust: ThrustSettings, - pub(crate) rate_pid: PidSettings, - pub(crate) actuator_limits: ActuatorLimits, - pub(crate) mavlink_logging: kaptan_log::LevelFilter, - pub(crate) mixer: [[f32; 4]; 4], + candidate: CandidateProfile, + capabilities: VehicleCapabilities, } impl VehicleProfile { + pub(super) fn validate( + candidate: &CandidateProfile, + capabilities: VehicleCapabilities, + ) -> Result { + ::validate(candidate)?; + let profile = Self { + candidate: *candidate, + capabilities, + }; + if !profile.navigation().validate() { + return Err(ConfigViolation::global(ValidationRule::Inconsistent)); + } + Ok(profile) + } + + pub(super) fn field_value(&self, id: FieldId) -> Option { + ::field_value(&self.candidate, id) + } + pub const fn safety(self) -> SafetySettings { - self.safety + self.candidate.safety } pub const fn timing(self) -> TimingSettings { - self.timing - } - pub const fn aiding(self) -> AidingSettings { - self.aiding + self.candidate.timing } pub const fn control(self) -> ControlSettings { - self.control + self.candidate.control } pub const fn thrust(self) -> ThrustSettings { - self.thrust + self.candidate.thrust } pub const fn rate_pid(self) -> PidSettings { - self.rate_pid + self.candidate.rate_pid } pub const fn actuator_limits(self) -> ActuatorLimits { - self.actuator_limits - } - /// Immutable boot-active policy for the MAVLink diagnostic sink. - pub const fn mavlink_logging(self) -> kaptan_log::LevelFilter { - self.mavlink_logging + self.candidate.actuator_limits } pub const fn mixer(self) -> [[f32; 4]; 4] { - self.mixer + self.capabilities.mixer + } + pub const fn flight(self) -> super::FlightSettings { + self.candidate.flight + } + pub const fn contact_geometry(self) -> ContactGeometry { + self.capabilities.installation.contact + } + pub fn navigation(self) -> NavigationConfig { + let f = self.candidate.flight; + NavigationConfig { + noise: NoiseDensities { + gyro: f.gyro_psd, + acceleration: f.acceleration_psd, + gyro_bias: f.gyro_bias_psd, + acceleration_bias: f.acceleration_bias_psd, + barometric_datum: f.barometric_datum_psd, + }, + maximum_time_uncertainty_microseconds: f.time_error_bound_us as u32, + maximum_imu_interval_microseconds: self.timing().maximum_imu_interval_microseconds, + imu_freshness_microseconds: self.timing().imu_freshness_microseconds, + aiding_freshness_microseconds: self.timing().local_state_freshness_microseconds, + lag_microseconds: 100_000, + installation: self.capabilities.installation, + } } } diff --git a/firmware/libs/kaptan-flight-config/src/profile/candidate.rs b/firmware/libs/kaptan-flight-config/src/profile/candidate.rs index f6ed456..9939a2a 100644 --- a/firmware/libs/kaptan-flight-config/src/profile/candidate.rs +++ b/firmware/libs/kaptan-flight-config/src/profile/candidate.rs @@ -1,93 +1,55 @@ -//! Private vehicle candidate and whole-profile validation boundary. +//! Editable vehicle candidate and whole-profile validation boundary. use kaptan_config::{ Config, ConfigModel, ConfigValue, ConfigViolation, FieldDescriptor, FieldId, FieldWriteError, - ValidationRule, }; -use kaptan_log::LevelFilter; use super::{ - ActuatorLimits, AidingSettings, ControlSettings, PidSettings, SafetySettings, ThrustSettings, - TimingSettings, VehicleCapabilities, VehicleProfile, - control::{ - ActuatorLimitsCandidate, AidingCandidate, ControlCandidate, PidCandidate, ThrustCandidate, - }, - mavlink_logging::MavlinkLoggingCandidate, - safety::SafetyCandidate, - timing::TimingCandidate, + ActuatorLimits, ControlSettings, PidSettings, SafetySettings, ThrustSettings, TimingSettings, + VehicleCapabilities, VehicleProfile, }; -/// Current persisted CBOR schema for the vehicle candidate. -pub const FORMAT_VERSION: u16 = 3; - /// Complete editable vehicle candidate. /// -/// Fields are private outside this crate. Local snapshot recovery and MAVLink -/// scalar writes both use the same descriptor/mutation and full-validation -/// path. -#[derive(Clone, Copy, Config)] +/// Descriptor writes require complete validation before activation. +#[derive(Clone, Copy, Debug, PartialEq, Config)] +#[config(post = Self::validate_flight)] pub struct CandidateProfile { - pub(super) safety: SafetyCandidate, - pub(super) timing: TimingCandidate, - pub(super) aiding: AidingCandidate, - pub(super) control: ControlCandidate, - pub(super) thrust: ThrustCandidate, - pub(super) rate_pid: PidCandidate, - pub(super) actuator_limits: ActuatorLimitsCandidate, - pub(super) mavlink_logging: MavlinkLoggingCandidate, + pub safety: SafetySettings, + pub timing: TimingSettings, + pub control: ControlSettings, + pub thrust: ThrustSettings, + pub rate_pid: PidSettings, + pub actuator_limits: ActuatorLimits, + pub flight: super::FlightSettings, } impl CandidateProfile { - /// Explicit factory candidate used by the vehicle-profile examples. - /// - /// Board firmware supplies its own factory [`Config`] through the board - /// composition root; configuration types never synthesize values through - /// [`Default`]. - pub const fn factory() -> Self { - Self { - safety: SafetyCandidate { - prearm_dwell_microseconds: 100_000, - }, - timing: TimingCandidate { - maximum_imu_interval_microseconds: 50_000, - imu_freshness_microseconds: 50_000, - local_state_freshness_microseconds: 150_000, - command_link_freshness_microseconds: 500_000, - }, - aiding: AidingCandidate { - position_variance_m2: 0.04, - velocity_variance_m2_per_second_squared: 0.04, - }, - control: ControlCandidate { - position_gain_per_second_squared: 0.8, - velocity_gain_per_second: 1.2, - maximum_horizontal_acceleration_mps2: 3.0, - maximum_upward_acceleration_mps2: 2.0, - maximum_downward_acceleration_mps2: 2.0, - }, - thrust: ThrustCandidate { - mass_kg: 1.2, - maximum_collective_thrust_newtons: 18.0, - maximum_tilt_tangent: 0.700_207_53, - }, - rate_pid: PidCandidate { - proportional: 0.035, - integral: 0.006, - derivative: 0.0, - output_limit: 0.10, - integral_limit: 0.025, - }, - actuator_limits: ActuatorLimitsCandidate { - minimum: 0.0, - maximum: 1.0, - slew_per_second: 8.0, - }, - mavlink_logging: MavlinkLoggingCandidate { - level_filter: LevelFilter::Info, - }, + fn validate_flight(&self) -> Result<(), ConfigViolation> { + use super::ids; + use kaptan_config::ValidationRule; + let field = if !(10_000..=20_000).contains(&self.timing.maximum_imu_interval_microseconds) { + Some(ids::MAXIMUM_IMU_INTERVAL_MICROSECONDS) + } else if self.timing.imu_freshness_microseconds != 20_000 { + Some(ids::IMU_FRESHNESS_MICROSECONDS) + } else if !(200_000..=500_000).contains(&self.timing.local_state_freshness_microseconds) { + Some(ids::LOCAL_STATE_FRESHNESS_MICROSECONDS) + } else if self.timing.offboard_timeout_microseconds != 300_000 { + Some(ids::OFFBOARD_TIMEOUT_MICROSECONDS) + } else if self.actuator_limits.minimum != 0. { + Some(ids::ACTUATOR_MINIMUM) } + // The 0.5-s landing cap ramp can descend by at most 2 command/s. + else if self.actuator_limits.slew_per_second < 2. { + Some(ids::ACTUATOR_SLEW_PER_SECOND) + } else { + None + }; + if let Some(id) = field { + return Err(ConfigViolation::field(id, ValidationRule::Inconsistent)); + } + Ok(()) } - /// Returns the complete transport-neutral descriptor registry. pub fn descriptors() -> impl ExactSizeIterator + Clone { (0..Self::FIELD_COUNT).map(|index| { @@ -95,130 +57,11 @@ impl CandidateProfile { .expect("derived aggregate descriptor count must be exact") }) } - - fn from_active(active: &VehicleProfile) -> Self { - let safety = active.safety(); - let timing = active.timing(); - let aiding = active.aiding(); - let control = active.control(); - let thrust = active.thrust(); - let rate_pid = active.rate_pid(); - let actuator_limits = active.actuator_limits(); - let mavlink_logging = active.mavlink_logging(); - Self { - safety: SafetyCandidate { - prearm_dwell_microseconds: safety.prearm_dwell_microseconds(), - }, - timing: TimingCandidate { - maximum_imu_interval_microseconds: timing.maximum_imu_interval_microseconds(), - imu_freshness_microseconds: timing.imu_freshness_microseconds(), - local_state_freshness_microseconds: timing.local_state_freshness_microseconds(), - command_link_freshness_microseconds: timing.command_link_freshness_microseconds(), - }, - aiding: AidingCandidate { - position_variance_m2: aiding.position_variance_m2(), - velocity_variance_m2_per_second_squared: aiding - .velocity_variance_m2_per_second_squared(), - }, - control: ControlCandidate { - position_gain_per_second_squared: control.position_gain_per_second_squared(), - velocity_gain_per_second: control.velocity_gain_per_second(), - maximum_horizontal_acceleration_mps2: control - .maximum_horizontal_acceleration_mps2(), - maximum_upward_acceleration_mps2: control.maximum_upward_acceleration_mps2(), - maximum_downward_acceleration_mps2: control.maximum_downward_acceleration_mps2(), - }, - thrust: ThrustCandidate { - mass_kg: thrust.mass_kg(), - maximum_collective_thrust_newtons: thrust.maximum_collective_thrust_newtons(), - maximum_tilt_tangent: thrust.maximum_tilt_tangent(), - }, - rate_pid: PidCandidate { - proportional: rate_pid.proportional(), - integral: rate_pid.integral(), - derivative: rate_pid.derivative(), - output_limit: rate_pid.output_limit(), - integral_limit: rate_pid.integral_limit(), - }, - actuator_limits: ActuatorLimitsCandidate { - minimum: actuator_limits.minimum(), - maximum: actuator_limits.maximum(), - slew_per_second: actuator_limits.slew_per_second(), - }, - mavlink_logging: MavlinkLoggingCandidate { - level_filter: mavlink_logging, - }, - } - } - - fn validate_complete( - &self, - capabilities: &VehicleCapabilities, - ) -> Result { - ::validate(self)?; - self.validate_capabilities(capabilities)?; - Ok(VehicleProfile { - safety: SafetySettings { - prearm_dwell_microseconds: self.safety.prearm_dwell_microseconds, - }, - timing: TimingSettings { - maximum_imu_interval_microseconds: self.timing.maximum_imu_interval_microseconds, - imu_freshness_microseconds: self.timing.imu_freshness_microseconds, - local_state_freshness_microseconds: self.timing.local_state_freshness_microseconds, - command_link_freshness_microseconds: self - .timing - .command_link_freshness_microseconds, - }, - aiding: AidingSettings { - position_variance_m2: self.aiding.position_variance_m2, - velocity_variance_m2_per_second_squared: self - .aiding - .velocity_variance_m2_per_second_squared, - }, - control: ControlSettings { - position_gain_per_second_squared: self.control.position_gain_per_second_squared, - velocity_gain_per_second: self.control.velocity_gain_per_second, - maximum_horizontal_acceleration_mps2: self - .control - .maximum_horizontal_acceleration_mps2, - maximum_upward_acceleration_mps2: self.control.maximum_upward_acceleration_mps2, - maximum_downward_acceleration_mps2: self.control.maximum_downward_acceleration_mps2, - }, - thrust: ThrustSettings { - mass_kg: self.thrust.mass_kg, - maximum_collective_thrust_newtons: self.thrust.maximum_collective_thrust_newtons, - maximum_tilt_tangent: self.thrust.maximum_tilt_tangent, - }, - rate_pid: PidSettings { - proportional: self.rate_pid.proportional, - integral: self.rate_pid.integral, - derivative: self.rate_pid.derivative, - output_limit: self.rate_pid.output_limit, - integral_limit: self.rate_pid.integral_limit, - }, - actuator_limits: ActuatorLimits { - minimum: self.actuator_limits.minimum, - maximum: self.actuator_limits.maximum, - slew_per_second: self.actuator_limits.slew_per_second, - }, - mavlink_logging: self.mavlink_logging.level_filter, - mixer: capabilities.mixer(), - }) - } - - fn validate_capabilities( - &self, - capabilities: &VehicleCapabilities, - ) -> Result<(), ConfigViolation> { - if capabilities.actuator_count() != 4 { - return Err(ConfigViolation::global( - ValidationRule::UnsupportedCapability, - )); - } - Ok(()) - } } +// ConfigModel exposes the complete registry count as u16 on the wire. +const _: () = assert!(CandidateProfile::FIELD_COUNT <= u16::MAX as usize); + impl ConfigModel for CandidateProfile { type Active = VehicleProfile; type Context = VehicleCapabilities; @@ -237,7 +80,7 @@ impl ConfigModel for CandidateProfile { } fn active_field_value(active: &Self::Active, id: FieldId) -> Option { - ::field_value(&Self::from_active(active), id) + active.field_value(id) } fn set_field(&mut self, id: FieldId, value: ConfigValue) -> Result<(), FieldWriteError> { @@ -245,7 +88,7 @@ impl ConfigModel for CandidateProfile { } fn validate(&self, context: &Self::Context) -> Result { - self.validate_complete(context) + VehicleProfile::validate(self, *context) } } @@ -253,18 +96,43 @@ impl ConfigModel for CandidateProfile { mod tests { use kaptan_config::{ConfigModel, ConfigValue, FieldId, FieldWriteError, ValidationRule}; - use super::{CandidateProfile, LevelFilter, VehicleCapabilities}; + use super::{CandidateProfile, VehicleCapabilities}; use crate::profile::ids; + use crate::test_support::{candidate as fixture_candidate, capabilities}; - const CAPABILITIES: VehicleCapabilities = VehicleCapabilities::quad_x(); + const CAPABILITIES: VehicleCapabilities = capabilities(); + + #[test] + fn installation_activation() { + let mut installation = crate::test_support::installation(); + installation.calibration = kaptan_navigation::CalibrationRevision::new(29); + installation.reference = kaptan_navigation::SensorReferenceId(42); + installation.atmosphere.pressure_pascals = 80_000.0; + installation.contact.half_span_x_metres = 0.3; + let mut candidate = fixture_candidate(); + let active = candidate + .validate(&VehicleCapabilities::quad_x(installation)) + .unwrap(); + assert_eq!(active.navigation().installation, installation); + assert_eq!(active.contact_geometry(), installation.contact); + candidate.thrust.mass_kg = f32::NAN; + assert!(candidate.validate(&CAPABILITIES).is_err()); + assert!(active.thrust().mass_kg.is_finite()); + installation.range.direction_frd = kaptan_math::Vector3::zero(); + assert_eq!( + fixture_candidate().validate(&VehicleCapabilities::quad_x(installation)), + Err(kaptan_config::ConfigViolation::global( + ValidationRule::Inconsistent + )) + ); + } #[test] fn generated_schema_fingerprint_is_stable() { assert_eq!( crate::SCHEMA_ID, [ - 0xb9, 0x8b, 0xf9, 0xa0, 0x62, 0x3f, 0x6e, 0xef, 0x4e, 0x88, 0xc6, 0xd8, 0x89, 0x4c, - 0x76, 0x8b, + 172, 176, 17, 7, 200, 246, 190, 201, 231, 26, 101, 3, 145, 64, 206, 150, ] ); } @@ -278,12 +146,72 @@ mod tests { #[test] fn every_descriptor_has_a_readable_factory_value() { - let candidate = CandidateProfile::factory(); + let candidate = fixture_candidate(); for descriptor in CandidateProfile::descriptors() { assert!(candidate.field_value(descriptor.id).is_some()); } } + #[test] + fn active_field_reads_preserve_validated_bits_after_candidate_edits() { + let factory = fixture_candidate(); + let mut candidate = factory; + for descriptor in CandidateProfile::descriptors() { + let value = match factory.field_value(descriptor.id).unwrap() { + ConfigValue::U64(value) => ConfigValue::U64(match descriptor.id.get() { + 2 => 19_000, + // Fixed engineering cadence and stream policy are still + // checked byte-for-byte in the immutable active snapshot. + 3 | 5 | 33 | 34 => value, + _ => value - u64::from(descriptor.id.get()), + }), + ConfigValue::F32(0.0) => ConfigValue::F32(-0.0), + ConfigValue::F32(value) => ConfigValue::F32(value * 0.875), + value => panic!("unexpected profile scalar: {value:?}"), + }; + candidate.set_field(descriptor.id, value).unwrap(); + } + let validated = candidate; + let active = candidate.validate(&CAPABILITIES).unwrap(); + for edit_candidate in [false, true] { + for descriptor in CandidateProfile::descriptors() { + if edit_candidate { + candidate + .set_field(descriptor.id, factory.field_value(descriptor.id).unwrap()) + .unwrap(); + } + let expected = validated.field_value(descriptor.id).unwrap(); + let actual = CandidateProfile::active_field_value(&active, descriptor.id).unwrap(); + match (actual, expected) { + (ConfigValue::F32(actual), ConfigValue::F32(expected)) => { + assert_eq!(actual.to_bits(), expected.to_bits(), "{descriptor:?}"); + } + _ => assert_eq!(actual, expected, "{descriptor:?}"), + } + } + } + assert_eq!(candidate, factory); + assert_eq!( + CandidateProfile::active_field_value(&active, FieldId::new(24)), + None + ); + } + + #[test] + fn editing_a_section_copy_cannot_mutate_the_active_profile() { + let active = fixture_candidate().validate(&CAPABILITIES).unwrap(); + let mut thrust = active.thrust(); + kaptan_config::Config::set_field(&mut thrust, ids::MASS_KG, ConfigValue::F32(100.0)) + .unwrap(); + assert_eq!(thrust.mass_kg, 100.0); + assert!(kaptan_config::Config::validate(&thrust).is_err()); + assert_eq!(active.thrust().mass_kg.to_bits(), 1.2_f32.to_bits()); + assert_eq!( + CandidateProfile::active_field_value(&active, ids::MASS_KG), + Some(ConfigValue::F32(1.2)) + ); + } + #[test] fn aggregated_descriptor_ids_are_unique() { for (index, descriptor) in CandidateProfile::descriptors().enumerate() { @@ -293,11 +221,19 @@ mod tests { .all(|other| other.id != descriptor.id) ); } + let mut candidate = fixture_candidate(); + for retired in [FieldId::new(6), FieldId::new(7), FieldId::new(25)] { + assert_eq!(candidate.field_value(retired), None); + assert_eq!( + candidate.set_field(retired, ConfigValue::F32(0.01)), + Err(FieldWriteError::UnknownField(retired)) + ); + } } #[test] fn cross_field_invalid_edit_is_retained_only_in_the_candidate() { - let mut candidate = CandidateProfile::factory(); + let mut candidate = fixture_candidate(); candidate .set_field( ids::MAXIMUM_IMU_INTERVAL_MICROSECONDS, @@ -312,13 +248,42 @@ mod tests { #[test] fn local_write_error_retains_exact_field_and_rule() { - let mut candidate = CandidateProfile::factory(); + let mut candidate = fixture_candidate(); assert_eq!( candidate.set_field(ids::MASS_KG, ConfigValue::F32(f32::NAN)), Err(FieldWriteError::non_finite(ids::MASS_KG)) ); } + #[test] + fn pid_gain_domain_includes_derivative_and_preserves_rejected_writes() { + for key in [ + "control.rate_pid.proportional", + "control.rate_pid.integral", + "control.rate_pid.derivative", + "control.yaw_rate_pid.proportional", + "control.yaw_rate_pid.integral", + "control.attitude_roll_pitch_gain_per_s", + "control.attitude_yaw_gain_per_s", + ] { + let id = descriptor_id(key); + for gain in [0.0, f32::MIN_POSITIVE, 0.001, 0.5, 1.0, 5.0, 10.0] { + let mut candidate = fixture_candidate(); + candidate.set_field(id, ConfigValue::F32(gain)).unwrap(); + let active = candidate.validate(&CAPABILITIES).unwrap(); + assert_eq!( + CandidateProfile::active_field_value(&active, id), + Some(ConfigValue::F32(gain)) + ); + for invalid in [-1.0, f32::NAN, f32::INFINITY, 10.01] { + let before = candidate; + assert!(candidate.set_field(id, ConfigValue::F32(invalid)).is_err()); + assert_eq!(candidate, before); + } + } + } + } + #[test] fn descriptor_bounds_do_not_conflict_with_derived_field_validation() { for descriptor in CandidateProfile::descriptors() { @@ -326,7 +291,7 @@ mod tests { .into_iter() .flatten() { - let mut candidate = CandidateProfile::factory(); + let mut candidate = fixture_candidate(); candidate .set_field(descriptor.id, bound) .expect("descriptor bound must pass the descriptor-backed setter"); @@ -341,62 +306,4 @@ mod tests { } } } - - #[test] - fn mavlink_logging_policy_is_persisted_and_restart_only() { - let mut candidate = CandidateProfile::factory(); - let level_descriptor = CandidateProfile::descriptors() - .find(|descriptor| descriptor.display_key == "mavlink.logging.level_filter") - .unwrap(); - assert_eq!( - level_descriptor.enum_options, - LevelFilter::CONFIG_ENUM_OPTIONS - ); - candidate - .set_field( - descriptor_id("mavlink.logging.level_filter"), - ConfigValue::U8(LevelFilter::Off.wire_value()), - ) - .unwrap(); - let active = candidate.validate(&CAPABILITIES).unwrap(); - assert_eq!(active.mavlink_logging(), LevelFilter::Off); - candidate - .set_field( - descriptor_id("mavlink.logging.level_filter"), - ConfigValue::U8(LevelFilter::Trace.wire_value()), - ) - .unwrap(); - assert_eq!( - candidate.validate(&CAPABILITIES).unwrap().mavlink_logging(), - LevelFilter::Trace - ); - assert!(matches!( - candidate.set_field( - descriptor_id("mavlink.logging.level_filter"), - ConfigValue::U8(6), - ), - Err(FieldWriteError::Violation(issue)) - if issue.rule() == ValidationRule::InvalidEnumValue - )); - } - - #[test] - fn every_persisted_log_filter_is_retained_exactly() { - for raw in 0..=5 { - let mut candidate = CandidateProfile::factory(); - candidate - .set_field( - descriptor_id("mavlink.logging.level_filter"), - ConfigValue::U8(raw), - ) - .expect("descriptor accepts the stable level filter"); - assert_eq!( - candidate - .validate(&CAPABILITIES) - .expect("valid complete profile") - .mavlink_logging(), - LevelFilter::try_from_wire(raw).unwrap() - ); - } - } } diff --git a/firmware/libs/kaptan-flight-config/src/profile/control.rs b/firmware/libs/kaptan-flight-config/src/profile/control.rs index de201aa..857c6d7 100644 --- a/firmware/libs/kaptan-flight-config/src/profile/control.rs +++ b/firmware/libs/kaptan-flight-config/src/profile/control.rs @@ -2,28 +2,10 @@ use kaptan_config::{Config, ConfigViolation, ValidationRule}; use super::ids; -#[derive(Clone, Copy, Config)] -pub(super) struct AidingCandidate { - #[config( - id = 6, - key = "aiding.position_variance_m2", - unit = "m2", - finite, - range(min = 0.000_001, max = 10_000.0) - )] - pub(super) position_variance_m2: f32, - #[config( - id = 7, - key = "aiding.velocity_variance_m2ps2", - unit = "m2/s2", - finite, - range(min = 0.000_001, max = 10_000.0) - )] - pub(super) velocity_variance_m2_per_second_squared: f32, -} - -#[derive(Clone, Copy, Config)] -pub(super) struct ControlCandidate { +/// Outer-loop controller policy. +#[derive(Clone, Copy, Debug, PartialEq, Config)] +#[config(post = Self::validate_control)] +pub struct ControlSettings { #[config( id = 8, key = "control.position_gain_per_s2", @@ -31,7 +13,7 @@ pub(super) struct ControlCandidate { finite, range(min = 0.0, max = 100.0) )] - pub(super) position_gain_per_second_squared: f32, + pub position_gain_per_second_squared: f32, #[config( id = 9, key = "control.velocity_gain_per_s", @@ -39,7 +21,7 @@ pub(super) struct ControlCandidate { finite, range(min = 0.0, max = 100.0) )] - pub(super) velocity_gain_per_second: f32, + pub velocity_gain_per_second: f32, #[config( id = 10, key = "control.maximum_horizontal_acceleration_mps2", @@ -47,7 +29,7 @@ pub(super) struct ControlCandidate { finite, range(min = 0.001, max = 100.0) )] - pub(super) maximum_horizontal_acceleration_mps2: f32, + pub maximum_horizontal_acceleration_mps2: f32, #[config( id = 11, key = "control.maximum_upward_acceleration_mps2", @@ -55,7 +37,7 @@ pub(super) struct ControlCandidate { finite, range(min = 0.001, max = 100.0) )] - pub(super) maximum_upward_acceleration_mps2: f32, + pub maximum_upward_acceleration_mps2: f32, #[config( id = 12, key = "control.maximum_downward_acceleration_mps2", @@ -63,11 +45,43 @@ pub(super) struct ControlCandidate { finite, range(min = 0.001, max = 100.0) )] - pub(super) maximum_downward_acceleration_mps2: f32, + pub maximum_downward_acceleration_mps2: f32, + #[config( + id = 41, + key = "control.attitude_roll_pitch_gain_per_s", + unit = "1/s", + finite, + range(min = 0.0, max = 10.0) + )] + pub attitude_roll_pitch_gain_per_second: f32, + #[config( + id = 42, + key = "control.attitude_yaw_gain_per_s", + unit = "1/s", + finite, + range(min = 0.0, max = 10.0) + )] + pub attitude_yaw_gain_per_second: f32, } -#[derive(Clone, Copy, Config)] + +impl ControlSettings { + fn validate_control(&self) -> Result<(), ConfigViolation> { + // Positive multicopter thrust requires downward acceleration strictly + // below gravity. An inclusive scalar maximum cannot express this rule. + if self.maximum_downward_acceleration_mps2 >= kaptan_navigation::GRAVITY_MPS2 { + return Err(ConfigViolation::field( + ids::MAXIMUM_DOWNWARD_ACCELERATION_MPS2, + ValidationRule::Inconsistent, + )); + } + Ok(()) + } +} + +/// Physical thrust envelope. +#[derive(Clone, Copy, Debug, PartialEq, Config)] #[config(post = Self::validate_thrust)] -pub(super) struct ThrustCandidate { +pub struct ThrustSettings { #[config( id = 13, key = "thrust.mass_kg", @@ -75,7 +89,7 @@ pub(super) struct ThrustCandidate { finite, range(min = 0.01, max = 100.0) )] - pub(super) mass_kg: f32, + pub mass_kg: f32, #[config( id = 14, key = "thrust.maximum_collective_thrust_n", @@ -83,7 +97,7 @@ pub(super) struct ThrustCandidate { finite, range(min = 0.1, max = 10_000.0) )] - pub(super) maximum_collective_thrust_newtons: f32, + pub maximum_collective_thrust_newtons: f32, #[config( id = 15, key = "thrust.maximum_tilt_tangent", @@ -91,11 +105,16 @@ pub(super) struct ThrustCandidate { finite, range(min = 0.001, max = 10.0) )] - pub(super) maximum_tilt_tangent: f32, + pub maximum_tilt_tangent: f32, } -impl ThrustCandidate { +impl ThrustSettings { fn validate_thrust(&self) -> Result<(), ConfigViolation> { - if self.maximum_collective_thrust_newtons <= self.mass_kg * 9.806_65 { + if self.maximum_collective_thrust_newtons <= self.mass_kg * kaptan_navigation::GRAVITY_MPS2 + || f64::from(self.maximum_collective_thrust_newtons) + > f64::from(self.mass_kg) + * (f64::from(kaptan_navigation::MAXIMUM_FREE_ACCELERATION_MPS2) + - f64::from(kaptan_navigation::GRAVITY_MPS2)) + { return Err(ConfigViolation::related( ids::MAXIMUM_COLLECTIVE_THRUST_NEWTONS, ids::MASS_KG, @@ -106,9 +125,10 @@ impl ThrustCandidate { } } -#[derive(Clone, Copy, Config)] +/// Roll/pitch and yaw P/I gains with shared derivative and contribution limits. +#[derive(Clone, Copy, Debug, PartialEq, Config)] #[config(post = Self::validate_pid)] -pub(super) struct PidCandidate { +pub struct PidSettings { #[config( id = 16, key = "control.rate_pid.proportional", @@ -116,7 +136,7 @@ pub(super) struct PidCandidate { finite, range(min = 0.0, max = 10.0) )] - pub(super) proportional: f32, + pub proportional: f32, #[config( id = 17, key = "control.rate_pid.integral", @@ -124,7 +144,7 @@ pub(super) struct PidCandidate { finite, range(min = 0.0, max = 10.0) )] - pub(super) integral: f32, + pub integral: f32, #[config( id = 18, key = "control.rate_pid.derivative", @@ -132,7 +152,7 @@ pub(super) struct PidCandidate { finite, range(min = 0.0, max = 10.0) )] - pub(super) derivative: f32, + pub derivative: f32, #[config( id = 19, key = "control.rate_pid.output_limit", @@ -140,7 +160,7 @@ pub(super) struct PidCandidate { finite, range(min = 0.001, max = 1.0) )] - pub(super) output_limit: f32, + pub output_limit: f32, #[config( id = 20, key = "control.rate_pid.integral_limit", @@ -148,9 +168,25 @@ pub(super) struct PidCandidate { finite, range(min = 0.0, max = 1.0) )] - pub(super) integral_limit: f32, + pub integral_limit: f32, + #[config( + id = 43, + key = "control.yaw_rate_pid.proportional", + unit = "", + finite, + range(min = 0.0, max = 10.0) + )] + pub yaw_proportional: f32, + #[config( + id = 44, + key = "control.yaw_rate_pid.integral", + unit = "", + finite, + range(min = 0.0, max = 10.0) + )] + pub yaw_integral: f32, } -impl PidCandidate { +impl PidSettings { fn validate_pid(&self) -> Result<(), ConfigViolation> { if self.integral_limit > self.output_limit { return Err(ConfigViolation::related( @@ -163,9 +199,10 @@ impl PidCandidate { } } -#[derive(Clone, Copy, Config)] +/// Actuator command bounds. +#[derive(Clone, Copy, Debug, PartialEq, Config)] #[config(post = Self::validate_actuator_limits)] -pub(super) struct ActuatorLimitsCandidate { +pub struct ActuatorLimits { #[config( id = 21, key = "actuator.minimum", @@ -173,7 +210,7 @@ pub(super) struct ActuatorLimitsCandidate { finite, range(min = 0.0, max = 1.0) )] - pub(super) minimum: f32, + pub minimum: f32, #[config( id = 22, key = "actuator.maximum", @@ -181,7 +218,7 @@ pub(super) struct ActuatorLimitsCandidate { finite, range(min = 0.0, max = 1.0) )] - pub(super) maximum: f32, + pub maximum: f32, #[config( id = 23, key = "actuator.slew_per_second", @@ -189,9 +226,9 @@ pub(super) struct ActuatorLimitsCandidate { finite, range(min = 0.001, max = 100.0) )] - pub(super) slew_per_second: f32, + pub slew_per_second: f32, } -impl ActuatorLimitsCandidate { +impl ActuatorLimits { fn validate_actuator_limits(&self) -> Result<(), ConfigViolation> { if self.minimum >= self.maximum { return Err(ConfigViolation::related( diff --git a/firmware/libs/kaptan-flight-config/src/profile/flight.rs b/firmware/libs/kaptan-flight-config/src/profile/flight.rs new file mode 100644 index 0000000..c4f0cea --- /dev/null +++ b/firmware/libs/kaptan-flight-config/src/profile/flight.rs @@ -0,0 +1,88 @@ +//! Restart-only flight policy and continuous process-noise densities. +use kaptan_config::Config; + +#[derive(Clone, Copy, Debug, PartialEq, Config)] +pub struct FlightSettings { + #[config( + id = 30, + key = "flight.command_ttl_us", + unit = "us", + range(min = 1, max = 250_000) + )] + pub command_ttl_us: u64, + #[config( + id = 31, + key = "flight.fallback_hold_us", + unit = "us", + range(min = 1, max = 1_000_000) + )] + pub fallback_hold_us: u64, + #[config( + id = 32, + key = "flight.output_lifetime_us", + unit = "us", + range(min = 10_000, max = 50_000) + )] + pub output_lifetime_us: u64, + #[config( + id = 33, + key = "flight.control_period_us", + unit = "us", + range(min = 10_000, max = 10_000) + )] + pub control_period_us: u64, + #[config( + id = 34, + key = "flight.control_deadline_us", + unit = "us", + range(min = 20_000, max = 20_000) + )] + pub control_deadline_us: u64, + #[config( + id = 35, + key = "navigation.time_error_bound_us", + unit = "us", + range(min = 0, max = 2_000) + )] + pub time_error_bound_us: u64, + #[config( + id = 36, + key = "navigation.gyro_psd", + unit = "rad2/s", + finite, + range(min = 1e-12, max = 0.01) + )] + pub gyro_psd: f32, + #[config( + id = 37, + key = "navigation.acceleration_psd", + unit = "m2/s3", + finite, + range(min = 1e-12, max = 0.1) + )] + pub acceleration_psd: f32, + #[config( + id = 38, + key = "navigation.gyro_bias_psd", + unit = "rad2/s3", + finite, + range(min = 1e-14, max = 0.01) + )] + pub gyro_bias_psd: f32, + #[config( + id = 39, + key = "navigation.acceleration_bias_psd", + unit = "m2/s5", + finite, + range(min = 1e-14, max = 0.1) + )] + pub acceleration_bias_psd: f32, + #[config( + id = 40, + key = "navigation.barometric_datum_psd", + unit = "m2/s", + finite, + range(min = 1e-12, max = 0.1) + )] + pub barometric_datum_psd: f32, +} diff --git a/firmware/libs/kaptan-flight-config/src/profile/ids.rs b/firmware/libs/kaptan-flight-config/src/profile/ids.rs index f5d6004..1b8bd92 100644 --- a/firmware/libs/kaptan-flight-config/src/profile/ids.rs +++ b/firmware/libs/kaptan-flight-config/src/profile/ids.rs @@ -1,20 +1,20 @@ -//! Symbolic projections used by relation checks and focused tests. -//! -//! These constants never define an ID. Each projects the metadata generated -//! from its owning `#[config(id = ...)]` declaration. +//! Field IDs derived from their owning `#[config(id = ...)]` declarations. use kaptan_config::FieldId; -use super::{ - control::{ActuatorLimitsCandidate, PidCandidate, ThrustCandidate}, - timing::TimingCandidate, -}; +use super::{ActuatorLimits, ControlSettings, PidSettings, ThrustSettings, TimingSettings}; -pub(super) const MAXIMUM_IMU_INTERVAL_MICROSECONDS: FieldId = TimingCandidate::DESCRIPTORS[0].id; -pub(super) const IMU_FRESHNESS_MICROSECONDS: FieldId = TimingCandidate::DESCRIPTORS[1].id; -pub(super) const MASS_KG: FieldId = ThrustCandidate::DESCRIPTORS[0].id; -pub(super) const MAXIMUM_COLLECTIVE_THRUST_NEWTONS: FieldId = ThrustCandidate::DESCRIPTORS[1].id; -pub(super) const RATE_PID_OUTPUT_LIMIT: FieldId = PidCandidate::DESCRIPTORS[3].id; -pub(super) const RATE_PID_INTEGRAL_LIMIT: FieldId = PidCandidate::DESCRIPTORS[4].id; -pub(super) const ACTUATOR_MINIMUM: FieldId = ActuatorLimitsCandidate::DESCRIPTORS[0].id; -pub(super) const ACTUATOR_MAXIMUM: FieldId = ActuatorLimitsCandidate::DESCRIPTORS[1].id; +// Field IDs 6, 7 and 25 remain reserved. + +pub(super) const MAXIMUM_IMU_INTERVAL_MICROSECONDS: FieldId = TimingSettings::DESCRIPTORS[0].id; +pub(super) const IMU_FRESHNESS_MICROSECONDS: FieldId = TimingSettings::DESCRIPTORS[1].id; +pub(super) const LOCAL_STATE_FRESHNESS_MICROSECONDS: FieldId = TimingSettings::DESCRIPTORS[2].id; +pub(super) const OFFBOARD_TIMEOUT_MICROSECONDS: FieldId = TimingSettings::DESCRIPTORS[3].id; +pub(super) const MAXIMUM_DOWNWARD_ACCELERATION_MPS2: FieldId = ControlSettings::DESCRIPTORS[4].id; +pub(super) const MASS_KG: FieldId = ThrustSettings::DESCRIPTORS[0].id; +pub(super) const MAXIMUM_COLLECTIVE_THRUST_NEWTONS: FieldId = ThrustSettings::DESCRIPTORS[1].id; +pub(super) const RATE_PID_OUTPUT_LIMIT: FieldId = PidSettings::DESCRIPTORS[3].id; +pub(super) const RATE_PID_INTEGRAL_LIMIT: FieldId = PidSettings::DESCRIPTORS[4].id; +pub(super) const ACTUATOR_MINIMUM: FieldId = ActuatorLimits::DESCRIPTORS[0].id; +pub(super) const ACTUATOR_MAXIMUM: FieldId = ActuatorLimits::DESCRIPTORS[1].id; +pub(super) const ACTUATOR_SLEW_PER_SECOND: FieldId = ActuatorLimits::DESCRIPTORS[2].id; diff --git a/firmware/libs/kaptan-flight-config/src/profile/mavlink_logging.rs b/firmware/libs/kaptan-flight-config/src/profile/mavlink_logging.rs deleted file mode 100644 index 12493bb..0000000 --- a/firmware/libs/kaptan-flight-config/src/profile/mavlink_logging.rs +++ /dev/null @@ -1,16 +0,0 @@ -//! Persisted, restart-only MAVLink diagnostic-sink policy. - -use kaptan_config::Config; -use kaptan_log::LevelFilter; - -/// Candidate policy for the MAVLink diagnostic sink. -#[derive(Clone, Copy, Config)] -pub(super) struct MavlinkLoggingCandidate { - #[config( - enum, - id = 25, - key = "mavlink.logging.level_filter", - unit = "log_level" - )] - pub(super) level_filter: LevelFilter, -} diff --git a/firmware/libs/kaptan-flight-config/src/profile/mod.rs b/firmware/libs/kaptan-flight-config/src/profile/mod.rs index b7617ce..fbcd61a 100644 --- a/firmware/libs/kaptan-flight-config/src/profile/mod.rs +++ b/firmware/libs/kaptan-flight-config/src/profile/mod.rs @@ -3,13 +3,14 @@ mod active; mod candidate; mod control; +mod flight; mod ids; -mod mavlink_logging; mod safety; mod timing; -pub use active::{ - ActuatorLimits, AidingSettings, ControlSettings, PidSettings, SafetySettings, ThrustSettings, - TimingSettings, VehicleCapabilities, VehicleProfile, -}; -pub use candidate::{CandidateProfile, FORMAT_VERSION}; +pub use active::{VehicleCapabilities, VehicleProfile}; +pub use candidate::CandidateProfile; +pub use control::{ActuatorLimits, ControlSettings, PidSettings, ThrustSettings}; +pub use flight::FlightSettings; +pub use safety::SafetySettings; +pub use timing::TimingSettings; diff --git a/firmware/libs/kaptan-flight-config/src/profile/safety.rs b/firmware/libs/kaptan-flight-config/src/profile/safety.rs index 57a86f9..b31666d 100644 --- a/firmware/libs/kaptan-flight-config/src/profile/safety.rs +++ b/firmware/libs/kaptan-flight-config/src/profile/safety.rs @@ -1,7 +1,8 @@ use kaptan_config::Config; -#[derive(Clone, Copy, Config)] -pub(super) struct SafetyCandidate { +/// Prearm timing policy, shared by editable candidates and active snapshots. +#[derive(Clone, Copy, Debug, Eq, PartialEq, Config)] +pub struct SafetySettings { #[config( id = 1, key = "safety.prearm_dwell_us", @@ -9,5 +10,5 @@ pub(super) struct SafetyCandidate { nonzero, range(min = 1, max = 10_000_000) )] - pub(super) prearm_dwell_microseconds: u64, + pub prearm_dwell_microseconds: u64, } diff --git a/firmware/libs/kaptan-flight-config/src/profile/timing.rs b/firmware/libs/kaptan-flight-config/src/profile/timing.rs index 571b0df..31e93fe 100644 --- a/firmware/libs/kaptan-flight-config/src/profile/timing.rs +++ b/firmware/libs/kaptan-flight-config/src/profile/timing.rs @@ -2,9 +2,10 @@ use kaptan_config::{Config, ConfigViolation, ValidationRule}; use super::ids; -#[derive(Clone, Copy, Config)] +/// Stream timing policy, shared by editable candidates and active snapshots. +#[derive(Clone, Copy, Debug, Eq, PartialEq, Config)] #[config(post = Self::validate_timing)] -pub(super) struct TimingCandidate { +pub struct TimingSettings { #[config( id = 2, key = "timing.imu_max_interval_us", @@ -12,7 +13,7 @@ pub(super) struct TimingCandidate { nonzero, range(min = 1, max = 1_000_000) )] - pub(super) maximum_imu_interval_microseconds: u64, + pub maximum_imu_interval_microseconds: u64, #[config( id = 3, key = "timing.imu_freshness_us", @@ -20,7 +21,7 @@ pub(super) struct TimingCandidate { nonzero, range(min = 1, max = 1_000_000) )] - pub(super) imu_freshness_microseconds: u64, + pub imu_freshness_microseconds: u64, #[config( id = 4, key = "timing.local_state_freshness_us", @@ -28,19 +29,29 @@ pub(super) struct TimingCandidate { nonzero, range(min = 1, max = 10_000_000) )] - pub(super) local_state_freshness_microseconds: u64, + pub local_state_freshness_microseconds: u64, #[config( id = 5, - key = "timing.link_freshness_us", + key = "timing.offboard_timeout_us", unit = "us", nonzero, range(min = 1, max = 10_000_000) )] - pub(super) command_link_freshness_microseconds: u64, + pub offboard_timeout_microseconds: u64, } -impl TimingCandidate { +impl TimingSettings { fn validate_timing(&self) -> Result<(), ConfigViolation> { + // The shared primitives retain their numerical upper bound. The flight + // candidate separately validates its tighter IMU and control cadences. + if self.maximum_imu_interval_microseconds + > kaptan_control::MAXIMUM_CONTROL_STEP_MICROSECONDS + { + return Err(ConfigViolation::field( + ids::MAXIMUM_IMU_INTERVAL_MICROSECONDS, + ValidationRule::Inconsistent, + )); + } if self.maximum_imu_interval_microseconds > self.imu_freshness_microseconds { return Err(ConfigViolation::related( ids::MAXIMUM_IMU_INTERVAL_MICROSECONDS, diff --git a/firmware/libs/kaptan-flight-config/tests/control_domain.rs b/firmware/libs/kaptan-flight-config/tests/control_domain.rs new file mode 100644 index 0000000..2468d67 --- /dev/null +++ b/firmware/libs/kaptan-flight-config/tests/control_domain.rs @@ -0,0 +1,275 @@ +mod support; + +use kaptan_config::{ConfigModel, ConfigValue, ConfigViolation, FieldId, ValidationRule}; +use kaptan_control::{ + HeadingNed, KinematicLimits, Pid, PidConfig, ThrustEnvelope, Trajectory, + TranslationalControlConfig, TranslationalController, TranslationalEstimate, force_to_attitude, +}; +use kaptan_flight_config::{CandidateProfile, VehicleCapabilities}; +use kaptan_navigation::{GRAVITY_MPS2, MAXIMUM_FREE_ACCELERATION_MPS2, PositionNed, VelocityNed}; + +const CAPABILITIES: VehicleCapabilities = support::capabilities(); +const IMU_INTERVAL: &str = "timing.imu_max_interval_us"; +const IMU_FRESHNESS: &str = "timing.imu_freshness_us"; +const DOWNWARD_ACCELERATION: &str = "control.maximum_downward_acceleration_mps2"; + +fn id(key: &str) -> FieldId { + CandidateProfile::descriptors() + .find(|field| field.display_key == key) + .unwrap() + .id +} + +fn set(candidate: &mut CandidateProfile, key: &str, value: ConfigValue) { + candidate.set_field(id(key), value).unwrap(); +} + +fn with_interval(microseconds: u64) -> CandidateProfile { + let mut candidate = support::candidate(); + set(&mut candidate, IMU_INTERVAL, ConfigValue::U64(microseconds)); + set(&mut candidate, IMU_FRESHNESS, ConfigValue::U64(20_000)); + candidate +} + +fn assert_rejected(candidate: CandidateProfile, issue: ConfigViolation) { + // Both whole-tree validation and active-profile construction must reject. + assert_eq!( + ::validate(&candidate), + Err(issue) + ); + assert_eq!(candidate.validate(&CAPABILITIES), Err(issue)); +} + +#[test] +fn imu_interval_cannot_exceed_the_controller_domain() { + // The generic primitive still permits 100 ms, but this flight profile's + // delayed estimator and consumer require 10..20 ms inertial intervals. + assert_eq!(kaptan_control::MAXIMUM_CONTROL_STEP_MICROSECONDS, 100_000); + for interval in [10_000, 15_000, 19_999, 20_000] { + let active = with_interval(interval).validate(&CAPABILITIES).unwrap(); + assert_eq!(active.timing().maximum_imu_interval_microseconds, interval); + } + for interval in [ + 1, 9_999, 20_001, 50_000, 100_000, 100_001, 200_000, 1_000_000, + ] { + let candidate = with_interval(interval); + assert!(candidate.validate(&CAPABILITIES).is_err()); + assert!(::validate(&candidate).is_err()); + } +} + +#[test] +fn downward_acceleration_has_a_strict_gravity_boundary() { + let below = f32::from_bits(GRAVITY_MPS2.to_bits() - 1); + let above = f32::from_bits(GRAVITY_MPS2.to_bits() + 1); + for acceleration in [0.001, 2.0, below] { + let mut candidate = support::candidate(); + set( + &mut candidate, + DOWNWARD_ACCELERATION, + ConfigValue::F32(acceleration), + ); + let active = candidate.validate(&CAPABILITIES).unwrap(); + assert_eq!( + active.control().maximum_downward_acceleration_mps2, + acceleration + ); + } + for acceleration in [GRAVITY_MPS2, above, 100.0] { + let mut candidate = support::candidate(); + set( + &mut candidate, + DOWNWARD_ACCELERATION, + ConfigValue::F32(acceleration), + ); + assert_rejected( + candidate, + ConfigViolation::field(id(DOWNWARD_ACCELERATION), ValidationRule::Inconsistent), + ); + } +} + +#[test] +fn thrust_margin_uses_the_same_gravity_contract() { + for thrust in [f32::from_bits(GRAVITY_MPS2.to_bits() - 1), GRAVITY_MPS2] { + let mut candidate = support::candidate(); + set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(1.0)); + set( + &mut candidate, + "thrust.maximum_collective_thrust_n", + ConfigValue::F32(thrust), + ); + assert_rejected( + candidate, + ConfigViolation::related( + id("thrust.maximum_collective_thrust_n"), + id("thrust.mass_kg"), + ValidationRule::Inconsistent, + ), + ); + } + let mut candidate = support::candidate(); + set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(1.0)); + set( + &mut candidate, + "thrust.maximum_collective_thrust_n", + ConfigValue::F32(f32::from_bits(GRAVITY_MPS2.to_bits() + 1)), + ); + assert!(candidate.validate(&CAPABILITIES).is_ok()); +} + +#[test] +fn thrust_to_mass_certifies_navigation_free_acceleration_without_epsilon() { + let issue = ConfigViolation::related( + id("thrust.maximum_collective_thrust_n"), + id("thrust.mass_kg"), + ValidationRule::Inconsistent, + ); + let mut former = support::candidate(); + set(&mut former, "thrust.mass_kg", ConfigValue::F32(0.01)); + assert_rejected(former, issue); + assert!(support::candidate().validate(&CAPABILITIES).is_ok()); + + let acceleration = f64::from(MAXIMUM_FREE_ACCELERATION_MPS2) - f64::from(GRAVITY_MPS2); + for mass in [0.01_f32, 1.0, 1.2, 100.0] { + let exact = f64::from(mass) * acceleration; + let rounded = exact as f32; + let floor = if f64::from(rounded) > exact { + rounded.next_down() + } else { + rounded + }; + let mut candidate = support::candidate(); + set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(mass)); + for thrust in [floor.next_down(), floor] { + set( + &mut candidate, + "thrust.maximum_collective_thrust_n", + ConfigValue::F32(thrust), + ); + assert!(candidate.validate(&CAPABILITIES).is_ok()); + } + set( + &mut candidate, + "thrust.maximum_collective_thrust_n", + ConfigValue::F32(floor.next_up()), + ); + assert_rejected(candidate, issue); + } + // At m=1 the upper boundary is exactly representable in binary32. + let exact_thrust = MAXIMUM_FREE_ACCELERATION_MPS2 - GRAVITY_MPS2; + assert_eq!(f64::from(exact_thrust), acceleration); + let mut candidate = support::candidate(); + set( + &mut candidate, + "thrust.maximum_collective_thrust_n", + ConfigValue::F32(exact_thrust), + ); + set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(1.0)); + assert!(candidate.validate(&CAPABILITIES).is_ok()); + set( + &mut candidate, + "thrust.mass_kg", + ConfigValue::F32(1.0_f32.next_down()), + ); + assert_rejected(candidate, issue); + set( + &mut candidate, + "thrust.mass_kg", + ConfigValue::F32(1.0_f32.next_up()), + ); + assert!(candidate.validate(&CAPABILITIES).is_ok()); +} + +#[test] +fn accepted_boundary_profiles_operate_the_real_control_primitives() { + for mass in [0.01, 1.0] { + let mut candidate = with_interval(20_000); + let downward_limit = f32::from_bits(GRAVITY_MPS2.to_bits() - 1); + set( + &mut candidate, + DOWNWARD_ACCELERATION, + ConfigValue::F32(downward_limit), + ); + set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(mass)); + set( + &mut candidate, + "thrust.maximum_collective_thrust_n", + ConfigValue::F32(15.0 * mass), + ); + for (key, gain) in [ + ("control.rate_pid.proportional", 0.1), + ("control.rate_pid.integral", 0.02), + ("control.rate_pid.derivative", 0.05), + ] { + set(&mut candidate, key, ConfigValue::F32(gain)); + } + let active = candidate.validate(&CAPABILITIES).unwrap(); + assert_eq!(active.timing().maximum_imu_interval_microseconds, 20_000); + // Preserve the independent 100-ms known answers of the generic + // primitives, without claiming it is a valid flight-estimator cadence. + let dt = 0.1; + let gains = active.rate_pid(); + let mut pid = Pid::new(PidConfig { + proportional: gains.proportional, + integral: gains.integral, + derivative: gains.derivative, + output_limit: gains.output_limit, + integral_limit: gains.integral_limit, + }) + .unwrap(); + // Known answer with all three terms active at the 100-ms boundary. + assert!((pid.update(0.2, 0.3, dt).unwrap() - 0.0354).abs() < 1.0e-7); + + let mut trajectory = Trajectory::new( + PositionNed::metres(0.0, 0.0, 0.0).vector(), + KinematicLimits { + maximum_horizontal_speed_mps: 1.0, + maximum_vertical_speed_mps: 1.0, + maximum_horizontal_acceleration_mps2: 1.0, + maximum_vertical_acceleration_mps2: 1.0, + }, + ) + .unwrap(); + trajectory + .set_goal(PositionNed::metres(0.0, 0.0, 1.0).vector()) + .unwrap(); + let reference = trajectory.update(dt).unwrap(); + assert!((reference.position.z() - 0.01).abs() < 1.0e-8); + + let control = active.control(); + let controller = TranslationalController::new(TranslationalControlConfig { + position_gain_per_second_squared: control.position_gain_per_second_squared, + velocity_gain_per_second: control.velocity_gain_per_second, + maximum_horizontal_acceleration_mps2: control.maximum_horizontal_acceleration_mps2, + maximum_upward_acceleration_mps2: control.maximum_upward_acceleration_mps2, + maximum_downward_acceleration_mps2: control.maximum_downward_acceleration_mps2, + }) + .unwrap(); + let acceleration = controller + .update( + reference, + TranslationalEstimate { + position: PositionNed::metres(0.0, 0.0, -100.0).vector(), + velocity: VelocityNed::metres_per_second(0.0, 0.0, 0.0).vector(), + }, + ) + .unwrap(); + assert_eq!(acceleration.z(), downward_limit); + + let thrust = active.thrust(); + let envelope = ThrustEnvelope { + mass_kg: thrust.mass_kg, + maximum_collective_thrust_newtons: thrust.maximum_collective_thrust_newtons, + maximum_tilt_tangent: thrust.maximum_tilt_tangent, + }; + let (force, saturated) = envelope + .force_for_acceleration(acceleration, GRAVITY_MPS2) + .unwrap(); + // Gravity is in [8, 16), so its f32 predecessor is exactly 2^-20 lower. + assert_eq!(force.vector().z(), -mass / 1_048_576.0); + assert!(!saturated); + let attitude = force_to_attitude(force, HeadingNed::new(1.0, 0.0).unwrap()).unwrap(); + assert_eq!(attitude.w(), 1.0); + } +} diff --git a/firmware/libs/kaptan-flight-config/tests/support/mod.rs b/firmware/libs/kaptan-flight-config/tests/support/mod.rs new file mode 100644 index 0000000..6698269 --- /dev/null +++ b/firmware/libs/kaptan-flight-config/tests/support/mod.rs @@ -0,0 +1,96 @@ +use kaptan_flight_config::profile::*; +use kaptan_math::Vector3; +use kaptan_navigation::{ + BarometricReference, CalibrationRevision, ContactGeometry, FrameId, NavigationInstallation, + NavigationSources, RangeInstallation, SensorReferenceId, SourceId, +}; + +pub const fn candidate() -> CandidateProfile { + CandidateProfile { + safety: SafetySettings { + prearm_dwell_microseconds: 500_000, + }, + timing: TimingSettings { + maximum_imu_interval_microseconds: 20_000, + imu_freshness_microseconds: 20_000, + local_state_freshness_microseconds: 250_000, + offboard_timeout_microseconds: 300_000, + }, + control: ControlSettings { + position_gain_per_second_squared: 0.8, + velocity_gain_per_second: 1.2, + maximum_horizontal_acceleration_mps2: 3.0, + maximum_upward_acceleration_mps2: 2.0, + maximum_downward_acceleration_mps2: 2.0, + attitude_roll_pitch_gain_per_second: 4.0, + attitude_yaw_gain_per_second: 2.0, + }, + thrust: ThrustSettings { + mass_kg: 1.2, + maximum_collective_thrust_newtons: 18.0, + maximum_tilt_tangent: 0.700_207_53, + }, + rate_pid: PidSettings { + proportional: 0.080, + integral: 0.006, + derivative: 0.0, + output_limit: 0.10, + integral_limit: 0.025, + yaw_proportional: 0.990, + yaw_integral: 0.074, + }, + actuator_limits: ActuatorLimits { + minimum: 0.0, + maximum: 1.0, + slew_per_second: 8.0, + }, + flight: FlightSettings { + command_ttl_us: 250_000, + fallback_hold_us: 1_000_000, + output_lifetime_us: 50_000, + control_period_us: 10_000, + control_deadline_us: 20_000, + time_error_bound_us: 2_000, + gyro_psd: 2.5e-7, + acceleration_psd: 2.5e-5, + gyro_bias_psd: 1e-10, + acceleration_bias_psd: 1e-8, + barometric_datum_psd: 1e-5, + }, + } +} + +pub const fn installation() -> NavigationInstallation { + NavigationInstallation { + sources: NavigationSources { + imu: SourceId::new(1), + local_position_velocity: SourceId::new(2), + magnetic: SourceId::new(3), + pressure: SourceId::new(4), + range: SourceId::new(5), + }, + range: RangeInstallation { + frame: FrameId::new(3), + offset_frd_metres: Vector3::new(0.0, 0.0, 0.04), + direction_frd: Vector3::new(0.0, 0.0, 1.0), + quantization_error_metres: 0.0005, + }, + contact: ContactGeometry { + half_span_x_metres: 0.12, + half_span_y_metres: 0.12, + centre_z_metres: 0.065, + radius_metres: 0.015, + }, + atmosphere: BarometricReference { + pressure_pascals: 101_325.0, + temperature_kelvin: 288.15, + }, + magnetic_field_ned_microtesla: Vector3::new(20.0, 0.0, 45.0), + calibration: CalibrationRevision::new(1), + reference: SensorReferenceId(1), + } +} + +pub const fn capabilities() -> VehicleCapabilities { + VehicleCapabilities::quad_x(installation()) +} diff --git a/firmware/libs/kaptan-flight-runtime/Cargo.toml b/firmware/libs/kaptan-flight-runtime/Cargo.toml index 564b40c..9e248b4 100644 --- a/firmware/libs/kaptan-flight-runtime/Cargo.toml +++ b/firmware/libs/kaptan-flight-runtime/Cargo.toml @@ -9,6 +9,12 @@ version.workspace = true workspace = true [dependencies] -kaptan-config = { path = "../kaptan-config" } kaptan-flight-config = { path = "../kaptan-flight-config" } -kaptan-log = { path = "../kaptan-log" } +kaptan-control = { path = "../kaptan-control" } +kaptan-math = { path = "../kaptan-math" } +kaptan-navigation = { path = "../kaptan-navigation" } +kaptan-safety = { path = "../kaptan-safety" } +kaptan-time = { path = "../kaptan-time" } + +[dev-dependencies] +kaptan-config = { path = "../kaptan-config" } diff --git a/firmware/libs/kaptan-flight-runtime/src/controller.rs b/firmware/libs/kaptan-flight-runtime/src/controller.rs new file mode 100644 index 0000000..c922e90 --- /dev/null +++ b/firmware/libs/kaptan-flight-runtime/src/controller.rs @@ -0,0 +1,214 @@ +use kaptan_control::{ + ActuatorLimits, Allocator, BodyRate, BodyWrench, HeadingNed, KinematicReference, MixerOutput, + PidConfig, PrecomputedMixer, RatePid, ThrustEnvelope, TranslationalControlConfig, + TranslationalController, TranslationalEstimate, force_to_attitude, +}; +use kaptan_flight_config::VehicleProfile; +use kaptan_math::{Frd, Ned, UnitQuaternion, Vector3}; +use kaptan_navigation::GRAVITY_MPS2; + +use crate::{FlightEstimate, RuntimeConfigError}; + +pub(crate) struct QuadController { + translational: TranslationalController, + thrust: ThrustEnvelope, + rate: RatePid, + allocator: Allocator<4>, + heading: Vector3, + attitude_gain_per_second: [f32; 3], +} + +impl QuadController { + pub fn new(config: VehicleProfile) -> Result { + let control = config.control(); + let thrust = config.thrust(); + let rate = config.rate_pid(); + let limits = config.actuator_limits(); + let translational = TranslationalController::new(TranslationalControlConfig { + position_gain_per_second_squared: control.position_gain_per_second_squared, + velocity_gain_per_second: control.velocity_gain_per_second, + maximum_horizontal_acceleration_mps2: control.maximum_horizontal_acceleration_mps2, + maximum_upward_acceleration_mps2: control.maximum_upward_acceleration_mps2, + maximum_downward_acceleration_mps2: control.maximum_downward_acceleration_mps2, + }) + .map_err(|_| RuntimeConfigError::Control)?; + let thrust = ThrustEnvelope { + mass_kg: thrust.mass_kg, + maximum_collective_thrust_newtons: thrust.maximum_collective_thrust_newtons, + maximum_tilt_tangent: thrust.maximum_tilt_tangent, + }; + thrust.validate().map_err(|_| RuntimeConfigError::Control)?; + let yaw_gains = (rate.yaw_proportional, rate.yaw_integral); + let rate = PidConfig { + proportional: rate.proportional, + integral: rate.integral, + derivative: rate.derivative, + output_limit: rate.output_limit, + integral_limit: rate.integral_limit, + }; + let limits = ActuatorLimits { + minimum: limits.minimum, + maximum: limits.maximum, + slew_per_second: limits.slew_per_second, + }; + let mixer = PrecomputedMixer::new(config.mixer().map(|coefficients| MixerOutput { + coefficients, + limits, + })); + Ok(Self { + translational, + thrust, + rate: RatePid::new( + rate, + rate, + PidConfig { + proportional: yaw_gains.0, + integral: yaw_gains.1, + ..rate + }, + ) + .map_err(|_| RuntimeConfigError::Control)?, + allocator: Allocator::new(mixer).map_err(|_| RuntimeConfigError::Control)?, + heading: Vector3::new(1., 0., 0.), + attitude_gain_per_second: [ + control.attitude_roll_pitch_gain_per_second, + control.attitude_roll_pitch_gain_per_second, + control.attitude_yaw_gain_per_second, + ], + }) + } + + pub fn update( + &mut self, + state: FlightEstimate, + mut reference: KinematicReference, + dt_seconds: f32, + yaw: Option, + position_feedback: [bool; 3], + collective_cap: f32, + ) -> Result<[f32; 4], ()> { + let measured_position = state.position.vector(); + reference.position = Vector3::new( + if position_feedback[0] { + reference.position.x() + } else { + measured_position.x() + }, + if position_feedback[1] { + reference.position.y() + } else { + measured_position.y() + }, + if position_feedback[2] { + reference.position.z() + } else { + measured_position.z() + }, + ); + let acceleration = self + .translational + .update( + reference, + TranslationalEstimate { + position: state.position.vector(), + velocity: state.velocity.vector(), + }, + ) + .map_err(|_| ())?; + let (force, limited) = self + .thrust + .force_for_acceleration(acceleration, GRAVITY_MPS2) + .map_err(|_| ())?; + if let Some(yaw) = yaw { + let half = UnitQuaternion::::from_small_rotation_vector(Vector3::new( + 0., + 0., + yaw * 0.5, + )) + .map_err(|_| ())?; + let goal = half + .compose(half) + .map_err(|_| ())? + .rotate(Vector3::new(1., 0., 0.)); + let sign = if self.heading.cross(goal).z() < 0. { + -1. + } else { + 1. + }; + let step = UnitQuaternion::::from_small_rotation_vector(Vector3::new( + 0., + 0., + sign * dt_seconds, + )) + .map_err(|_| ())?; + let cosine = step.rotate(Vector3::new(1., 0., 0.)).x(); + self.heading = if self.heading.dot(goal) >= cosine { + goal + } else { + step.rotate(self.heading) + }; + } + let heading = HeadingNed::new(self.heading.x(), self.heading.y()).ok_or(())?; + let target = force_to_attitude(force, heading).map_err(|_| ())?; + let error = state.attitude.inverse().compose(target).map_err(|_| ())?; + let direction = if error.w() < 0.0 { -1.0 } else { 1.0 }; + let target_rate = BodyRate::radians_per_second( + direction * 2.0 * self.attitude_gain_per_second[0] * error.x(), + direction * 2.0 * self.attitude_gain_per_second[1] * error.y(), + direction * 2.0 * self.attitude_gain_per_second[2] * error.z(), + ); + let torque = self + .rate + .update( + target_rate, + BodyRate::radians_per_second( + state.angular_rate.vector().x(), + state.angular_rate.vector().y(), + state.angular_rate.vector().z(), + ), + dt_seconds, + ) + .map_err(|_| ())?; + let allocation = self + .allocator + .allocate_capped( + BodyWrench { + roll: torque.x(), + pitch: torque.y(), + yaw: torque.z(), + collective: (force.vector().norm() + / self.thrust.maximum_collective_thrust_newtons) + .min(collective_cap), + }, + dt_seconds, + collective_cap * 4., + ) + .map_err(|_| ())?; + if limited + || allocation.saturated + || force.vector().norm() / self.thrust.maximum_collective_thrust_newtons + > collective_cap + { + self.rate.reset_integrators(); + } + Ok(allocation.commands) + } + + pub fn reset(&mut self) { + self.rate.reset(); + self.allocator.reset_commands(); + } + pub fn capture_heading(&mut self, attitude: UnitQuaternion) -> Result<(), ()> { + let forward = attitude.body_to_ned(Vector3::new(1., 0., 0.)); + self.heading = Vector3::new(forward.x(), forward.y(), 0.) + .normalized() + .ok_or(())?; + Ok(()) + } + pub fn heading_aligned(&self, attitude: UnitQuaternion) -> bool { + let forward = attitude.body_to_ned(Vector3::new(1., 0., 0.)); + Vector3::new(forward.x(), forward.y(), 0.) + .normalized() + .is_some_and(|v| v.dot(self.heading) >= 0.996_194_7) + } +} diff --git a/firmware/libs/kaptan-flight-runtime/src/execution.rs b/firmware/libs/kaptan-flight-runtime/src/execution.rs new file mode 100644 index 0000000..00c0cec --- /dev/null +++ b/firmware/libs/kaptan-flight-runtime/src/execution.rs @@ -0,0 +1,2147 @@ +use crate::{controller::QuadController, *}; +use kaptan_control::{KinematicLimits, Trajectory}; +use kaptan_flight_config::VehicleProfile; +use kaptan_math::{Ned, Vector3}; +use kaptan_navigation::{GRAVITY_MPS2, Navigation, SourceStatus}; +use kaptan_safety::{CommitOutcome, SafetyConfig, SafetyInputs, SafetyState, SafetySupervisor}; +use kaptan_time::Timestamp; + +pub struct FlightEndpoint { + profile: VehicleProfile, + navigation: Navigation, + controller: QuadController, + safety: SafetySupervisor, + epoch: RuntimeEpoch, + owner: CommandOwner, + now: Option, + control_at: Option, + next_control_due: Option, + arm: Option, + phase: VehiclePhase, + since: u64, + deadline: Option, + flight_deadline: Option, + fallback: bool, + terminal: Option, + offboard: Option, + stream_highwater: Option, + stream_expiry: Option, + request_highwater: Option, + active: Option, + last_terminal: Option, + outcomes: [Option; 2], + inhibited_publication_failed: bool, + trajectory: Option, + target: Vector3, + setpoint: Option, + settled_since: Option, + ground_since: Option, + ground: GroundState, + cap: f32, + last_collective: f32, + descent_attempted: bool, + output_sequence: u64, + output_healthy: bool, + last_commit: Option, +} + +// Persistent owner budget on every compiled target. Peak numerical stack and +// the rest of each complete firmware image require separate target evidence. +const _: () = assert!(core::mem::size_of::() <= 32 * 1024); + +fn time(v: u64) -> Timestamp { + Timestamp::from_microseconds(v) +} +fn limits() -> KinematicLimits { + KinematicLimits { + maximum_horizontal_speed_mps: 3., + maximum_vertical_speed_mps: 1.5, + maximum_horizontal_acceleration_mps2: 2., + maximum_vertical_acceleration_mps2: 1.5, + } +} +impl FlightEndpoint { + pub fn new(profile: VehicleProfile, binding: RunBinding) -> Result { + if binding.session.0 == 0 { + return Err(RuntimeConfigError::Timing); + } + Ok(Self { + navigation: Navigation::new(profile.navigation()) + .map_err(|_| RuntimeConfigError::Navigation)?, + controller: QuadController::new(profile)?, + safety: SafetySupervisor::new( + SafetyConfig::new(profile.safety().prearm_dwell_microseconds) + .map_err(|_| RuntimeConfigError::Safety)?, + ), + profile, + epoch: RuntimeEpoch::from_parts(binding.session, 0), + owner: binding.owner, + now: None, + control_at: None, + next_control_due: None, + arm: None, + phase: VehiclePhase::Disarmed, + since: 0, + deadline: None, + flight_deadline: None, + fallback: false, + terminal: None, + offboard: None, + stream_highwater: None, + stream_expiry: None, + request_highwater: None, + active: None, + last_terminal: None, + outcomes: [None; 2], + inhibited_publication_failed: false, + trajectory: None, + target: Vector3::zero(), + setpoint: None, + settled_since: None, + ground_since: None, + ground: GroundState::Unknown, + cap: 1., + last_collective: 0., + descent_attempted: false, + output_sequence: 0, + output_healthy: false, + last_commit: None, + }) + } + pub fn status(&self) -> FlightStatus { + FlightStatus { + epoch: self.epoch, + owner: self.owner, + at: time(self.now.unwrap_or(0)), + arm: self.arm, + phase: self.phase, + phase_since: time(self.since), + phase_deadline: self.deadline.map(time), + flight_deadline: self.flight_deadline.map(time), + fallback_latched: self.fallback, + fallback_reason: self.fallback.then_some(FallbackReason::OffboardExpired), + terminal_reason: self.terminal, + offboard: self.offboard, + setpoint_sequence: self.stream_highwater, + setpoint_expires_at: self.stream_expiry.map(time), + command_highwater: self.request_highwater, + active_command: self.active, + last_terminal_command: self.last_terminal, + estimate: self.navigation.estimate(), + ground_state: self.ground, + local_output_healthy: self.output_healthy, + last_output_sequence: (self.output_sequence != 0).then_some(self.output_sequence), + last_local_commit_at: self.last_commit.map(time), + source_status: self.navigation.source_status(), + } + } + /// Read after every observe/command/setpoint/supervise/tick/begin_epoch call, including + /// Err, before making another effectful call. One old accepted transaction + /// and one newly admitted transaction bound the summary to two outcomes. + pub fn command_outcomes(&self) -> [Option; 2] { + self.outcomes + } + /// Latest valid admitted window. This does not refresh the cached estimate + /// or grant permission to defer a deadline or renew an enabled output. + pub fn admitted_imu_coverage(&self) -> Option { + self.navigation.admitted_imu_coverage() + } + fn begin_call(&mut self) { + self.outcomes = [None; 2]; + self.inhibited_publication_failed = false; + } + fn remember_terminal(&mut self, status: CommandStatus) { + // Taking active consumes its one terminal transition. A command call + // may additionally admit and finish one new transaction, never a third. + let slot = self + .outcomes + .iter_mut() + .find(|entry| entry.is_none()) + .expect("one prior and one current accepted command per call"); + *slot = Some(CommandOutcome { + status, + published: false, + }); + self.last_terminal = Some(status); + } + fn finish_call( + &mut self, + result: Result>, + out: &mut O, + ) -> Result> { + let mut first_error = None; + // Always attempt both, in commit order. A full notification queue cannot + // prevent a cancellation from reaching the adapter's reconciliation cache. + for outcome in self.outcomes.iter_mut().flatten() { + match out.command_status(outcome.status) { + Ok(()) => outcome.published = true, + Err(error) => { + if first_error.is_none() { + first_error = Some(error); + } + } + } + } + match (result, first_error) { + (Ok(value), None) => Ok(value), + (Ok(_), Some(error)) => Err(EndpointFailure::Notification(error).into()), + (Err(error), Some(notification)) if self.inhibited_publication_failed => { + let inhibition = match error.primary { + EndpointFailure::Output(value) => Some(value), + _ => error.inhibition, + }; + Err(EndpointError { + primary: EndpointFailure::Notification(notification), + inhibition, + }) + } + (Err(error), _) => Err(error), + } + } + fn clock( + &mut self, + now: Timestamp, + out: &mut O, + ) -> Result> { + let n = now.as_microseconds(); + if self.now.is_some_and(|t| n < t) { + return Err(self.fault( + TerminalReason::ClockRegression, + EndpointFailure::ClockRegression, + out, + )); + } + if self.now.is_none() { + self.since = n; + } + self.now = Some(n); + Ok(n) + } + fn healthy(&self, now: u64) -> bool { + self.navigation + .estimate() + .and_then(|e| e.capabilities.valid_until()) + .is_some_and(|t| now < t.as_microseconds()) + && self + .navigation + .source_status() + .iter() + .all(|source| source.integrity == SourceStatus::Healthy) + && self.output_healthy + } + fn inputs(&self, now: u64) -> SafetyInputs { + SafetyInputs { + estimator_healthy: self.healthy(now), + imu_fresh: self.navigation.estimate().is_some_and(|e| { + e.capabilities + .tilt + .valid_until + .is_some_and(|t| now < t.as_microseconds()) + }), + phase_authorized: self.arm.is_some() && self.phase != VehiclePhase::Terminated, + output_healthy: self.output_healthy, + } + } + fn airborne(&self) -> bool { + matches!( + self.phase, + VehiclePhase::Takeoff + | VehiclePhase::Hold + | VehiclePhase::Offboard + | VehiclePhase::FallbackHold + | VehiclePhase::Land + | VehiclePhase::TouchdownConfirm + ) + } + fn mission_active(&self) -> bool { + self.active + .is_some_and(|command| command.kind == CommandKind::TakeoffHoverLand) + } + fn change(&mut self, phase: VehiclePhase, now: u64, deadline: Option) { + self.phase = phase; + self.since = now; + self.deadline = deadline; + self.settled_since = None; + } + fn clear_arm(&mut self) { + self.arm = None; + self.safety.request_disarm(); + self.offboard = None; + self.stream_expiry = None; + self.stream_highwater = None; + self.trajectory = None; + self.setpoint = None; + self.controller.reset(); + self.cap = 1.; + self.last_collective = 0.; + self.descent_attempted = false; + } + fn latch(&mut self, why: TerminalReason) { + // Preserve terminal command state even when a broken sink prevents its + // notification. Fatal cleanup retains the primary and inhibition errors. + self.finish_pending(CommandLifecycle::Failed, CommandReason::Terminated); + self.clear_arm(); + if self.phase != VehiclePhase::Terminated { + self.since = self.now.unwrap_or(0); + } + self.phase = VehiclePhase::Terminated; + self.terminal = Some(why); + self.deadline = None; + } + fn record( + &mut self, + enabled: bool, + valid_until: u64, + motors: [f32; 4], + ) -> Option { + let next = self.output_sequence.checked_add(1)?; + self.output_sequence = next; + if next == u64::MAX { + self.latch(TerminalReason::SequenceExhausted); + } + let permitted = enabled && next != u64::MAX; + let now = self.now.unwrap_or(0); + Some(OutputRecord { + epoch: self.epoch, + arm: self.arm, + sequence: next, + produced_at: time(now), + valid_until: time(if permitted { valid_until } else { now }), + permitted, + motors: if permitted { motors } else { [0.; 4] }, + }) + } + fn inhibit_raw(&mut self, out: &mut O) -> Option { + let record = self.record(false, 0, [0.; 4])?; + match out.actuator(record) { + Ok(()) => { + self.last_commit = self.now; + self.output_healthy = self.terminal != Some(TerminalReason::OutputFault); + None + } + Err(e) => { + self.output_healthy = false; + Some(e) + } + } + } + fn fault( + &mut self, + why: TerminalReason, + primary: EndpointFailure, + out: &mut O, + ) -> EndpointError { + self.latch(why); + let inhibition = self.inhibit_raw(out); + EndpointError { + primary, + inhibition, + } + } + fn publish( + &mut self, + enabled: bool, + motors: [f32; 4], + out: &mut O, + ) -> Result<(), EndpointError> { + let now = self.now.unwrap_or(0); + let mut until = now; + if enabled { + until = now + .checked_add(self.profile.flight().output_lifetime_us) + .ok_or_else(|| { + self.fault( + TerminalReason::ClockRegression, + EndpointFailure::ClockRegression, + out, + ) + })?; + let estimate = self + .navigation + .estimate() + .and_then(|e| e.capabilities.valid_until()) + .map(|t| t.as_microseconds()) + .unwrap_or(now); + until = until.min(estimate).min( + self.control_at + .unwrap_or(now) + .saturating_add(self.profile.flight().control_deadline_us), + ); + // A nonterminal mode transition does not revoke vehicle authority. + // Otherwise an off-grid offboard expiry would cut output before the + // next healthy control tick can publish its hold/land allocation. + if matches!( + self.phase, + VehiclePhase::Land | VehiclePhase::TouchdownConfirm + ) { + until = until.min(self.deadline.unwrap_or(now)); + } + if let Some(t) = self.flight_deadline { + until = until.min(t); + } + if until <= now + || motors + .iter() + .any(|v| !v.is_finite() || !(0.0..=1.0).contains(v)) + { + return Err(self.fault(TerminalReason::Numerical, EndpointFailure::Control, out)); + } + } + let inputs = self.inputs(now); + let Some(record) = self.record(enabled, until, motors) else { + self.latch(TerminalReason::SequenceExhausted); + return Err(EndpointFailure::OutputSequenceExhausted.into()); + }; + let result = if record.permitted { + match self + .safety + .evaluate_and_commit(time(now), inputs, || out.actuator(record)) + { + CommitOutcome::Committed(()) => Ok(()), + CommitOutcome::Failed(e) => Err(e), + CommitOutcome::Inhibited(_) => { + return Err(self.fault( + TerminalReason::NavigationLost, + EndpointFailure::Navigation(NavigationFailure::RequiredCapabilityLost), + out, + )); + } + } + } else { + out.actuator(record) + }; + if record.sequence == u64::MAX { + self.inhibited_publication_failed |= result.is_err(); + self.output_healthy = result.is_ok(); + if result.is_ok() { + self.last_commit = self.now; + } + return Err(EndpointError { + primary: EndpointFailure::OutputSequenceExhausted, + inhibition: result.err(), + }); + } + match result { + Ok(()) => { + self.output_healthy = self.terminal != Some(TerminalReason::OutputFault); + self.last_commit = self.now; + Ok(()) + } + Err(e) => { + self.inhibited_publication_failed |= !enabled; + self.output_healthy = false; + self.latch(TerminalReason::OutputFault); + let inhibition = if enabled { self.inhibit_raw(out) } else { None }; + Err(EndpointError { + primary: EndpointFailure::Output(e), + inhibition, + }) + } + } + } + fn notify( + &mut self, + s: CommandStatus, + out: &mut O, + ) -> Result<(), EndpointError> { + if matches!( + s.lifecycle, + CommandLifecycle::Completed | CommandLifecycle::Cancelled | CommandLifecycle::Failed + ) { + self.remember_terminal(s); + return Ok(()); + } + out.command_status(s) + .map_err(|e| EndpointFailure::Notification(e).into()) + } + fn finish_pending(&mut self, life: CommandLifecycle, reason: CommandReason) { + let Some(mut s) = self.active.take() else { + return; + }; + s.lifecycle = life; + s.reason = reason; + s.at = time(self.now.unwrap_or(0)); + self.remember_terminal(s); + } + fn stop( + &mut self, + phase: VehiclePhase, + reason: CommandReason, + out: &mut O, + ) -> Result<(), EndpointError> { + self.clear_arm(); + self.change(phase, self.now.unwrap_or(0), None); + self.flight_deadline = None; + if let Some(active) = &mut self.active { + active.arm = None; + } + if reason != CommandReason::None { + self.finish_pending(CommandLifecycle::Failed, reason); + } + self.publish(false, [0.; 4], out)?; + self.finish_pending(CommandLifecycle::Completed, CommandReason::None); + Ok(()) + } + fn seed(&mut self) -> Result<(), ()> { + let e = self.navigation.estimate().ok_or(())?; + self.trajectory = Some( + Trajectory::from_state(e.position.vector(), e.velocity.vector(), limits()) + .map_err(|_| ())?, + ); + self.target = e.position.vector(); + self.controller.capture_heading(e.attitude)?; + self.setpoint = None; + Ok(()) + } + fn land( + &mut self, + now: u64, + deadline: u64, + out: &mut O, + ) -> Result<(), EndpointError> { + if matches!( + self.phase, + VehiclePhase::Land | VehiclePhase::TouchdownConfirm + ) { + return Ok(()); + } + if self.seed().is_err() { + return Err(self.fault(TerminalReason::Numerical, EndpointFailure::Control, out)); + } + self.offboard = None; + self.stream_expiry = None; + self.ground_since = None; + self.cap = 1.; + self.descent_attempted = false; + self.change( + VehiclePhase::Land, + now, + Some(deadline.min(self.flight_deadline.unwrap_or(deadline))), + ); + Ok(()) + } + /// Supervise actual time, cached health and phase/stream deadlines without + /// advancing navigation or integrating control. May inhibit and notify, but + /// never renews enabled output or consumes a nominal control release. + pub fn supervise( + &mut self, + now: Timestamp, + out: &mut O, + ) -> Result<(), EndpointError> { + self.begin_call(); + let result = self.supervise_inner(now, out); + self.finish_call(result, out) + } + fn supervise_inner( + &mut self, + now: Timestamp, + out: &mut O, + ) -> Result<(), EndpointError> { + let now = self.clock(now, out)?; + if self.phase == VehiclePhase::Terminated { + return self.publish(false, [0.; 4], out); + } + self.check_control_gap(now, out)?; + self.supervise_phase(now, out) + } + fn check_control_gap( + &mut self, + now: u64, + out: &mut O, + ) -> Result<(), EndpointError> { + if self.airborne() + && self + .control_at + .is_some_and(|t| now - t > self.profile.flight().control_deadline_us) + { + return Err(self.fault( + TerminalReason::ControlDeadline, + EndpointFailure::ControlDeadline, + out, + )); + } + Ok(()) + } + fn supervise_phase( + &mut self, + now: u64, + out: &mut O, + ) -> Result<(), EndpointError> { + if self.phase == VehiclePhase::Terminated { + return Ok(()); + } + if self.phase == VehiclePhase::PreArm && !self.healthy(now) { + return self.stop( + VehiclePhase::Disarmed, + CommandReason::MissingCapability, + out, + ); + } + if (self.airborne() || self.phase == VehiclePhase::ArmedIdle) && !self.healthy(now) { + return Err(self.fault( + TerminalReason::NavigationLost, + EndpointFailure::Navigation(NavigationFailure::RequiredCapabilityLost), + out, + )); + } + if self.phase == VehiclePhase::Offboard && self.stream_expiry.is_some_and(|t| now >= t) { + let end = self + .stream_expiry + .unwrap() + .saturating_add(self.profile.flight().fallback_hold_us); + self.fallback = true; + if self.seed().is_err() { + return Err(self.fault(TerminalReason::Numerical, EndpointFailure::Control, out)); + } + self.offboard = None; + self.stream_expiry = None; + self.change(VehiclePhase::FallbackHold, now, Some(end)); + } + if self.flight_deadline.is_some_and(|t| now >= t) { + return Err(self.fault( + TerminalReason::FlightDeadline, + EndpointFailure::ControlDeadline, + out, + )); + } + if self.airborne() + && self + .flight_deadline + .is_some_and(|t| now >= t.saturating_sub(LANDING_BUDGET_US)) + { + if !matches!( + self.phase, + VehiclePhase::Land | VehiclePhase::TouchdownConfirm + ) { + self.finish_pending(CommandLifecycle::Failed, CommandReason::PhaseDeadline); + } + self.land(now, self.flight_deadline.unwrap(), out)?; + } + if self.deadline.is_some_and(|t| now >= t) { + match self.phase { + VehiclePhase::ArmedIdle => { + return self.stop(VehiclePhase::Disarmed, CommandReason::PhaseDeadline, out); + } + VehiclePhase::Takeoff | VehiclePhase::Hold => { + let bound = self.deadline.unwrap().saturating_add(LANDING_BUDGET_US); + if self.phase != VehiclePhase::Hold || !self.mission_active() { + self.finish_pending(CommandLifecycle::Failed, CommandReason::PhaseDeadline); + } + self.land(now, bound, out)?; + } + VehiclePhase::FallbackHold => self.land( + now, + self.deadline.unwrap().saturating_add(LANDING_BUDGET_US), + out, + )?, + VehiclePhase::Land | VehiclePhase::TouchdownConfirm => { + return Err(self.fault( + TerminalReason::LandingDeadline, + EndpointFailure::ControlDeadline, + out, + )); + } + _ => {} + } + } + Ok(()) + } + pub fn observe( + &mut self, + epoch: RuntimeEpoch, + record: SensorRecord, + processing_time: Timestamp, + out: &mut O, + ) -> Result> { + self.begin_call(); + let result = self.observe_inner(epoch, record, processing_time, out); + self.finish_call(result, out) + } + fn observe_inner( + &mut self, + epoch: RuntimeEpoch, + record: SensorRecord, + processing_time: Timestamp, + out: &mut O, + ) -> Result> { + if epoch != self.epoch { + return Ok(ObservationAdmission::Rejected( + ObservationRejection::WrongEpoch, + )); + } + self.clock(processing_time, out)?; + if self.phase == VehiclePhase::Terminated { + return Ok(ObservationAdmission::Rejected( + ObservationRejection::Terminated, + )); + } + self.navigation + .observe(record, processing_time) + .map_err(|e| self.fault(navigation_reason(e), EndpointFailure::Navigation(e), out)) + } + pub fn begin_epoch( + &mut self, + owner: CommandOwner, + now: Timestamp, + out: &mut O, + ) -> Result> { + self.begin_call(); + let result = self.begin_epoch_inner(owner, now, out); + self.finish_call(result, out) + } + fn begin_epoch_inner( + &mut self, + owner: CommandOwner, + now: Timestamp, + out: &mut O, + ) -> Result> { + self.clock(now, out)?; + if self.phase != VehiclePhase::Disarmed { + return Err(EndpointFailure::NotDisarmed.into()); + } + let generation = self.epoch.generation.checked_add(1).ok_or_else(|| { + self.fault( + TerminalReason::SequenceExhausted, + EndpointFailure::EpochExhausted, + out, + ) + })?; + self.publish(false, [0.; 4], out)?; + self.navigation = + Navigation::new(self.profile.navigation()).map_err(EndpointFailure::Navigation)?; + self.epoch.generation = generation; + self.owner = owner; + self.request_highwater = None; + self.active = None; + self.last_terminal = None; + self.ground = GroundState::Unknown; + self.ground_since = None; + self.control_at = None; + self.next_control_due = None; + self.fallback = false; + self.output_healthy = false; + Ok(self.epoch) + } + pub fn command( + &mut self, + r: CommandRequest, + now: Timestamp, + out: &mut O, + ) -> Result> { + self.begin_call(); + let result = self.command_inner(r, now, out); + self.finish_call(result, out) + } + fn command_inner( + &mut self, + r: CommandRequest, + now: Timestamp, + out: &mut O, + ) -> Result> { + let now = self.clock(now, out)?; + // A fresh Terminate has no navigation prerequisite. Other commands + // cannot rescue expired phase/stream authority before supervision. + if r.command != FlightCommand::Terminate { + self.supervise_phase(now, out)?; + } + let reason = self.admit_command(r, now); + let mut s = CommandStatus { + epoch: r.context.epoch, + owner: r.context.owner, + sequence: r.context.sequence, + kind: r.command.kind(), + lifecycle: CommandLifecycle::Rejected, + at: time(now), + reason, + arm: self.arm, + }; + if reason != CommandReason::None { + if r.command == FlightCommand::Terminate { + self.supervise_phase(now, out)?; + } + self.notify(s, out)?; + return Ok(s); + } + if r.command == FlightCommand::Land + && matches!( + self.phase, + VehiclePhase::Land | VehiclePhase::TouchdownConfirm + ) + && !self.mission_active() + { + s.reason = CommandReason::AlreadyActive; + self.notify(s, out)?; + return Ok(s); + } + self.finish_pending(CommandLifecycle::Cancelled, CommandReason::Superseded); + s.lifecycle = CommandLifecycle::Accepted; + self.active = Some(s); + match r.command { + FlightCommand::Arm => { + self.arm = Some(ArmId(r.context.sequence.0)); + s.arm = self.arm; + self.active = Some(s); + self.change(VehiclePhase::PreArm, now, None); + self.safety.request_arm(time(now), self.inputs(now)); + // Publish the new arm binding while explicitly inhibited first. + self.publish(false, [0.; 4], out)?; + } + FlightCommand::GroundDisarm => { + self.active = Some(s); + self.stop(VehiclePhase::Disarmed, CommandReason::None, out)?; + s.lifecycle = CommandLifecycle::Completed; + s.arm = None; + return Ok(s); + } + FlightCommand::Terminate => { + self.terminal = Some(TerminalReason::Requested); + s.arm = None; + self.active = Some(s); + self.stop(VehiclePhase::Terminated, CommandReason::None, out)?; + s.lifecycle = CommandLifecycle::Completed; + return Ok(s); + } + FlightCommand::Takeoff { + height_above_launch_metres: h, + } + | FlightCommand::TakeoffHoverLand { + height_above_launch_metres: h, + } => { + self.seed().map_err(|_| { + self.fault(TerminalReason::Numerical, EndpointFailure::Control, out) + })?; + self.target = Vector3::new(self.target.x(), self.target.y(), -h); + self.trajectory + .as_mut() + .unwrap() + .set_goal(self.target) + .map_err(|_| { + self.fault(TerminalReason::Numerical, EndpointFailure::Control, out) + })?; + self.flight_deadline = Some(now.saturating_add(120_000_000)); + self.ground_since = None; + self.ground = GroundState::Airborne; + self.change( + VehiclePhase::Takeoff, + now, + Some(now.saturating_add(15_000_000)), + ); + } + FlightCommand::Hold => { + self.seed().map_err(|_| { + self.fault(TerminalReason::Numerical, EndpointFailure::Control, out) + })?; + self.offboard = None; + self.stream_expiry = None; + self.change( + VehiclePhase::Hold, + now, + Some(now.saturating_add(10_000_000)), + ); + } + FlightCommand::EnterOffboard { initial, .. } => { + self.offboard = Some(r.context.sequence); + self.stream_highwater = None; + self.stream_expiry = r + .context + .issued_at + .as_microseconds() + .checked_sub(u64::from(r.context.time_uncertainty_microseconds)) + .and_then(|t| { + t.checked_add(self.profile.timing().offboard_timeout_microseconds) + }); + self.setpoint = Some(initial); + self.change(VehiclePhase::Offboard, now, None); + s.lifecycle = CommandLifecycle::Completed; + } + FlightCommand::Land => { + if self.phase == VehiclePhase::ArmedIdle { + self.active = Some(s); + self.stop(VehiclePhase::Disarmed, CommandReason::None, out)?; + s.lifecycle = CommandLifecycle::Completed; + s.arm = None; + return Ok(s); + } + self.land(now, now.saturating_add(LANDING_BUDGET_US), out)?; + } + } + if s.lifecycle == CommandLifecycle::Accepted { + self.active = Some(s); + } else { + self.active = None; + self.last_terminal = Some(s); + } + self.notify(s, out)?; + Ok(s) + } + fn admit_command(&mut self, r: CommandRequest, now: u64) -> CommandReason { + use CommandReason as R; + let c = r.context; + if c.epoch != self.epoch { + return R::WrongEpoch; + } + if c.owner != self.owner { + return R::WrongOwner; + } + if self.request_highwater == Some(RequestSequence(u64::MAX)) { + return R::SequenceExhausted; + } + if c.sequence.0 == 0 || self.request_highwater.is_some_and(|s| c.sequence <= s) { + return R::Replay; + } + self.request_highwater = Some(c.sequence); + let u = u64::from(c.time_uncertainty_microseconds); + if u > self.profile.flight().time_error_bound_us + || c.issued_at.as_microseconds().checked_sub(u).is_none() + || c.issued_at + .as_microseconds() + .checked_add(u) + .is_none_or(|t| t > now) + || c.expires_at + .as_microseconds() + .checked_sub(c.issued_at.as_microseconds()) + .is_none_or(|t| t == 0 || t > self.profile.flight().command_ttl_us) + { + return R::InvalidTime; + } + if c.expires_at + .as_microseconds() + .checked_sub(u) + .is_none_or(|t| now >= t) + { + return R::Expired; + } + if r.command == FlightCommand::Terminate { + return R::None; + } + if self.phase == VehiclePhase::Terminated { + return R::Terminated; + } + if r.command == FlightCommand::Arm { + return if self.phase != VehiclePhase::Disarmed { + R::WrongPhase + } else if c.arm.is_some() { + R::WrongArm + } else if self.ground != GroundState::Grounded || !self.healthy(now) { + R::NotReady + } else { + R::None + }; + } + if r.command == FlightCommand::GroundDisarm + && self.phase == VehiclePhase::Disarmed + && c.arm.is_none() + { + return R::None; + } + if c.arm != self.arm || c.arm.is_none() { + return R::WrongArm; + } + match r.command { + FlightCommand::GroundDisarm => { + if matches!(self.phase, VehiclePhase::PreArm | VehiclePhase::ArmedIdle) + && self.ground == GroundState::Grounded + { + R::None + } else { + R::WrongPhase + } + } + FlightCommand::Takeoff { + height_above_launch_metres: h, + } + | FlightCommand::TakeoffHoverLand { + height_above_launch_metres: h, + } => { + if !h.is_finite() || !(0.2..=5.).contains(&h) { + R::InvalidValue + } else if self.phase != VehiclePhase::ArmedIdle { + R::WrongPhase + } else if self.ground != GroundState::Grounded { + R::NotReady + } else { + R::None + } + } + FlightCommand::Hold => { + if matches!( + self.phase, + VehiclePhase::Takeoff | VehiclePhase::Hold | VehiclePhase::Offboard + ) { + R::None + } else { + R::WrongPhase + } + } + FlightCommand::EnterOffboard { origin, initial } => { + if self.fallback { + R::FallbackLatched + } else if self.phase != VehiclePhase::Hold { + R::WrongPhase + } else if self + .navigation + .estimate() + .is_none_or(|e| e.origin.id != origin) + { + R::WrongOrigin + } else if c + .issued_at + .as_microseconds() + .checked_sub(u) + .and_then(|t| { + t.checked_add(self.profile.timing().offboard_timeout_microseconds) + }) + .is_none() + { + R::InvalidTime + } else if !valid_setpoint(initial) { + R::InvalidValue + } else { + R::None + } + } + FlightCommand::Land => { + if self.airborne() || self.phase == VehiclePhase::ArmedIdle { + R::None + } else { + R::WrongPhase + } + } + _ => R::WrongPhase, + } + } + pub fn setpoint( + &mut self, + r: SetpointRequest, + now: Timestamp, + out: &mut O, + ) -> Result> { + self.begin_call(); + let result = self.setpoint_inner(r, now, out); + self.finish_call(result, out) + } + fn setpoint_inner( + &mut self, + r: SetpointRequest, + now: Timestamp, + out: &mut O, + ) -> Result> { + let now = self.clock(now, out)?; + self.supervise_phase(now, out)?; + let reason = self.admit_setpoint(r, now); + Ok(SetpointStatus { + offboard: r.offboard, + sequence: r.sequence, + accepted: reason == CommandReason::None, + reason, + expires_at: if reason == CommandReason::None { + self.stream_expiry.map(time) + } else { + None + }, + }) + } + fn admit_setpoint(&mut self, r: SetpointRequest, now: u64) -> CommandReason { + use CommandReason as R; + if r.epoch != self.epoch { + return R::WrongEpoch; + } + if r.owner != self.owner { + return R::WrongOwner; + } + if Some(r.arm) != self.arm { + return R::WrongArm; + } + if self + .navigation + .estimate() + .is_none_or(|e| e.origin.id != r.origin) + { + return R::WrongOrigin; + } + if Some(r.offboard) != self.offboard { + return R::WrongOffboard; + } + if self.phase != VehiclePhase::Offboard { + return R::WrongPhase; + } + if self.stream_highwater == Some(RequestSequence(u64::MAX)) { + return R::SequenceExhausted; + } + if r.sequence.0 == 0 || self.stream_highwater.is_some_and(|s| r.sequence <= s) { + return R::Replay; + } + self.stream_highwater = Some(r.sequence); + let u = u64::from(r.time_uncertainty_microseconds); + let t = r.issued_at.as_microseconds(); + if u > self.profile.flight().time_error_bound_us || t.checked_add(u).is_none_or(|t| t > now) + { + return R::InvalidTime; + } + let Some(end) = t + .checked_sub(u) + .and_then(|t| t.checked_add(self.profile.timing().offboard_timeout_microseconds)) + else { + return R::InvalidTime; + }; + if now >= end { + return R::Expired; + } + if !valid_setpoint(r.setpoint) { + return R::InvalidValue; + } + self.setpoint = Some(r.setpoint); + self.stream_expiry = Some(end); + R::None + } + /// Service the fixed control grid. Only due calls advance navigation or + /// integrate control. Early calls supervise cached health and phase deadlines. + /// Actual airborne control gaps are checked before navigation can refresh. + pub fn tick( + &mut self, + now: Timestamp, + out: &mut O, + ) -> Result<(), EndpointError> { + self.begin_call(); + // Inner fault paths have already latched termination and attempted + // inhibition. Only a bare error from a nonterminal path needs cleanup. + let result = match self.tick_inner(now, out) { + Err(EndpointError { + primary: EndpointFailure::Control, + inhibition: None, + }) if self.phase != VehiclePhase::Terminated => { + Err(self.fault(TerminalReason::Numerical, EndpointFailure::Control, out)) + } + result => result, + }; + self.finish_call(result, out) + } + fn tick_inner( + &mut self, + now: Timestamp, + out: &mut O, + ) -> Result<(), EndpointError> { + let now = self.clock(now, out)?; + if self.phase == VehiclePhase::Terminated { + return self.publish(false, [0.; 4], out); + } + // Fresh queued observations cannot rescue an expired actual control gap. + self.check_control_gap(now, out)?; + if self.next_control_due.is_some_and(|due| now < due) { + return self.supervise_phase(now, out); + } + self.navigation + .tick(time(now)) + .map_err(|e| self.fault(navigation_reason(e), EndpointFailure::Navigation(e), out))?; + let period = self.profile.flight().control_period_us; + // Immediate initial inhibition is allowed off-grid. Subsequent releases + // use the zero-origin endpoint grid, as TIM2 does, not actual wake + P. + let due = self.next_control_due.unwrap_or(now - now % period); + let next = now + .checked_add(period - ((now - due) % period)) + .ok_or_else(|| { + self.fault( + TerminalReason::ClockRegression, + EndpointFailure::ClockRegression, + out, + ) + })?; + let previous_phase = self.phase; + self.supervise_phase(now, out)?; + let dt = self.control_at.map_or(0.01, |t| (now - t) as f32 * 1e-6); + self.control_at = Some(now); + self.next_control_due = Some(next); + if previous_phase != VehiclePhase::Disarmed && self.phase == VehiclePhase::Disarmed { + // Phase supervision already completed the stop and its inhibition. + return Ok(()); + } + self.update_ground(now); + if self.phase == VehiclePhase::PreArm { + if self.ground != GroundState::Grounded { + return self.stop( + VehiclePhase::Disarmed, + CommandReason::MissingCapability, + out, + ); + } + let inputs = self.inputs(now); + self.safety + .evaluate_and_commit(time(now), inputs, || Ok::<(), ()>(())); + if self.safety.state() == SafetyState::Armed { + self.fallback = false; + self.change( + VehiclePhase::ArmedIdle, + now, + Some(now.saturating_add(10_000_000)), + ); + self.finish_pending(CommandLifecycle::Completed, CommandReason::None); + } + } + if !self.airborne() { + return self.publish(false, [0.; 4], out); + } + let e = self.navigation.estimate().ok_or(EndpointFailure::Control)?; + let mut velocity_mode = false; + let mut vertical_only = false; + let mut yaw = None; + let trajectory = self.trajectory.as_mut().ok_or(EndpointFailure::Control)?; + let reference = if matches!( + self.phase, + VehiclePhase::Land | VehiclePhase::TouchdownConfirm + ) { + vertical_only = true; + let upper_clearance = e + .ground_clearance + .and_then(|c| c.upper_at(time(now), &e, self.profile.contact_geometry())); + let near = upper_clearance.is_some_and(|u| u <= 0.05); + let contact_cap = 0.4 * self.profile.thrust().mass_kg * GRAVITY_MPS2 + / self.profile.thrust().maximum_collective_thrust_newtons; + let target = if near { contact_cap } else { 1. }; + let step = (1. - contact_cap) * dt / 0.5; + self.cap += (target - self.cap).clamp(-step, step); + // Braking 0.5 -> 0.2 m/s at 1.5 m/s² consumes 0.07 m before + // range age, output delay and motor response. Flare before contact. + let descent = if upper_clearance.is_some_and(|u| u <= 0.30) { + 0.2 + } else { + 0.5 + }; + let mut r = trajectory + .update_velocity(e.position.vector(), Vector3::new(0., 0., descent), dt) + .map_err(|_| EndpointFailure::Control)?; + r.position = Vector3::new(self.target.x(), self.target.y(), r.position.z()); + r + } else if let Some(setpoint) = self.setpoint { + match setpoint { + LocalSetpoint::PositionYaw { + position, + yaw_radians, + } => { + yaw = Some(yaw_radians); + trajectory + .set_goal(position.vector()) + .map_err(|_| EndpointFailure::Control)?; + trajectory + .update(dt) + .map_err(|_| EndpointFailure::Control)? + } + LocalSetpoint::VelocityYaw { + velocity, + yaw_radians, + } => { + yaw = Some(yaw_radians); + velocity_mode = true; + trajectory + .update_velocity(e.position.vector(), velocity.vector(), dt) + .map_err(|_| EndpointFailure::Control)? + } + } + } else { + trajectory + .update(dt) + .map_err(|_| EndpointFailure::Control)? + }; + let axes = if velocity_mode { + [false; 3] + } else { + [true, true, !vertical_only] + }; + let motors = self + .controller + .update(e, reference, dt, yaw, axes, self.cap) + .map_err(|_| self.fault(TerminalReason::Numerical, EndpointFailure::Control, out))?; + self.last_collective = motors.iter().sum::() + * self.profile.thrust().maximum_collective_thrust_newtons + / 4.; + if matches!( + self.phase, + VehiclePhase::Land | VehiclePhase::TouchdownConfirm + ) && self.ground == GroundState::Grounded + { + return self.stop(VehiclePhase::Disarmed, CommandReason::None, out); + } + self.publish(true, motors, out)?; + if matches!( + self.phase, + VehiclePhase::Land | VehiclePhase::TouchdownConfirm + ) { + self.descent_attempted = true; + } + if self.phase == VehiclePhase::Takeoff + || (self.phase == VehiclePhase::Hold + && self.deadline.is_some() + && !self.mission_active()) + { + let error = self.target.sub(e.position.vector()); + let reference_settled = self + .trajectory + .as_ref() + .is_some_and(|trajectory| trajectory.is_complete(0.05, 0.05)); + let settled = reference_settled + && error.x().abs() <= 0.35 + && error.y().abs() <= 0.35 + && error.z().abs() <= 0.35 + && e.velocity.vector().norm() <= 0.2 + && self.controller.heading_aligned(e.attitude); + if settled { + let since = *self.settled_since.get_or_insert(now); + if now - since >= 2_000_000 { + if self.mission_active() { + self.change( + VehiclePhase::Hold, + now, + Some(now.saturating_add(30_000_000)), + ); + } else { + self.change(VehiclePhase::Hold, now, None); + self.finish_pending(CommandLifecycle::Completed, CommandReason::None); + } + } + } else { + self.settled_since = None; + } + } + Ok(()) + } + fn update_ground(&mut self, now: u64) { + let Some(e) = self.navigation.estimate() else { + self.ground = GroundState::Unknown; + self.ground_since = None; + return; + }; + let speed = e.velocity.vector().norm(); + let upper = e + .ground_clearance + .and_then(|c| c.upper_at(time(now), &e, self.profile.contact_geometry())); + let stationary = speed <= 0.10 + && e.angular_rate.vector().norm() <= 0.10 + && upper.is_some_and(|u| u <= 0.05) + && e.capabilities.flight_ready(); + let landing = matches!( + self.phase, + VehiclePhase::Land | VehiclePhase::TouchdownConfirm + ); + let supported = if landing { + self.last_collective < 0.5 * self.profile.thrust().mass_kg * GRAVITY_MPS2 + && self.descent_attempted + } else { + matches!( + self.phase, + VehiclePhase::Disarmed | VehiclePhase::PreArm | VehiclePhase::ArmedIdle + ) && self.last_collective == 0. + && self.output_healthy + }; + if stationary && supported { + let since = *self.ground_since.get_or_insert(now); + self.ground = if now - since >= 1_000_000 { + GroundState::Grounded + } else { + GroundState::ContactCandidate + }; + } else { + self.ground_since = None; + self.ground = if upper.is_none() { + GroundState::Unknown + } else { + GroundState::Airborne + }; + } + if landing { + let phase = if matches!( + self.ground, + GroundState::ContactCandidate | GroundState::Grounded + ) { + VehiclePhase::TouchdownConfirm + } else { + VehiclePhase::Land + }; + if self.phase != phase { + self.phase = phase; + self.since = now; + } + } + } +} +fn navigation_reason(failure: NavigationFailure) -> TerminalReason { + match failure { + NavigationFailure::ImuGap => TerminalReason::ImuLost, + NavigationFailure::QueueOverrun => TerminalReason::IngressOverrun, + NavigationFailure::Numerical => TerminalReason::Numerical, + NavigationFailure::WorkBudget | NavigationFailure::RequiredCapabilityLost => { + TerminalReason::NavigationLost + } + } +} + +fn valid_setpoint(s: LocalSetpoint) -> bool { + let (v, yaw, limit) = match s { + LocalSetpoint::PositionYaw { + position, + yaw_radians, + } => (position.vector(), yaw_radians, 20.), + LocalSetpoint::VelocityYaw { + velocity, + yaw_radians, + } => { + if velocity.vector().z().abs() > 1.5 { + return false; + } + (velocity.vector(), yaw_radians, 3.) + } + }; + v.is_finite() && v.norm() <= limit && yaw.is_finite() && yaw.abs() <= core::f32::consts::PI +} + +#[cfg(test)] +mod tests { + use super::*; + use kaptan_config::ConfigModel; + mod support { + use crate as kaptan_flight_runtime; + use std::{vec, vec::Vec}; + include!("../tests/support/mod.rs"); + } + use support::{Recording, armed, profile_fixture, ready, request}; + + const MISSION: FlightCommand = FlightCommand::TakeoffHoverLand { + height_above_launch_metres: 2., + }; + + #[test] + fn mission_requires_a_fresh_armed_start() { + let (mut ep, mut out, now) = ready(); + assert_eq!( + ep.command(request(&ep, 1, MISSION, now), time(now), &mut out) + .unwrap() + .reason, + CommandReason::WrongArm + ); + assert_eq!(ep.phase, VehiclePhase::Disarmed); + let (mut ep, mut out, now) = armed(); + for (sequence, height) in [(2, f32::NAN), (3, 0.19), (4, 5.01)] { + let command = FlightCommand::TakeoffHoverLand { + height_above_launch_metres: height, + }; + assert_eq!( + ep.command(request(&ep, sequence, command, now), time(now), &mut out) + .unwrap() + .reason, + CommandReason::InvalidValue + ); + } + let stale = request(&ep, 5, MISSION, now - 250_001); + assert_eq!( + ep.command(stale, time(now), &mut out).unwrap().reason, + CommandReason::Expired + ); + let start = request(&ep, 6, MISSION, now); + assert_eq!( + ep.command(start, time(now), &mut out).unwrap().lifecycle, + CommandLifecycle::Accepted + ); + for (request, reason) in [ + (start, CommandReason::Replay), + (request(&ep, 7, MISSION, now), CommandReason::WrongPhase), + ] { + assert_eq!( + ep.command(request, time(now), &mut out).unwrap().reason, + reason + ); + } + } + + #[test] + fn supervisory_commands_cancel_the_mission() { + for (phase, command) in [ + (VehiclePhase::Takeoff, FlightCommand::Hold), + (VehiclePhase::Takeoff, FlightCommand::Terminate), + (VehiclePhase::Hold, FlightCommand::Land), + (VehiclePhase::Land, FlightCommand::Land), + (VehiclePhase::TouchdownConfirm, FlightCommand::Land), + ] { + let (mut ep, mut out, now) = armed(); + ep.command(request(&ep, 2, MISSION, now), time(now), &mut out) + .unwrap(); + // Isolate command takeover in each phase without repeating the + // sensor-trajectory lifecycle test. + ep.phase = phase; + let deadline = ep.deadline; + ep.command(request(&ep, 3, command, now), time(now), &mut out) + .unwrap(); + let cancelled = ep.command_outcomes()[0].unwrap().status; + assert_eq!(cancelled.kind, CommandKind::TakeoffHoverLand); + assert_eq!(cancelled.lifecycle, CommandLifecycle::Cancelled); + assert_eq!(cancelled.reason, CommandReason::Superseded); + assert!(!ep.mission_active()); + if matches!(phase, VehiclePhase::Land | VehiclePhase::TouchdownConfirm) { + assert_eq!(ep.deadline, deadline); + assert_eq!(ep.active.unwrap().kind, CommandKind::Land); + assert_eq!( + ep.command(request(&ep, 4, command, now), time(now), &mut out) + .unwrap() + .reason, + CommandReason::AlreadyActive + ); + assert_eq!(ep.deadline, deadline); + } + } + } + + #[test] + fn mission_failure_preserves_landing_reserve() { + for reserve in [false, true] { + let (mut ep, mut out, now) = armed(); + ep.command(request(&ep, 2, MISSION, now), time(now), &mut out) + .unwrap(); + if reserve { + ep.phase = VehiclePhase::Hold; + ep.flight_deadline = Some(now + LANDING_BUDGET_US); + } else { + ep.deadline = Some(now); + } + ep.supervise(time(now), &mut out).unwrap(); + assert_eq!(ep.phase, VehiclePhase::Land); + assert_eq!(ep.deadline, Some(now + LANDING_BUDGET_US)); + let failure = ep.command_outcomes()[0].unwrap().status; + assert_eq!(failure.kind, CommandKind::TakeoffHoverLand); + assert_eq!(failure.lifecycle, CommandLifecycle::Failed); + assert_eq!(failure.reason, CommandReason::PhaseDeadline); + assert!(!ep.mission_active()); + } + } + + #[test] + fn failed_stop_never_completes_a_command() { + for command in [ + FlightCommand::GroundDisarm, + FlightCommand::Terminate, + MISSION, + ] { + for exhaust in [false, true] { + let (mut ep, mut out, now) = armed(); + if command == MISSION { + ep.command(request(&ep, 2, command, now), time(now), &mut out) + .unwrap(); + } + if exhaust { + ep.output_sequence = u64::MAX - 1; + } else { + out.fail_inhibit = true; + } + let error = if command == MISSION { + ep.stop(VehiclePhase::Disarmed, CommandReason::None, &mut out) + .unwrap_err() + } else { + ep.command(request(&ep, 2, command, now), time(now), &mut out) + .unwrap_err() + }; + assert_eq!( + error.primary, + if exhaust { + EndpointFailure::OutputSequenceExhausted + } else { + EndpointFailure::Output("inhibit") + } + ); + let failure = ep.command_outcomes()[0].unwrap().status; + assert_eq!(failure.kind, command.kind()); + assert_eq!(failure.lifecycle, CommandLifecycle::Failed); + assert_eq!(ep.phase, VehiclePhase::Terminated); + assert!(!out.outputs.last().unwrap().permitted()); + if exhaust && command == FlightCommand::Terminate { + let outputs = out.outputs.len(); + assert_eq!( + ep.command(request(&ep, 3, command, now), time(now), &mut out) + .unwrap_err() + .primary, + EndpointFailure::OutputSequenceExhausted + ); + let failure = ep.command_outcomes()[0].unwrap().status; + assert_eq!(failure.sequence, RequestSequence(3)); + assert_eq!(failure.lifecycle, CommandLifecycle::Failed); + assert_eq!(ep.active, None); + assert_eq!(out.outputs.len(), outputs); + } + } + } + } + + fn cold() -> FlightEndpoint { + FlightEndpoint::new( + profile_fixture::candidate() + .validate(&profile_fixture::capabilities()) + .unwrap(), + RunBinding { + session: TransportSessionId(1), + owner: CommandOwner(1), + }, + ) + .unwrap() + } + fn admit_at(ep: &mut FlightEndpoint, out: &mut Recording, coverage_end: u64, processing: u64) { + for mut record in support::records(coverage_end + 1_000) { + let context = match &mut record { + SensorRecord::Imu(v) => &mut v.context, + SensorRecord::LocalPositionVelocity(v) => &mut v.context, + SensorRecord::Magnetic(v) => &mut v.context, + SensorRecord::Pressure(v) => &mut v.context, + SensorRecord::Range(v) => &mut v.context, + }; + // Real acquisition cadence is independent of this test's jittered + // delivery/service schedule. Delayed GNSS keeps its original capture. + context.receipt_time = time(processing); + assert_eq!( + ep.observe(ep.status().epoch, record, time(processing), out) + .unwrap(), + ObservationAdmission::Queued + ); + } + } + fn takeoff(ep: &mut FlightEndpoint, out: &mut Recording, now: u64) { + ep.command( + request( + ep, + 2, + FlightCommand::Takeoff { + height_above_launch_metres: 2.0, + }, + now, + ), + time(now), + out, + ) + .unwrap(); + } + + #[test] + fn nominal_phase_releases_jittered_ticks_with_actual_elapsed_integration() { + let (mut ep, mut out, now) = armed(); + takeoff(&mut ep, &mut out, now); + let base = now - now % 10_000; + admit_at(&mut ep, &mut out, base + 10_000, base + 10_100); + ep.tick(time(base + 10_100), &mut out).unwrap(); + let first = *out.outputs.last().unwrap(); + assert!(first.permitted()); + assert_eq!(first.produced_at(), time(base + 10_100)); + assert_eq!(ep.next_control_due, Some(base + 20_000)); + let reference = ep.trajectory.as_ref().unwrap().reference(); + let estimate = ep.status().estimate; + for actual in [10_100, 19_999] { + ep.supervise(time(base + actual), &mut out).unwrap(); + ep.tick(time(base + actual), &mut out).unwrap(); + assert_eq!(*out.outputs.last().unwrap(), first); + assert_eq!(ep.trajectory.as_ref().unwrap().reference(), reference); + assert_eq!(ep.status().estimate, estimate); + } + admit_at(&mut ep, &mut out, base + 20_000, base + 20_000); + let cached = ep.status().estimate; + ep.supervise(time(base + 20_000), &mut out).unwrap(); + assert_eq!(ep.status().estimate, cached); + assert_eq!(ep.next_control_due, Some(base + 20_000)); + assert_eq!(*out.outputs.last().unwrap(), first); + ep.tick(time(base + 20_000), &mut out).unwrap(); + let second = *out.outputs.last().unwrap(); + assert!(second.permitted()); + assert_eq!(second.sequence(), first.sequence() + 1); + assert_eq!(second.produced_at(), time(base + 20_000)); + assert_eq!(second.valid_until(), time(base + 40_000)); + assert!(first.valid_until() > second.produced_at()); + // Independently integrate the fixture's 1.5 m/s^2 vertical reference + // acceleration for 9900 us. A substituted 10 ms dt misses by 0.00015 m/s. + let expected = f64::from(reference.velocity.z()) - 1.5 * 0.0099; + let actual = f64::from(ep.trajectory.as_ref().unwrap().reference().velocity.z()); + assert!((actual - expected).abs() < 1e-8); + assert_eq!(ep.control_at, Some(base + 20_000)); + assert_eq!(ep.next_control_due, Some(base + 30_000)); + + ep.command( + request(&ep, 3, FlightCommand::Hold, base + 20_001), + time(base + 20_001), + &mut out, + ) + .unwrap(); + let origin = ep.status().estimate.unwrap().origin.id; + let initial = LocalSetpoint::PositionYaw { + position: PositionNed::metres(0.0, 0.0, -2.0), + yaw_radians: 0.0, + }; + ep.command( + request( + &ep, + 4, + FlightCommand::EnterOffboard { origin, initial }, + base + 20_002, + ), + time(base + 20_002), + &mut out, + ) + .unwrap(); + ep.setpoint( + SetpointRequest { + epoch: ep.status().epoch, + owner: ep.status().owner, + arm: ep.status().arm.unwrap(), + origin, + offboard: RequestSequence(4), + sequence: RequestSequence(1), + issued_at: time(base + 20_003), + time_uncertainty_microseconds: 0, + setpoint: initial, + }, + time(base + 20_003), + &mut out, + ) + .unwrap(); + ep.supervise(time(base + 20_004), &mut out).unwrap(); + assert_eq!(ep.phase, VehiclePhase::Offboard); + assert_eq!(ep.control_at, Some(base + 20_000)); + assert_eq!(ep.next_control_due, Some(base + 30_000)); + assert_eq!(*out.outputs.last().unwrap(), second); + } + + #[test] + fn due_control_can_predict_without_a_new_imu_window() { + let (mut ep, mut out, now) = armed(); + takeoff(&mut ep, &mut out, now); + let coverage = ep.admitted_imu_coverage().unwrap(); + let due = now - now % 10_000 + 10_000; + ep.supervise(time(due), &mut out).unwrap(); + ep.tick(time(due), &mut out).unwrap(); + assert!(out.outputs.last().unwrap().permitted()); + assert_eq!(out.outputs.last().unwrap().produced_at(), time(due)); + assert_eq!(ep.admitted_imu_coverage(), Some(coverage)); + let estimate = ep.status().estimate.unwrap(); + assert_eq!(estimate.at, time(due)); + assert_eq!(estimate.last_imu_at, coverage.end); + assert!(estimate.capabilities.flight_ready()); + } + + #[test] + fn early_ticks_retain_prediction_and_queued_input_until_service_or_cached_expiry() { + for expires_early in [false, true] { + let (mut ep, mut out, now) = armed(); + takeoff(&mut ep, &mut out, now); + let base = now - now % 10_000; + let mut sample = support::imu(base + 10_000); + sample.context.receipt_time = time(base + 12_000); + sample.context.time_uncertainty_microseconds = if expires_early { 2_000 } else { 0 }; + ep.observe( + ep.status().epoch, + SensorRecord::Imu(sample), + time(base + 12_000), + &mut out, + ) + .unwrap(); + ep.tick(time(base + 12_000), &mut out).unwrap(); + ep.tick(time(base + 20_000), &mut out).unwrap(); + let cached = ep.status().estimate; + let output = *out.outputs.last().unwrap(); + // This burst arrives after the last release and before the next. + admit_at(&mut ep, &mut out, base + 20_000, base + 25_000); + assert_eq!(ep.admitted_imu_coverage().unwrap().end, time(base + 20_000)); + for early in [25_000, 25_000, 27_999] { + ep.tick(time(base + early), &mut out).unwrap(); + assert_eq!(ep.status().estimate, cached); + assert_eq!(*out.outputs.last().unwrap(), output); + } + if expires_early { + let count = out.outputs.len(); + let error = ep.tick(time(base + 28_000), &mut out).unwrap_err(); + assert_eq!( + error.primary, + EndpointFailure::Navigation(NavigationFailure::RequiredCapabilityLost) + ); + assert_eq!(ep.status().estimate, cached); + assert_eq!(out.outputs.len(), count + 1); + assert!(!out.outputs.last().unwrap().permitted()); + } else { + ep.tick(time(base + 30_000), &mut out).unwrap(); + let current = ep.status().estimate.unwrap(); + assert_eq!(current.at, time(base + 30_000)); + assert_eq!(current.last_imu_at, time(base + 20_000)); + assert!(current.fused_through > cached.unwrap().fused_through); + assert_eq!( + out.outputs.last().unwrap().sequence(), + output.sequence() + 1 + ); + } + } + } + + #[test] + fn initialization_warmup_becomes_visible_only_on_control_service_cadence() { + let mut ep = cold(); + let mut out = Recording::default(); + ep.tick(time(0), &mut out).unwrap(); + for source_end in (10_000..=2_100_000).step_by(10_000) { + ep.tick(time(source_end), &mut out).unwrap(); + for mut record in support::records(source_end + 1_000) { + let context = match &mut record { + SensorRecord::Imu(v) => &mut v.context, + SensorRecord::LocalPositionVelocity(v) => &mut v.context, + SensorRecord::Magnetic(v) => &mut v.context, + SensorRecord::Pressure(v) => &mut v.context, + SensorRecord::Range(v) => &mut v.context, + }; + // A fixed +1 ms clock offset puts acquisition boundaries off + // the control grid. Receipt is separately 1 ms after capture. + context.measurement_time = time(context.measurement_time.as_microseconds() + 1_000); + context.receipt_time = time(source_end + 2_000); + let admission = ep + .observe( + ep.status().epoch, + record, + time(source_end + 2_000), + &mut out, + ) + .unwrap(); + if source_end == 2_030_000 && record.kind() == 2 { + // This delayed fix coincides with the already committed + // initialization boundary. It must not be fused backwards. + assert_eq!(record.context().measurement_time, time(2_001_000)); + assert_eq!( + admission, + ObservationAdmission::Rejected(ObservationRejection::BehindFusionHorizon) + ); + } else { + assert_eq!(admission, ObservationAdmission::Queued); + } + } + let estimate = ep.status().estimate; + ep.tick(time(source_end + 2_000), &mut out).unwrap(); + assert_eq!(ep.status().estimate, estimate); + } + assert_eq!(ep.status().estimate, None); + ep.tick(time(2_110_000), &mut out).unwrap(); + let estimate = ep.status().estimate.unwrap(); + assert_eq!(estimate.origin.established_at, time(2_001_000)); + assert_eq!(estimate.at, time(2_110_000)); + } + + #[test] + fn prearm_health_stop_publishes_one_inhibition_per_call() { + for fail in [false, true] { + let (mut ep, mut out, now) = ready(); + ep.command( + request(&ep, 1, FlightCommand::Arm, now), + time(now), + &mut out, + ) + .unwrap(); + let count = out.outputs.len(); + out.fail_inhibit = fail; + let result = ep.tick(time(now + 25_000), &mut out); + if fail { + assert_eq!( + result.unwrap_err().primary, + EndpointFailure::Output("inhibit") + ); + assert_eq!(ep.phase, VehiclePhase::Terminated); + } else { + result.unwrap(); + assert_eq!(ep.phase, VehiclePhase::Disarmed); + // A successful stop consumed this service slot, too. + ep.tick(time(now + 25_000), &mut out).unwrap(); + } + assert_eq!(out.outputs.len(), count + 1); + assert!(!out.outputs.last().unwrap().permitted()); + assert_eq!( + ep.status().last_terminal_command.unwrap().lifecycle, + CommandLifecycle::Failed + ); + assert_eq!( + ep.status().last_terminal_command.unwrap().reason, + CommandReason::MissingCapability + ); + out.fail_inhibit = false; + ep.tick(time(now + 35_000), &mut out).unwrap(); + assert_eq!(out.outputs.len(), count + 2); + } + } + + #[test] + fn numerical_rejection_inhibits_once_and_preserves_failed_inhibition() { + for inner_fault in [false, true] { + for fail in [false, true] { + let (mut ep, mut out, now) = armed(); + takeoff(&mut ep, &mut out, now); + if inner_fault { + // Fault injection at the controller's checked numeric input. + // This path already latches and inhibits inside tick_inner. + ep.setpoint = Some(LocalSetpoint::PositionYaw { + position: ep.status().estimate.unwrap().position, + yaw_radians: f32::NAN, + }); + } else { + // The plain Control error path is cleaned up by tick's wrapper. + ep.trajectory = None; + } + let count = out.outputs.len(); + out.fail_inhibit = fail; + let error = ep.tick(time(now + 9_000), &mut out).unwrap_err(); + assert_eq!(error.primary, EndpointFailure::Control); + assert_eq!(error.inhibition, fail.then_some("inhibit")); + assert_eq!(ep.terminal, Some(TerminalReason::Numerical)); + assert_eq!(out.outputs.len(), count + 1); + assert!(!out.outputs.last().unwrap().permitted()); + assert_eq!( + ep.command_outcomes()[0].unwrap().status.lifecycle, + CommandLifecycle::Failed + ); + out.fail_inhibit = false; + ep.tick(time(now + 9_001), &mut out).unwrap(); + assert_eq!(out.outputs.len(), count + 2); + } + } + } + + #[test] + fn off_grid_start_and_epoch_reset_keep_the_continuous_zero_origin_grid() { + let mut ep = cold(); + let mut out = Recording::default(); + assert_eq!(ep.admitted_imu_coverage(), None); + ep.supervise(time(9_550), &mut out).unwrap(); + assert!(out.outputs.is_empty()); + assert_eq!(ep.control_at, None); + assert_eq!(ep.next_control_due, None); + ep.tick(time(9_550), &mut out).unwrap(); + ep.tick(time(9_999), &mut out).unwrap(); + assert_eq!(out.outputs.len(), 1); + ep.tick(time(10_000), &mut out).unwrap(); + assert_eq!(out.outputs.len(), 2); + assert_eq!(ep.next_control_due, Some(20_000)); + let record = support::imu(20_000); + ep.observe( + ep.status().epoch, + SensorRecord::Imu(record), + time(20_000), + &mut out, + ) + .unwrap(); + assert_eq!( + ep.admitted_imu_coverage(), + Some(ImuCoverage { + end: time(20_000), + acquisition_duration_us: 10_000, + time_uncertainty_microseconds: 0 + }) + ); + assert_eq!(ep.status().estimate, None); + let epoch = ep + .begin_epoch(CommandOwner(2), time(21_050), &mut out) + .unwrap(); + assert_eq!(ep.admitted_imu_coverage(), None); + assert_eq!(ep.status().at, time(21_050)); + assert_eq!(ep.control_at, None); + assert_eq!(ep.next_control_due, None); + let sequence = out.outputs.last().unwrap().sequence(); + ep.tick(time(25_050), &mut out).unwrap(); + assert_eq!(ep.next_control_due, Some(30_000)); + ep.tick(time(30_000), &mut out).unwrap(); + assert_eq!(out.outputs.last().unwrap().epoch(), epoch); + assert_eq!(out.outputs.last().unwrap().sequence(), sequence + 2); + assert_eq!(out.outputs.last().unwrap().produced_at(), time(30_000)); + } + + #[test] + fn missed_slots_are_coalesced_once_and_future_grid_overflow_fails_closed() { + let mut ep = cold(); + let mut out = Recording::default(); + ep.tick(time(123), &mut out).unwrap(); + ep.tick(time(1_000_000_003), &mut out).unwrap(); + assert_eq!(out.outputs.len(), 2); + assert_eq!(ep.next_control_due, Some(1_000_010_000)); + ep.tick(time(1_000_009_999), &mut out).unwrap(); + assert_eq!(out.outputs.len(), 2); + ep.tick(time(1_000_010_000), &mut out).unwrap(); + ep.tick(time(1_000_010_000), &mut out).unwrap(); + assert_eq!(out.outputs.len(), 3); + let previous = ep.control_at; + ep.supervise(time(u64::MAX - 1), &mut out).unwrap(); + assert_eq!(out.outputs.len(), 3); + assert_eq!(ep.control_at, previous); + out.fail_inhibit = true; + let error = ep.tick(time(u64::MAX - 1), &mut out).unwrap_err(); + assert_eq!(error.primary, EndpointFailure::ClockRegression); + assert_eq!(error.inhibition, Some("inhibit")); + assert_eq!(ep.control_at, previous); + assert_eq!(ep.phase, VehiclePhase::Terminated); + assert_eq!(out.outputs.len(), 4); + assert!(!out.outputs.last().unwrap().permitted()); + assert_eq!( + out.outputs.last().unwrap().produced_at(), + time(u64::MAX - 1) + ); + } + + #[test] + fn supervisor_does_not_complete_arm_dwell_or_refresh_cached_health_from_queued_input() { + let (mut ep, mut out, now) = ready(); + ep.command( + request(&ep, 1, FlightCommand::Arm, now), + time(now), + &mut out, + ) + .unwrap(); + for t in (now + 10_000..=now + 490_000).step_by(10_000) { + support::feed(&mut ep, &mut out, t); + } + let count = out.outputs.len(); + let cached = ep.status().estimate; + ep.supervise(time(now + 500_000), &mut out).unwrap(); + assert_eq!(ep.phase, VehiclePhase::PreArm); + assert_eq!(ep.status().estimate, cached); + assert_eq!(out.outputs.len(), count); + assert_eq!(ep.command_outcomes(), [None; 2]); + support::feed(&mut ep, &mut out, now + 500_000); + assert_eq!(ep.phase, VehiclePhase::ArmedIdle); + + let (mut ep, mut out, now) = ready(); + ep.command( + request(&ep, 1, FlightCommand::Arm, now), + time(now), + &mut out, + ) + .unwrap(); + let cached = ep.status().estimate; + let due = ep.next_control_due; + let base = now - now % 10_000; + admit_at(&mut ep, &mut out, base + 10_000, base + 11_000); + admit_at(&mut ep, &mut out, base + 20_000, base + 21_000); + assert_eq!(ep.admitted_imu_coverage().unwrap().end, time(base + 20_000)); + ep.supervise(time(base + 21_000), &mut out).unwrap(); + assert_eq!(ep.status().estimate, cached); + assert_eq!(ep.next_control_due, due); + assert_eq!(ep.phase, VehiclePhase::Disarmed); + assert!(!out.outputs.last().unwrap().permitted()); + let result = ep.command_outcomes()[0].unwrap(); + assert_eq!(result.status.lifecycle, CommandLifecycle::Failed); + assert_eq!(result.status.reason, CommandReason::MissingCapability); + assert!(result.published); + } + + #[test] + fn fresh_admitted_input_cannot_rescue_the_actual_airborne_control_gap() { + let (mut ep, mut out, now) = armed(); + takeoff(&mut ep, &mut out, now); + let base = now - now % 10_000; + admit_at(&mut ep, &mut out, base + 10_000, now + 20_000); + admit_at(&mut ep, &mut out, base + 20_000, now + 20_000); + let outputs = out.outputs.len(); + ep.tick(time(now + 20_000), &mut out).unwrap(); + assert_eq!(out.outputs.len(), outputs + 1); + assert!(out.outputs.last().unwrap().permitted()); + assert_eq!(ep.control_at, Some(now + 20_000)); + for supervision in [false, true] { + let (mut ep, mut out, now) = armed(); + takeoff(&mut ep, &mut out, now); + let cached = ep.status().estimate; + let due = ep.next_control_due; + let base = now - now % 10_000; + admit_at(&mut ep, &mut out, base + 10_000, now + 20_001); + admit_at(&mut ep, &mut out, base + 20_000, now + 20_001); + assert_eq!(ep.admitted_imu_coverage().unwrap().end, time(base + 20_000)); + out.fail_inhibit = true; + out.fail_notification = true; + let error = if supervision { + ep.supervise(time(now + 20_001), &mut out) + } else { + ep.tick(time(now + 20_001), &mut out) + } + .unwrap_err(); + assert_eq!(error.primary, EndpointFailure::ControlDeadline); + assert_eq!(error.inhibition, Some("inhibit")); + assert_eq!(ep.status().estimate, cached); + assert_eq!(ep.next_control_due, due); + assert_eq!(ep.control_at, Some(now)); + assert_eq!(ep.terminal, Some(TerminalReason::ControlDeadline)); + assert!(!out.outputs.last().unwrap().permitted()); + let outcome = ep.command_outcomes()[0].unwrap(); + assert_eq!(outcome.status.sequence, RequestSequence(2)); + assert_eq!(outcome.status.lifecycle, CommandLifecycle::Failed); + assert!(!outcome.published); + assert_eq!(out.commands.last(), Some(&outcome.status)); + } + } + + #[test] + fn supervisor_clock_fault_and_terminal_retry_preserve_outcomes_without_control() { + let (mut ep, mut out, now) = armed(); + takeoff(&mut ep, &mut out, now); + let due = ep.next_control_due; + let error = ep.supervise(time(now - 1), &mut out).unwrap_err(); + assert_eq!(error.primary, EndpointFailure::ClockRegression); + assert_eq!(ep.phase, VehiclePhase::Terminated); + assert_eq!(ep.next_control_due, due); + assert!(ep.command_outcomes()[0].unwrap().published); + let last = *out.outputs.last().unwrap(); + assert!(!last.permitted()); + ep.supervise(time(now + 1), &mut out).unwrap(); + assert!(!out.outputs.last().unwrap().permitted()); + assert_eq!(out.outputs.last().unwrap().sequence(), last.sequence() + 1); + assert_eq!(ep.command_outcomes(), [None; 2]); + assert_eq!(ep.terminal, Some(TerminalReason::ClockRegression)); + assert_eq!(ep.control_at, Some(now)); + assert_eq!(ep.next_control_due, due); + } + struct Sink { + records: std::vec::Vec, + fail: bool, + } + impl FlightOutput for Sink { + type Error = &'static str; + fn actuator(&mut self, r: OutputRecord) -> Result<(), Self::Error> { + self.records.push(r); + if self.fail { Err("inhibit") } else { Ok(()) } + } + fn command_status(&mut self, _: CommandStatus) -> Result<(), Self::Error> { + Ok(()) + } + } + #[test] + fn output_exhaustion_uses_terminal_identity_once_and_cannot_begin_epoch() { + let profile = profile_fixture::candidate() + .validate(&profile_fixture::capabilities()) + .unwrap(); + let mut ep = FlightEndpoint::new( + profile, + RunBinding { + session: TransportSessionId(1), + owner: CommandOwner(1), + }, + ) + .unwrap(); + // Only the unreachable counter distance is seeded. Production methods + // perform the exhaustion, publication and subsequent rejection. + ep.output_sequence = u64::MAX - 1; + let mut sink = Sink { + records: std::vec::Vec::new(), + fail: true, + }; + let error = ep.tick(time(1), &mut sink).unwrap_err(); + assert_eq!(error.primary, EndpointFailure::OutputSequenceExhausted); + assert_eq!(error.inhibition, Some("inhibit")); + assert_eq!(sink.records.len(), 1); + assert_eq!(sink.records[0].sequence(), u64::MAX); + assert!(!sink.records[0].permitted()); + assert_eq!(ep.status().phase, VehiclePhase::Terminated); + assert_eq!( + ep.tick(time(2), &mut sink).unwrap_err().primary, + EndpointFailure::OutputSequenceExhausted + ); + assert_eq!( + ep.begin_epoch(CommandOwner(2), time(3), &mut sink) + .unwrap_err() + .primary, + EndpointFailure::NotDisarmed + ); + assert_eq!(sink.records.len(), 1); + } + #[test] + fn endpoint_state_is_bounded_without_a_heap_owner() { + let bytes = core::mem::size_of::(); + std::println!("FlightEndpoint host layout: {bytes} bytes"); + assert!(bytes <= 32 * 1024); + assert!(!core::mem::needs_drop::()); + } +} diff --git a/firmware/libs/kaptan-flight-runtime/src/lib.rs b/firmware/libs/kaptan-flight-runtime/src/lib.rs index feb68f7..dce9fee 100644 --- a/firmware/libs/kaptan-flight-runtime/src/lib.rs +++ b/firmware/libs/kaptan-flight-runtime/src/lib.rs @@ -1,148 +1,353 @@ -//! Central, transport-neutral flight-runtime composition. +//! Deterministic sensor-driven flight endpoint. The caller owns the clock and +//! borrows its bounded output sink for each effectful call. Local publication +//! never proves remote application. Consumers enforce their own absolute expiry. //! -//! This crate owns the one configuration manager and the bounded queue for the -//! image's MAVLink diagnostic sink. MAVLink adapters receive mutable borrows -//! for bounded dispatch or draining, but cannot own configuration lifecycle or -//! link state. - +//! Callbacks must return errors, not panic. If an unwind crosses this boundary, +//! discard the endpoint and callback owner. A halted CPU does not inhibit remote +//! outputs; independently enforced consumer expiry remains required. #![no_std] +#![forbid(unsafe_code)] #[cfg(test)] extern crate std; -use kaptan_config::{ConfigManager, ConfigStore, StartupError}; -use kaptan_flight_config::{CandidateProfile, VehicleCapabilities}; -use kaptan_log::{LogRecord, LogSinkQueue}; +mod controller; +mod execution; +pub use execution::FlightEndpoint; -/// Default bounded allocation for the MAVLink diagnostic sink. Error records -/// are partitioned from optional detail. -type RuntimeMavlinkLogQueue = LogSinkQueue<8, 16>; +/// Core-owned time reserved for an admitted landing and its completion. +pub const LANDING_BUDGET_US: u64 = 20_000_000; +pub use kaptan_navigation::{ + FlightEstimate, ImuCoverage, NavigationFailure, ObservationAdmission, ObservationRejection, + OriginId, SensorRecord, +}; +use kaptan_navigation::{PositionNed, SensorStatus, VelocityNed}; +use kaptan_time::Timestamp; -/// One boot-active vehicle runtime. It deliberately has no MAVLink dependency. -pub struct FlightRuntime { - configuration: ConfigManager, - mavlink_logs: RuntimeMavlinkLogQueue, +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct CommandOwner(pub u64); +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct TransportSessionId(pub u128); +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct RuntimeEpoch { + session: TransportSessionId, + generation: u64, } - -impl FlightRuntime -where - S: ConfigStore, -{ - /// Recovers the central manager, validates its boot-active snapshot, and - /// copies the restart-only MAVLink sink policy into its bounded queue. - pub fn recover( - factory: CandidateProfile, - capabilities: VehicleCapabilities, - store: S, - ) -> Result> { - let configuration = ConfigManager::recover(factory, capabilities, store)?; - let mavlink_logs = RuntimeMavlinkLogQueue::new(configuration.active().mavlink_logging()); - Ok(Self { - configuration, - mavlink_logs, - }) - } - - /// The sole central configuration manager. - pub const fn configuration(&self) -> &ConfigManager { - &self.configuration - } - - /// Composition-owned admission boundary for one preformatted event. - pub fn publish_log_event(&mut self, record: LogRecord) { - self.mavlink_logs.publish_record(record); - } - - /// Submits the next record and removes it only after transport admission. - pub fn submit_next_mavlink( - &mut self, - submit: impl FnOnce(&LogRecord) -> (kaptan_log::TransportAdmission, R), - ) -> Option { - self.mavlink_logs.submit_next(submit) - } -} - -#[cfg(test)] -mod tests { - use kaptan_config::{ConfigModel, ConfigStore, ConfigValue}; - use kaptan_log::TransportAdmission; - use std::sync::Mutex; - - use super::FlightRuntime; - - static LOG_TEST: Mutex<()> = Mutex::new(()); - - struct Store; - impl ConfigStore for Store { - type Error = (); - - fn load( - &mut self, - _: &kaptan_flight_config::CandidateProfile, - ) -> Result, Self::Error> { - Ok(None) - } - - fn persist( - &mut self, - _: &kaptan_flight_config::CandidateProfile, - ) -> Result<(), Self::Error> { - Ok(()) +impl RuntimeEpoch { + pub const fn from_parts(session: TransportSessionId, generation: u64) -> Self { + Self { + session, + generation, } } - - #[test] - fn runtime_uses_the_active_mavlink_policy_and_owns_one_sink_queue() { - let _guard = LOG_TEST.lock().unwrap(); - while kaptan_log::drain_one(|_| ()).is_some() {} - let mut runtime = FlightRuntime::recover( - kaptan_flight_config::CandidateProfile::factory(), - kaptan_flight_config::VehicleCapabilities::quad_x(), - Store, - ) - .expect("factory configuration validates"); - assert_eq!( - runtime.configuration().active().mavlink_logging(), - kaptan_log::LevelFilter::Info - ); - kaptan_log::info!("runtime ready"); - runtime.publish_log_event(kaptan_log::drain_one(|record| *record).unwrap()); - assert_eq!( - runtime.submit_next_mavlink(|record| { - assert_eq!(record.level(), kaptan_log::Level::Info); - (TransportAdmission::Accepted, ()) - }), - Some(()) - ); + pub const fn session(self) -> TransportSessionId { + self.session } - - #[test] - fn disabled_mavlink_sink_filters_every_level() { - let _guard = LOG_TEST.lock().unwrap(); - while kaptan_log::drain_one(|_| ()).is_some() {} - let mut factory = kaptan_flight_config::CandidateProfile::factory(); - let level_filter = kaptan_flight_config::CandidateProfile::descriptors() - .find(|descriptor| descriptor.display_key == "mavlink.logging.level_filter") - .expect("MAVLink log-level descriptor"); - factory - .set_field(level_filter.id, ConfigValue::U8(0)) - .unwrap(); - let mut runtime = FlightRuntime::recover( - factory, - kaptan_flight_config::VehicleCapabilities::quad_x(), - Store, - ) - .expect("disabled MAVLink sink is valid"); - - assert_eq!( - runtime.configuration().active().mavlink_logging(), - kaptan_log::LevelFilter::Off - ); - kaptan_log::error!("not routed to MAVLink"); - runtime.publish_log_event(kaptan_log::drain_one(|record| *record).unwrap()); - assert_eq!( - runtime.submit_next_mavlink(|_| (TransportAdmission::Accepted, ())), - None - ); + pub const fn generation(self) -> u64 { + self.generation } } +#[derive(Clone, Copy, Debug, Eq, PartialEq, Ord, PartialOrd)] +pub struct RequestSequence(pub u64); +impl RequestSequence { + pub const fn checked_next(self) -> Option { + match self.0.checked_add(1) { + Some(v) => Some(Self(v)), + None => None, + } + } +} +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct ArmId(pub u64); +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct RunBinding { + pub session: TransportSessionId, + pub owner: CommandOwner, +} +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct RequestContext { + pub epoch: RuntimeEpoch, + pub owner: CommandOwner, + pub sequence: RequestSequence, + pub issued_at: Timestamp, + pub expires_at: Timestamp, + pub time_uncertainty_microseconds: u32, + pub arm: Option, +} +#[derive(Clone, Copy, Debug, PartialEq)] +pub enum LocalSetpoint { + PositionYaw { + position: PositionNed, + yaw_radians: f32, + }, + VelocityYaw { + velocity: VelocityNed, + yaw_radians: f32, + }, +} +#[derive(Clone, Copy, Debug, PartialEq)] +pub enum FlightCommand { + Arm, + GroundDisarm, + Takeoff { + height_above_launch_metres: f32, + }, + Hold, + EnterOffboard { + origin: OriginId, + initial: LocalSetpoint, + }, + Land, + Terminate, + /// Take off, hold for 30 seconds after settling, then land and inhibit output. + TakeoffHoverLand { + height_above_launch_metres: f32, + }, +} +impl FlightCommand { + pub const fn kind(self) -> CommandKind { + match self { + Self::Arm => CommandKind::Arm, + Self::GroundDisarm => CommandKind::GroundDisarm, + Self::Takeoff { .. } => CommandKind::Takeoff, + Self::Hold => CommandKind::Hold, + Self::EnterOffboard { .. } => CommandKind::EnterOffboard, + Self::Land => CommandKind::Land, + Self::Terminate => CommandKind::Terminate, + Self::TakeoffHoverLand { .. } => CommandKind::TakeoffHoverLand, + } + } +} +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct CommandRequest { + pub context: RequestContext, + pub command: FlightCommand, +} +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct SetpointRequest { + pub epoch: RuntimeEpoch, + pub owner: CommandOwner, + pub arm: ArmId, + pub origin: OriginId, + pub offboard: RequestSequence, + pub sequence: RequestSequence, + pub issued_at: Timestamp, + pub time_uncertainty_microseconds: u32, + pub setpoint: LocalSetpoint, +} +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +#[repr(u8)] +pub enum CommandKind { + Arm = 1, + GroundDisarm = 2, + Takeoff = 3, + Hold = 4, + EnterOffboard = 5, + Land = 6, + Terminate = 7, + TakeoffHoverLand = 8, +} +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +#[repr(u8)] +pub enum CommandLifecycle { + Rejected, + Accepted, + Completed, + Cancelled, + Failed, +} +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +#[repr(u16)] +pub enum CommandReason { + None = 0, + WrongEpoch = 1, + WrongOwner = 2, + Replay = 3, + SequenceExhausted = 4, + InvalidTime = 5, + Expired = 6, + InvalidValue = 7, + NotReady = 8, + WrongPhase = 9, + WrongArm = 10, + WrongOrigin = 11, + WrongOffboard = 12, + MissingCapability = 13, + FallbackLatched = 14, + AlreadyActive = 15, + Superseded = 16, + PhaseDeadline = 17, + Terminated = 18, +} +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct CommandStatus { + pub epoch: RuntimeEpoch, + pub owner: CommandOwner, + pub sequence: RequestSequence, + pub kind: CommandKind, + pub lifecycle: CommandLifecycle, + pub at: Timestamp, + pub reason: CommandReason, + pub arm: Option, +} +/// A committed terminal transition from the most recent effectful call. +/// The adapter must reconcile these records even when that call returned Err. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct CommandOutcome { + pub status: CommandStatus, + pub published: bool, +} +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct SetpointStatus { + pub offboard: RequestSequence, + pub sequence: RequestSequence, + pub accepted: bool, + pub reason: CommandReason, + pub expires_at: Option, +} +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +#[repr(u8)] +pub enum VehiclePhase { + Disarmed, + PreArm, + ArmedIdle, + Takeoff, + Hold, + Offboard, + FallbackHold, + Land, + TouchdownConfirm, + Terminated, +} +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +#[repr(u8)] +pub enum GroundState { + Unknown, + Airborne, + ContactCandidate, + Grounded, +} +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +#[repr(u16)] +pub enum FallbackReason { + OffboardExpired = 1, +} +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +#[repr(u16)] +pub enum TerminalReason { + Requested = 1, + ClockRegression = 2, + SequenceExhausted = 3, + ImuLost = 4, + NavigationLost = 5, + Numerical = 6, + ControlDeadline = 7, + OutputFault = 8, + LandingDeadline = 9, + FlightDeadline = 10, + IngressOverrun = 11, +} +/// Output evidence issued by the runtime. Consumers cannot grant permission +/// by changing the record's private fields. +/// ```compile_fail,E0616 +/// use kaptan_flight_runtime::OutputRecord; +/// fn grant_permission(mut record: OutputRecord) { +/// record.permitted = true; +/// } +/// ``` +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct OutputRecord { + epoch: RuntimeEpoch, + arm: Option, + sequence: u64, + produced_at: Timestamp, + valid_until: Timestamp, + permitted: bool, + motors: [f32; 4], +} +impl OutputRecord { + pub const fn epoch(self) -> RuntimeEpoch { + self.epoch + } + pub const fn arm(self) -> Option { + self.arm + } + pub const fn sequence(self) -> u64 { + self.sequence + } + pub const fn produced_at(self) -> Timestamp { + self.produced_at + } + pub const fn valid_until(self) -> Timestamp { + self.valid_until + } + pub const fn permitted(self) -> bool { + self.permitted + } + pub const fn motors(self) -> [f32; 4] { + self.motors + } +} +pub trait FlightOutput { + type Error; + fn actuator(&mut self, record: OutputRecord) -> Result<(), Self::Error>; + fn command_status(&mut self, record: CommandStatus) -> Result<(), Self::Error>; +} +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct FlightStatus { + pub epoch: RuntimeEpoch, + pub owner: CommandOwner, + pub at: Timestamp, + pub arm: Option, + pub phase: VehiclePhase, + pub phase_since: Timestamp, + pub phase_deadline: Option, + pub flight_deadline: Option, + pub fallback_latched: bool, + pub fallback_reason: Option, + pub terminal_reason: Option, + pub offboard: Option, + pub setpoint_sequence: Option, + pub setpoint_expires_at: Option, + pub command_highwater: Option, + pub active_command: Option, + pub last_terminal_command: Option, + pub estimate: Option, + pub ground_state: GroundState, + pub local_output_healthy: bool, + pub last_output_sequence: Option, + pub last_local_commit_at: Option, + pub source_status: [SensorStatus; 5], +} +#[derive(Debug, PartialEq)] +pub struct EndpointError { + pub primary: EndpointFailure, + pub inhibition: Option, +} +#[derive(Debug, PartialEq)] +pub enum EndpointFailure { + ClockRegression, + EpochExhausted, + OutputSequenceExhausted, + NotDisarmed, + ControlDeadline, + Navigation(NavigationFailure), + Control, + Output(E), + Notification(E), +} +impl From> for EndpointError { + fn from(primary: EndpointFailure) -> Self { + Self { + primary, + inhibition: None, + } + } +} +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum RuntimeConfigError { + Navigation, + Control, + Timing, + Safety, +} diff --git a/firmware/libs/kaptan-flight-runtime/tests/lifecycle.rs b/firmware/libs/kaptan-flight-runtime/tests/lifecycle.rs new file mode 100644 index 0000000..02d70ae --- /dev/null +++ b/firmware/libs/kaptan-flight-runtime/tests/lifecycle.rs @@ -0,0 +1,586 @@ +//! Direct scripted sensor trajectories exercise the core flight phases. +mod support; +use kaptan_flight_runtime::*; +use kaptan_navigation::{AccelerationFrd, GRAVITY_MPS2, PositionNed, SensorRecord, VelocityNed}; +use support::*; + +fn climb(t: u64, start: u64, duration: f64) -> (f64, f64, f64) { + let s = (t.saturating_sub(start) as f64 * 1e-6 / duration).clamp(0., 1.); + ( + 2. * (10. * s.powi(3) - 15. * s.powi(4) + 6. * s.powi(5)), + 2. / duration * (30. * s * s - 60. * s.powi(3) + 30. * s.powi(4)), + 2. / (duration * duration) * (60. * s - 180. * s * s + 120. * s.powi(3)), + ) +} +fn fly(ep: &mut FlightEndpoint, out: &mut Recording, t: u64, start: u64, land: Option) { + fly_with_bounce(ep, out, t, start, land, None); +} + +fn fly_with_bounce( + ep: &mut FlightEndpoint, + out: &mut Recording, + t: u64, + start: u64, + land: Option, + bounce: Option, +) { + let motion = |measured: u64| { + let (mut h, mut v, _) = climb(measured, start, 5.); + if let Some(land) = land + && measured >= land + { + let (down, dv, _) = climb(measured, land, 6.); + h = 2. - down; + v = -dv; + } + if let Some(bounce) = bounce + && measured >= bounce + && measured <= bounce + 1_000_000 + { + let phase = (measured - bounce) as f64 * 1e-6 * core::f64::consts::TAU; + h += 0.05 * (1.0 - phase.cos()); + v += 0.05 * core::f64::consts::TAU * phase.sin(); + } + (h, v) + }; + for mut record in records(t) { + let measured = record.context().measurement_time.as_microseconds(); + let (h, v) = motion(measured); + match &mut record { + SensorRecord::Imu(r) => { + // Complete declared window, not endpoint acceleration. + let duration = u64::from(r.acquisition_duration_us); + let a = (v - motion(measured - duration).1) / (duration as f64 * 1e-6); + r.acceleration = + AccelerationFrd::metres_per_second_squared(0., 0., -GRAVITY_MPS2 - a as f32) + } + SensorRecord::LocalPositionVelocity(r) => { + r.position = PositionNed::metres(12., -7., 4. - h as f32); + r.velocity = VelocityNed::metres_per_second(0., 0., -v as f32); + } + SensorRecord::Pressure(r) => { + r.pascals = + (101325. * (-h / (287.05 * 288.15 / f64::from(GRAVITY_MPS2))).exp()) as f32 + } + SensorRecord::Range(r) => r.metres = 0.04 + h as f32, + SensorRecord::Magnetic(_) => {} + } + assert_eq!( + ep.observe(ep.status().epoch, record, time(t), out).unwrap(), + ObservationAdmission::Queued + ); + } + ep.tick(time(t), out).unwrap(); +} + +#[test] +fn onboard_hover_lands_and_stops() { + let (mut ep, mut out, start) = armed(); + let mission = request( + &ep, + 2, + FlightCommand::TakeoffHoverLand { + height_above_launch_metres: 2., + }, + start, + ); + assert_eq!( + ep.command(mission, time(start), &mut out) + .unwrap() + .lifecycle, + CommandLifecycle::Accepted + ); + let flight_deadline = ep.status().flight_deadline; + for t in (start + 10_000..=start + 8_000_000).step_by(10_000) { + fly(&mut ep, &mut out, t, start, None); + } + let hold = ep.status(); + assert_eq!(hold.phase, VehiclePhase::Hold); + let land = hold.phase_since.as_microseconds() + 30_000_000; + assert_eq!(hold.phase_deadline, Some(time(land))); + assert_eq!(hold.active_command.unwrap().sequence, RequestSequence(2)); + for t in (start + 8_010_000..land).step_by(10_000) { + fly(&mut ep, &mut out, t, start, None); + } + assert_eq!(ep.status().phase_since, hold.phase_since); + assert_eq!(ep.status().phase_deadline, Some(time(land))); + assert!(!out.commands.iter().any(|command| { + command.sequence == RequestSequence(2) && command.lifecycle == CommandLifecycle::Completed + })); + let output = *out.outputs.last().unwrap(); + ep.supervise(time(land), &mut out).unwrap(); + assert_eq!(ep.status().phase, VehiclePhase::Land); + assert_eq!(ep.status().flight_deadline, flight_deadline); + assert_eq!( + ep.status().phase_deadline, + Some(time(land + LANDING_BUDGET_US)) + ); + assert_eq!(*out.outputs.last().unwrap(), output); + fly(&mut ep, &mut out, land, start, Some(land)); + for t in (land + 10_000..=land + 9_000_000).step_by(10_000) { + fly(&mut ep, &mut out, t, start, Some(land)); + } + assert_eq!(ep.status().phase, VehiclePhase::Disarmed); + assert_eq!(ep.status().arm, None); + assert_eq!(ep.status().active_command, None); + assert_eq!(ep.status().flight_deadline, None); + assert!(!out.outputs.last().unwrap().permitted()); + let completed = ep.status().last_terminal_command.unwrap(); + assert_eq!(completed.kind, CommandKind::TakeoffHoverLand); + assert_eq!(completed.lifecycle, CommandLifecycle::Completed); + assert_eq!(completed.arm, None); + assert!(completed.at > time(land)); +} + +#[test] +fn takeoff_hold_offboard_loss_latches_lands_and_requires_new_arm() { + let (mut ep, mut out, start) = armed(); + ep.command( + request( + &ep, + 2, + FlightCommand::Takeoff { + height_above_launch_metres: 2., + }, + start, + ), + time(start), + &mut out, + ) + .unwrap(); + for t in (start + 10_000..=start + 8_000_000).step_by(10_000) { + fly(&mut ep, &mut out, t, start, None); + } + assert_eq!(ep.status().phase, VehiclePhase::Hold); + assert_eq!(ep.status().phase_deadline, None); + let held_since = ep.status().phase_since; + // Autonomous phase outlives the finite command TTL and settling deadline. + for t in (start + 8_010_000..=start + 38_000_000).step_by(10_000) { + fly(&mut ep, &mut out, t, start, None); + } + assert_eq!(ep.status().phase_since, held_since); + assert!(ep.status().flight_deadline.is_some()); + let enter = start + 38_000_000; + let origin = ep.status().estimate.unwrap().origin.id; + let initial = LocalSetpoint::PositionYaw { + position: PositionNed::metres(0., 0., -2.), + yaw_radians: 0., + }; + let mut entry = request( + &ep, + 3, + FlightCommand::EnterOffboard { origin, initial }, + enter - 3_000, + ); + entry.context.time_uncertainty_microseconds = 2_000; + assert_eq!( + ep.command(entry, time(enter), &mut out).unwrap().lifecycle, + CommandLifecycle::Completed + ); + let sp = SetpointRequest { + epoch: ep.status().epoch, + owner: ep.status().owner, + arm: ep.status().arm.unwrap(), + origin, + offboard: RequestSequence(3), + sequence: RequestSequence(0), + issued_at: time(enter - 3_000), + time_uncertainty_microseconds: 2_000, + setpoint: initial, + }; + assert_eq!( + ep.setpoint(sp, time(enter), &mut out).unwrap().reason, + CommandReason::Replay + ); + assert!( + ep.setpoint( + SetpointRequest { + sequence: RequestSequence(1), + ..sp + }, + time(enter), + &mut out + ) + .unwrap() + .accepted + ); + for t in (enter + 10_000..=enter + 300_000).step_by(10_000) { + fly(&mut ep, &mut out, t, start, None); + if t == enter + 290_000 { + assert!(out.outputs.last().unwrap().valid_until() > time(enter + 300_000)); + } + } + assert_eq!(ep.status().phase, VehiclePhase::FallbackHold); + assert_eq!(ep.status().phase_deadline, Some(time(enter + 1_295_000))); + let retry = ep + .command( + request( + &ep, + 4, + FlightCommand::EnterOffboard { origin, initial }, + enter + 300_000, + ), + time(enter + 300_000), + &mut out, + ) + .unwrap(); + assert_eq!(retry.reason, CommandReason::FallbackLatched); + assert!( + !ep.setpoint( + SetpointRequest { + sequence: RequestSequence(2), + issued_at: time(enter + 298_000), + ..sp + }, + time(enter + 300_000), + &mut out + ) + .unwrap() + .accepted + ); + let land = enter + 1_300_000; + for t in (enter + 310_000..=land).step_by(10_000) { + fly(&mut ep, &mut out, t, start, None); + if t == land - 10_000 { + assert!(out.outputs.last().unwrap().valid_until() > time(land)); + } + } + assert_eq!(ep.status().phase, VehiclePhase::Land); + let deadline = ep.status().phase_deadline; + assert_eq!(deadline, Some(time(enter + 21_295_000))); + for sequence in [5, 6] { + let repeated = ep + .command( + request(&ep, sequence, FlightCommand::Land, land), + time(land), + &mut out, + ) + .unwrap(); + assert_eq!(repeated.reason, CommandReason::AlreadyActive); + assert_eq!(repeated.lifecycle, CommandLifecycle::Rejected); + assert_eq!(ep.status().phase_deadline, deadline); + } + for t in (land + 10_000..=land + 9_000_000).step_by(10_000) { + fly(&mut ep, &mut out, t, start, Some(land)); + } + assert_eq!(ep.status().phase, VehiclePhase::Disarmed); + assert!(ep.status().fallback_latched); + assert_eq!(ep.status().ground_state, GroundState::Grounded); + let inhibit = out.outputs.last().unwrap(); + assert!(!inhibit.permitted()); + assert_eq!(inhibit.arm(), None); + let now = land + 9_000_000; + let s = ep + .command( + request(&ep, 7, FlightCommand::Arm, now), + time(now), + &mut out, + ) + .unwrap(); + assert_eq!(s.arm, Some(ArmId(7))); + assert!(ep.status().fallback_latched); + for t in (now + 10_000..=now + 600_000).step_by(10_000) { + fly(&mut ep, &mut out, t, start, Some(land)); + } + assert_eq!(ep.status().phase, VehiclePhase::ArmedIdle); + assert!(!ep.status().fallback_latched); +} + +#[test] +fn failed_inhibition_and_terminal_notification_are_both_retained() { + let (mut ep, mut out, now) = armed(); + out.fail_inhibit = true; + out.fail_notification = true; + let err = ep + .command( + request(&ep, 2, FlightCommand::Terminate, now), + time(now), + &mut out, + ) + .unwrap_err(); + assert_eq!(err.primary, EndpointFailure::Notification("notification")); + assert_eq!(err.inhibition, Some("inhibit")); + assert_eq!(ep.status().phase, VehiclePhase::Terminated); + assert!(!out.outputs.last().unwrap().permitted()); + assert_eq!(out.commands.last().unwrap().kind, CommandKind::Terminate); +} + +#[test] +fn supervision_latches_stream_expiry_and_land_without_consuming_control_release() { + let (mut ep, mut out, start) = armed(); + ep.command( + request( + &ep, + 2, + FlightCommand::Takeoff { + height_above_launch_metres: 2.0, + }, + start, + ), + time(start), + &mut out, + ) + .unwrap(); + for t in (start + 10_000..=start + 8_000_000).step_by(10_000) { + fly(&mut ep, &mut out, t, start, None); + } + assert_eq!(ep.status().phase, VehiclePhase::Hold); + let enter = start + 8_000_000; + let origin = ep.status().estimate.unwrap().origin.id; + ep.command( + request( + &ep, + 3, + FlightCommand::EnterOffboard { + origin, + initial: LocalSetpoint::PositionYaw { + position: PositionNed::metres(0.0, 0.0, -2.0), + yaw_radians: 0.0, + }, + }, + enter, + ), + time(enter), + &mut out, + ) + .unwrap(); + let expiry = enter + 300_000; + for t in (enter + 10_000..expiry).step_by(10_000) { + fly(&mut ep, &mut out, t, start, None); + } + let output = *out.outputs.last().unwrap(); + let cached = ep.status().estimate; + ep.supervise(time(expiry), &mut out).unwrap(); + assert_eq!(ep.status().phase, VehiclePhase::FallbackHold); + assert!(ep.status().fallback_latched); + assert_eq!(ep.status().phase_deadline, Some(time(expiry + 1_000_000))); + assert_eq!(ep.status().estimate, cached); + assert_eq!(*out.outputs.last().unwrap(), output); + ep.supervise(time(expiry), &mut out).unwrap(); + assert_eq!(*out.outputs.last().unwrap(), output); + // The post-ingress tick must still release on the due grid point. + fly(&mut ep, &mut out, expiry, start, None); + assert_eq!( + out.outputs.last().unwrap().sequence(), + output.sequence() + 1 + ); + assert!(out.outputs.last().unwrap().permitted()); + for t in (expiry + 10_000..expiry + 1_000_000).step_by(10_000) { + fly(&mut ep, &mut out, t, start, None); + } + let output = *out.outputs.last().unwrap(); + let cached = ep.status().estimate; + ep.supervise(time(expiry + 1_000_000), &mut out).unwrap(); + assert_eq!(ep.status().phase, VehiclePhase::Land); + assert_eq!(ep.status().phase_deadline, Some(time(expiry + 21_000_000))); + assert_eq!(ep.status().estimate, cached); + assert_eq!(*out.outputs.last().unwrap(), output); +} + +#[test] +fn unsuccessful_hold_enters_land_without_resetting_flight_deadline() { + let (mut ep, mut out, start) = armed(); + ep.command( + request( + &ep, + 2, + FlightCommand::Takeoff { + height_above_launch_metres: 2., + }, + start, + ), + time(start), + &mut out, + ) + .unwrap(); + for t in (start + 10_000..=start + 2_000_000).step_by(10_000) { + fly(&mut ep, &mut out, t, start, None); + } + let at = start + 2_000_000; + ep.command(request(&ep, 3, FlightCommand::Hold, at), time(at), &mut out) + .unwrap(); + let flight_deadline = ep.status().flight_deadline; + for t in (at + 10_000..=at + 10_000_000).step_by(10_000) { + fly(&mut ep, &mut out, t, start, None); + } + assert_eq!(ep.status().phase, VehiclePhase::Land); + assert_eq!(ep.status().flight_deadline, flight_deadline); + let result = ep.status().last_terminal_command.unwrap(); + assert_eq!(result.kind, CommandKind::Hold); + assert_eq!(result.lifecycle, CommandLifecycle::Failed); + assert_eq!(result.reason, CommandReason::PhaseDeadline); +} + +#[test] +fn streamed_velocity_and_yaw_reach_the_actual_controller() { + let (mut ep, mut out, start) = armed(); + ep.command( + request( + &ep, + 2, + FlightCommand::Takeoff { + height_above_launch_metres: 2.0, + }, + start, + ), + time(start), + &mut out, + ) + .unwrap(); + for t in (start + 10_000..=start + 8_000_000).step_by(10_000) { + fly(&mut ep, &mut out, t, start, None); + } + let now = start + 8_000_000; + let status = ep.status(); + let origin = status.estimate.unwrap().origin.id; + let initial = LocalSetpoint::VelocityYaw { + velocity: VelocityNed::metres_per_second(0.3, 0.0, 0.0), + yaw_radians: 0.5, + }; + ep.command( + request( + &ep, + 3, + FlightCommand::EnterOffboard { origin, initial }, + now, + ), + time(now), + &mut out, + ) + .unwrap(); + for t in (now + 10_000..=now + 50_000).step_by(10_000) { + fly(&mut ep, &mut out, t, start, None); + } + let u = out.outputs.last().unwrap().motors(); + assert!(u[0] - u[1] + u[2] - u[3] > 0.0001); // positive yaw, mixer reaction signs + assert!(u[0] - u[1] - u[2] + u[3] < -0.001); // nose-down pitch for north velocity + let sp = SetpointRequest { + epoch: status.epoch, + owner: status.owner, + arm: status.arm.unwrap(), + origin, + offboard: RequestSequence(3), + sequence: RequestSequence(1), + issued_at: time(now + 50_000), + time_uncertainty_microseconds: 0, + setpoint: LocalSetpoint::VelocityYaw { + velocity: VelocityNed::metres_per_second(0.3, 0.0, 0.0), + yaw_radians: -0.5, + }, + }; + assert!( + ep.setpoint(sp, time(now + 50_000), &mut out) + .unwrap() + .accepted + ); + let expires = ep.status().setpoint_expires_at; + assert_eq!( + ep.setpoint(sp, time(now + 50_000), &mut out) + .unwrap() + .reason, + CommandReason::Replay + ); + assert_eq!(ep.status().setpoint_expires_at, expires); + for t in (now + 60_000..=now + 150_000).step_by(10_000) { + fly(&mut ep, &mut out, t, start, None); + } + let u = out.outputs.last().unwrap().motors(); + assert!(u[0] - u[1] + u[2] - u[3] < -0.0001); +} + +#[test] +fn sensed_bounce_restarts_contact_dwell_without_extending_landing() { + let (mut ep, mut out, start) = armed(); + ep.command( + request( + &ep, + 2, + FlightCommand::Takeoff { + height_above_launch_metres: 2.0, + }, + start, + ), + time(start), + &mut out, + ) + .unwrap(); + for t in (start + 10_000..=start + 8_000_000).step_by(10_000) { + fly(&mut ep, &mut out, t, start, None); + } + let land = start + 8_000_000; + ep.command( + request(&ep, 3, FlightCommand::Land, land), + time(land), + &mut out, + ) + .unwrap(); + let deadline = ep.status().phase_deadline; + let mut bounce = None; + let mut observed_motion = false; + for t in (land + 10_000..=land + 11_000_000).step_by(10_000) { + fly_with_bounce(&mut ep, &mut out, t, start, Some(land), bounce); + if bounce.is_none() && ep.status().phase == VehiclePhase::TouchdownConfirm { + bounce = Some(t + 100_000); + } + if let Some(at) = bounce { + if t >= at + 200_000 && t <= at + 800_000 { + assert_ne!(ep.status().ground_state, GroundState::Grounded); + assert_eq!(ep.status().phase_deadline, deadline); + observed_motion = true; + } + if t >= at && t < at + 2_000_000 { + assert_ne!(ep.status().phase, VehiclePhase::Disarmed); + } + } + } + assert!(observed_motion); + assert_eq!(ep.status().phase, VehiclePhase::Disarmed); + assert_eq!( + ep.status().last_terminal_command.unwrap().kind, + CommandKind::Land + ); +} + +#[test] +fn flight_reserve_fails_pending_hold_before_autonomous_landing() { + let (mut ep, mut out, start) = armed(); + ep.command( + request( + &ep, + 2, + FlightCommand::Takeoff { + height_above_launch_metres: 2.0, + }, + start, + ), + time(start), + &mut out, + ) + .unwrap(); + for t in (start + 10_000..=start + 99_000_000).step_by(10_000) { + fly(&mut ep, &mut out, t, start, None); + } + let hold = start + 99_000_000; + ep.command( + request(&ep, 3, FlightCommand::Hold, hold), + time(hold), + &mut out, + ) + .unwrap(); + let land = start + 100_000_000; + for t in (hold + 10_000..=land).step_by(10_000) { + fly(&mut ep, &mut out, t, start, None); + } + assert_eq!(ep.status().phase, VehiclePhase::Land); + assert_eq!(ep.status().active_command, None); + let failure = ep.command_outcomes()[0].unwrap().status; + assert_eq!(failure.sequence, RequestSequence(3)); + assert_eq!(failure.lifecycle, CommandLifecycle::Failed); + assert_eq!(failure.reason, CommandReason::PhaseDeadline); + for t in (land + 10_000..=land + 9_000_000).step_by(10_000) { + fly(&mut ep, &mut out, t, start, Some(land)); + } + assert_eq!(ep.status().phase, VehiclePhase::Disarmed); + assert!(!out.commands.iter().any(|c|c.sequence==RequestSequence(3) && c.lifecycle==CommandLifecycle::Completed)); + assert_eq!(ep.status().last_terminal_command, Some(failure)); +} diff --git a/firmware/libs/kaptan-flight-runtime/tests/navigation.rs b/firmware/libs/kaptan-flight-runtime/tests/navigation.rs new file mode 100644 index 0000000..a14c705 --- /dev/null +++ b/firmware/libs/kaptan-flight-runtime/tests/navigation.rs @@ -0,0 +1,164 @@ +mod support; +use kaptan_config::ConfigModel; +use kaptan_flight_runtime::*; +use kaptan_math::Vector3; +use kaptan_navigation::{ + AccelerationFrd, AngularRateFrd, NavigationFailure, PositionNed, SensorRecord, VelocityNed, +}; +use support::*; + +#[test] +fn actual_navigation_fixed_reference_origin_and_delayed_aiding() { + let (ep, out, now) = ready(); + let estimate = ep.status().estimate.unwrap(); + assert!( + estimate.capabilities.flight_ready(), + "{:?}", + estimate.capabilities + ); + assert!(estimate.position.vector().norm() < 0.01); + assert!((estimate.origin.launch_in_reference.vector().x() - 12.).abs() < 0.001); + assert!(estimate.fused_through.as_microseconds() <= now - 100_000); + assert_eq!(estimate.at, time(now)); + assert!( + estimate + .ground_clearance + .unwrap() + .contact_clearance_metres + .abs() + < 0.002 + ); + assert!(out.outputs.iter().all(|v| !v.permitted())); +} +#[test] +fn navigation_failures_retain_their_reason() { + for (delay, observe, failure, reason) in [ + ( + 30_000, + true, + NavigationFailure::ImuGap, + TerminalReason::ImuLost, + ), + ( + 100_000, + false, + NavigationFailure::WorkBudget, + TerminalReason::NavigationLost, + ), + ] { + let (mut ep, mut out, now) = ready(); + let now = now + delay; + let error = if observe { + ep.observe( + ep.status().epoch, + SensorRecord::Imu(imu(now)), + time(now), + &mut out, + ) + .unwrap_err() + } else { + ep.tick(time(now), &mut out).unwrap_err() + }; + assert_eq!(error.primary, EndpointFailure::Navigation(failure)); + assert_eq!(ep.status().terminal_reason, Some(reason)); + assert_eq!(ep.status().phase, VehiclePhase::Terminated); + assert!(!out.outputs.last().unwrap().permitted()); + } +} + +#[test] +fn healthy_armed_idle_expires_without_rearming() { + let (mut ep, mut out, now) = armed(); + for t in (now + 10_000..=now + 10_500_000).step_by(10_000) { + feed(&mut ep, &mut out, t); + } + assert_eq!(ep.status().phase, VehiclePhase::Disarmed); + assert_eq!(ep.status().arm, None); + assert!(out.outputs.iter().all(|v| !v.permitted())); +} + +#[test] +fn declared_noise_realizations_initialize_and_remain_grounded() { + // Box-Muller Gaussian draws, independently implemented at the sensor boundary. + // q/T gives IMU per-sample variance; no packet carries an ImuNoise value. + let mut seed = 0xaca5_8113_78ab_1234_u64; + let mut uniform = || { + seed ^= seed << 13; + seed ^= seed >> 7; + seed ^= seed << 17; + ((seed >> 11) as f64 + 0.5) / 9_007_199_254_740_992. + }; + let mut normal = || (-2. * uniform().ln()).sqrt() * (core::f64::consts::TAU * uniform()).cos(); + let profile = profile_fixture::candidate() + .validate(&profile_fixture::capabilities()) + .unwrap(); + let mut ep = FlightEndpoint::new( + profile, + RunBinding { + session: TransportSessionId(7), + owner: CommandOwner(1), + }, + ) + .unwrap(); + let mut out = Recording::default(); + for m in (10_000..=6_000_000).step_by(10_000) { + let t = m + 1_000; + for mut record in records(t) { + match &mut record { + SensorRecord::Imu(r) => { + let s = (2.5e-5_f64 / 0.01).sqrt(); + r.acceleration = AccelerationFrd::metres_per_second_squared( + (normal() * s) as f32, + (normal() * s) as f32, + (-f64::from(kaptan_navigation::GRAVITY_MPS2) + normal() * s) as f32, + ); + let s = (2.5e-7_f64 / 0.01).sqrt(); + r.angular_rate = AngularRateFrd::radians_per_second( + (normal() * s) as f32, + (normal() * s) as f32, + (normal() * s) as f32, + ); + } + SensorRecord::LocalPositionVelocity(r) => { + r.position = PositionNed::metres( + (12. + normal() * 0.1) as f32, + (-7. + normal() * 0.1) as f32, + (4. + normal() * 0.1) as f32, + ); + r.velocity = VelocityNed::metres_per_second( + (normal() * 0.05) as f32, + (normal() * 0.05) as f32, + (normal() * 0.05) as f32, + ); + } + SensorRecord::Magnetic(r) => { + r.microtesla_frd = Vector3::new( + (20. + normal() * 0.02) as f32, + (normal() * 0.02) as f32, + (45. + normal() * 0.02) as f32, + ) + } + SensorRecord::Pressure(r) => r.pascals = (101_325. + normal() * 0.3) as f32, + SensorRecord::Range(r) => { + r.metres = (((0.04 + normal() * 0.002) / 0.001).round() * 0.001) as f32 + } + } + let admitted = ep + .observe(ep.status().epoch, record, time(t), &mut out) + .unwrap(); + assert!( + admitted == ObservationAdmission::Queued + || admitted + == ObservationAdmission::Rejected( + ObservationRejection::BehindFusionHorizon + ) + ); + } + ep.tick(time(t), &mut out).unwrap(); + } + let e = ep.status().estimate.unwrap(); + assert!(e.capabilities.flight_ready(), "{:?}", e.capabilities); + assert!(e.position.vector().norm() < 0.1, "{:?}", e.position); + assert_eq!(ep.status().ground_state, GroundState::Grounded); + assert!(out.outputs.iter().all(|r| !r.permitted())); +} diff --git a/firmware/libs/kaptan-flight-runtime/tests/replay.rs b/firmware/libs/kaptan-flight-runtime/tests/replay.rs new file mode 100644 index 0000000..ca420c8 --- /dev/null +++ b/firmware/libs/kaptan-flight-runtime/tests/replay.rs @@ -0,0 +1,413 @@ +//! Sensor-driven lifecycle and output boundary regressions. +mod support; +use kaptan_flight_runtime::*; +use support::*; + +#[test] +fn sensor_startup_arm_binding_and_ground_disarm() { + let (mut ep, mut out, now) = ready(); + assert_eq!(ep.status().ground_state, GroundState::Grounded); + let s = ep + .command( + request(&ep, 1, FlightCommand::Arm, now), + time(now), + &mut out, + ) + .unwrap(); + assert_eq!(s.lifecycle, CommandLifecycle::Accepted); + assert_eq!(s.arm, Some(ArmId(1))); + assert_eq!(ep.status().phase, VehiclePhase::PreArm); + let binding = out.outputs.last().unwrap(); + assert_eq!(binding.arm(), s.arm); + assert!(!binding.permitted()); + let s = ep + .command( + request(&ep, 2, FlightCommand::GroundDisarm, now), + time(now), + &mut out, + ) + .unwrap(); + assert_eq!(s.lifecycle, CommandLifecycle::Completed); + assert_eq!(ep.status().phase, VehiclePhase::Disarmed); + assert_eq!(out.outputs.last().unwrap().arm(), None); +} +#[test] +fn command_ttl_replay_and_epoch_do_not_renew_authority() { + let (mut ep, mut out, now) = ready(); + let zero = request(&ep, 0, FlightCommand::Arm, now); + assert_eq!( + ep.command(zero, time(now), &mut out).unwrap().reason, + CommandReason::Replay + ); + let mut wrong = request(&ep, 50, FlightCommand::Arm, now); + wrong.context.owner = CommandOwner(999); + assert_eq!( + ep.command(wrong, time(now), &mut out).unwrap().reason, + CommandReason::WrongOwner + ); + assert_eq!(ep.status().command_highwater, None); + let mut expired = request(&ep, 1, FlightCommand::Arm, now); + expired.context.expires_at = time(now); + assert_eq!( + ep.command(expired, time(now), &mut out).unwrap().lifecycle, + CommandLifecycle::Rejected + ); + assert_eq!(ep.status().command_highwater, Some(RequestSequence(1))); + assert_eq!( + ep.command( + request(&ep, 1, FlightCommand::Arm, now), + time(now), + &mut out + ) + .unwrap() + .reason, + CommandReason::Replay + ); + ep.command( + request(&ep, 2, FlightCommand::Arm, now), + time(now), + &mut out, + ) + .unwrap(); + for t in (now + 10_000..=now + 600_000).step_by(10_000) { + feed(&mut ep, &mut out, t); + } + assert_eq!(ep.status().phase, VehiclePhase::ArmedIdle); // TTL expired, arm completed. + ep.command( + request(&ep, 3, FlightCommand::GroundDisarm, now + 600_000), + time(now + 600_000), + &mut out, + ) + .unwrap(); + let old = ep.status().epoch; + let seq = out.outputs.last().unwrap().sequence(); + let epoch = ep + .begin_epoch(CommandOwner(8), time(now + 600_000), &mut out) + .unwrap(); + assert_ne!(old, epoch); + assert_eq!(out.outputs.last().unwrap().sequence(), seq + 1); + assert_eq!(ep.status().at, time(now + 600_000)); +} +#[test] +fn failed_sink_consumes_sequence_and_attempts_new_inhibition() { + let (mut ep, mut out, now) = armed(); + ep.command( + request( + &ep, + 2, + FlightCommand::Takeoff { + height_above_launch_metres: 2., + }, + now, + ), + time(now), + &mut out, + ) + .unwrap(); + out.fail_enabled = true; + out.fail_inhibit = true; + let attempts = out.outputs.len(); + let err = feed_result(&mut ep, &mut out, now + 10_000).unwrap_err(); + assert_eq!(out.outputs.len(), attempts + 2); + assert_eq!(err.primary, EndpointFailure::Output("enabled")); + assert_eq!(err.inhibition, Some("inhibit")); + assert_eq!(ep.status().phase, VehiclePhase::Terminated); + let n = out.outputs.len(); + assert!(out.outputs[n - 2].permitted()); + assert!(!out.outputs[n - 1].permitted()); + assert_eq!( + out.outputs[n - 1].sequence(), + out.outputs[n - 2].sequence() + 1 + ); + assert!( + ep.begin_epoch(CommandOwner(1), time(now + 10_000), &mut out) + .is_err() + ); +} + +#[test] +fn failed_arm_binding_publication_retains_the_failed_transaction() { + let (mut ep, mut out, now) = ready(); + out.fail_inhibit = true; + let error = ep + .command( + request(&ep, 1, FlightCommand::Arm, now), + time(now), + &mut out, + ) + .unwrap_err(); + assert_eq!(error.primary, EndpointFailure::Output("inhibit")); + assert_eq!(ep.status().phase, VehiclePhase::Terminated); + assert_eq!(ep.status().arm, None); + let failed = ep.status().last_terminal_command.unwrap(); + assert_eq!(failed.sequence, RequestSequence(1)); + assert_eq!(failed.lifecycle, CommandLifecycle::Failed); + assert_eq!(failed.arm, Some(ArmId(1))); + assert!(!out.outputs.last().unwrap().permitted()); +} +#[test] +fn asynchronous_tick_deadline_and_clock_fault_inhibit() { + let (mut ep, mut out, now) = armed(); + ep.command( + request( + &ep, + 2, + FlightCommand::Takeoff { + height_above_launch_metres: 2., + }, + now, + ), + time(now), + &mut out, + ) + .unwrap(); + feed(&mut ep, &mut out, now + 10_000); + let record = *out.outputs.last().unwrap(); + assert!(record.permitted()); + assert!( + record.valid_until().as_microseconds() <= record.produced_at().as_microseconds() + 20_000 + ); + ep.tick(time(now + 10_000), &mut out).unwrap(); + assert_eq!(*out.outputs.last().unwrap(), record); + let error = ep.tick(time(now + 30_001), &mut out).unwrap_err(); + assert_eq!(error.primary, EndpointFailure::ControlDeadline); + assert!(!out.outputs.last().unwrap().permitted()); + let error = ep.tick(time(now), &mut out).unwrap_err(); + assert_eq!(error.primary, EndpointFailure::ClockRegression); + assert_eq!(ep.status().phase, VehiclePhase::Terminated); +} +#[test] +fn ordinary_prearm_health_failure_clears_arm_without_terminal_fault() { + let (mut ep, mut out, now) = ready(); + ep.command( + request(&ep, 1, FlightCommand::Arm, now), + time(now), + &mut out, + ) + .unwrap(); + ep.tick(time(now + 25_000), &mut out).unwrap(); + assert_eq!(ep.status().phase, VehiclePhase::Disarmed); + assert_eq!(ep.status().arm, None); + assert_eq!( + ep.status().last_terminal_command.unwrap().lifecycle, + CommandLifecycle::Failed + ); + assert!(!out.outputs.last().unwrap().permitted()); +} +#[test] +fn typed_bad_quality_consumes_sensor_watermark_not_health() { + let (mut ep, mut out, now) = ready(); + // The previous complete acquisition ended 1 ms before the service tick. + let mut bad = imu(now + 9_000); + bad.context.quality = 0; + assert_eq!( + ep.observe( + ep.status().epoch, + SensorRecord::Imu(bad), + time(now + 10_000), + &mut out + ) + .unwrap(), + ObservationAdmission::Rejected(ObservationRejection::InvalidQuality) + ); + bad.context.quality = 255; + assert_eq!( + ep.observe( + ep.status().epoch, + SensorRecord::Imu(bad), + time(now + 10_000), + &mut out + ) + .unwrap(), + ObservationAdmission::Rejected(ObservationRejection::Replay) + ); +} + +#[test] +fn fresh_terminate_does_not_require_current_navigation() { + let (mut ep, mut out, now) = armed(); + let now = now + 50_000; + let result = ep + .command( + request(&ep, 2, FlightCommand::Terminate, now), + time(now), + &mut out, + ) + .unwrap(); + assert_eq!(result.lifecycle, CommandLifecycle::Completed); + assert_eq!(ep.status().phase, VehiclePhase::Terminated); + assert_eq!(ep.status().terminal_reason, Some(TerminalReason::Requested)); + assert!(!out.outputs.last().unwrap().permitted()); + let sources = ep.status().source_status; + for t in (now + 10_000..=now + 500_000).step_by(10_000) { + for record in records(t) { + assert_eq!( + ep.observe(ep.status().epoch, record, time(t), &mut out) + .unwrap(), + ObservationAdmission::Rejected(ObservationRejection::Terminated) + ); + } + ep.tick(time(t), &mut out).unwrap(); + } + assert_eq!(ep.status().source_status, sources); + assert_eq!(ep.status().terminal_reason, Some(TerminalReason::Requested)); +} + +#[test] +fn new_source_isolation_is_seen_before_the_next_control_tick() { + let (mut ep, mut out, now) = ready(); + for step in 1..=15 { + let t = now + step * 10_000; + for mut record in records(t) { + let bad = if let SensorRecord::Magnetic(ref mut magnetic) = record { + magnetic.context.quality = 0; + true + } else { + false + }; + let outcome = ep + .observe(ep.status().epoch, record, time(t), &mut out) + .unwrap(); + assert_eq!( + outcome, + if bad { + ObservationAdmission::Rejected(ObservationRejection::InvalidQuality) + } else { + ObservationAdmission::Queued + } + ); + } + if step == 15 { + let arm = ep + .command(request(&ep, 1, FlightCommand::Arm, t), time(t), &mut out) + .unwrap(); + assert_eq!(arm.reason, CommandReason::NotReady); + assert_eq!(ep.status().phase, VehiclePhase::Disarmed); + } + ep.tick(time(t), &mut out).unwrap(); + } + for t in (now + 160_000..=now + 1_400_000).step_by(10_000) { + feed(&mut ep, &mut out, t); + } + assert_eq!(ep.status().phase, VehiclePhase::Disarmed); + assert_eq!(ep.status().ground_state, GroundState::Grounded); + assert!(ep.status().estimate.unwrap().capabilities.heading.available); + let t = now + 1_400_000; + assert_eq!( + ep.command(request(&ep, 2, FlightCommand::Arm, t), time(t), &mut out) + .unwrap() + .lifecycle, + CommandLifecycle::Accepted + ); +} + +#[test] +fn sensed_motion_during_prearm_consumes_the_arm_transaction() { + let (mut ep, mut out, now) = ready(); + ep.command( + request(&ep, 1, FlightCommand::Arm, now), + time(now), + &mut out, + ) + .unwrap(); + for step in 1..=3 { + let t = now + step * 10_000; + for mut record in records(t) { + if let SensorRecord::Imu(ref mut imu) = record { + imu.acceleration = kaptan_navigation::AccelerationFrd::metres_per_second_squared( + 0.0, + 0.0, + -kaptan_navigation::GRAVITY_MPS2 - 5.0, + ); + } + ep.observe(ep.status().epoch, record, time(t), &mut out) + .unwrap(); + } + ep.tick(time(t), &mut out).unwrap(); + } + assert_eq!(ep.status().phase, VehiclePhase::Disarmed); + assert_eq!(ep.status().arm, None); + assert_eq!(ep.status().terminal_reason, None); + assert_eq!( + ep.status().last_terminal_command.unwrap().lifecycle, + CommandLifecycle::Failed + ); + assert!(out.outputs.iter().all(|record| !record.permitted())); +} + +#[test] +fn full_notification_sink_observes_both_cancelled_arm_and_ground_disarm() { + let (mut ep, mut out, now) = ready(); + ep.command( + request(&ep, 1, FlightCommand::Arm, now), + time(now), + &mut out, + ) + .unwrap(); + out.fail_notification = true; + let error = ep + .command( + request(&ep, 2, FlightCommand::GroundDisarm, now), + time(now), + &mut out, + ) + .unwrap_err(); + assert_eq!(error.primary, EndpointFailure::Notification("notification")); + assert_eq!(ep.status().phase, VehiclePhase::Disarmed); + assert!(!out.outputs.last().unwrap().permitted()); + let outcomes = ep.command_outcomes(); + let cancelled = outcomes[0].unwrap(); + let disarmed = outcomes[1].unwrap(); + assert_eq!(cancelled.status.sequence, RequestSequence(1)); + assert_eq!(cancelled.status.lifecycle, CommandLifecycle::Cancelled); + assert_eq!(disarmed.status.sequence, RequestSequence(2)); + assert_eq!(disarmed.status.lifecycle, CommandLifecycle::Completed); + assert!(!cancelled.published); + assert!(!disarmed.published); + assert_eq!( + &out.commands[out.commands.len() - 2..], + &[cancelled.status, disarmed.status] + ); + assert_eq!(ep.status().last_terminal_command, Some(disarmed.status)); +} + +#[test] +fn asynchronous_tick_and_observe_faults_deliver_failed_command_outcomes() { + for observe_fault in [false, true] { + let (mut ep, mut out, now) = armed(); + ep.command( + request( + &ep, + 2, + FlightCommand::Takeoff { + height_above_launch_metres: 2.0, + }, + now, + ), + time(now), + &mut out, + ) + .unwrap(); + out.fail_notification = true; + let error = if observe_fault { + let sample = imu(now + 30_000); + ep.observe( + ep.status().epoch, + SensorRecord::Imu(sample), + time(now + 30_000), + &mut out, + ) + .unwrap_err() + } else { + ep.tick(time(now + 30_000), &mut out).unwrap_err() + }; + assert!(!matches!(error.primary, EndpointFailure::Notification(_))); + let outcome = ep.command_outcomes()[0].unwrap(); + assert_eq!(outcome.status.sequence, RequestSequence(2)); + assert_eq!(outcome.status.lifecycle, CommandLifecycle::Failed); + assert!(!outcome.published); + assert_eq!(out.commands.last(), Some(&outcome.status)); + assert_eq!(ep.status().phase, VehiclePhase::Terminated); + assert!(!out.outputs.last().unwrap().permitted()); + } +} diff --git a/firmware/libs/kaptan-flight-runtime/tests/support/mod.rs b/firmware/libs/kaptan-flight-runtime/tests/support/mod.rs new file mode 100644 index 0000000..950d363 --- /dev/null +++ b/firmware/libs/kaptan-flight-runtime/tests/support/mod.rs @@ -0,0 +1,173 @@ +#[path = "../../../kaptan-flight-config/tests/support/mod.rs"] +pub mod profile_fixture; + +use kaptan_config::ConfigModel; +use kaptan_flight_runtime::*; +use kaptan_math::Vector3; +use kaptan_navigation::*; +use kaptan_time::Timestamp; + +pub fn time(v: u64) -> Timestamp { + Timestamp::from_microseconds(v) +} +pub fn context(t: u64, receipt: u64, kind: u8, period: u64, frame: FrameId) -> ObservationContext { + ObservationContext { + measurement_time: time(t), + receipt_time: time(receipt), + source: SourceId::new(kind), + frame, + calibration: CalibrationRevision::new(1), + quality: 255, + validity: Validity::Valid, + sequence: ObservationSequence::new((t / period) as u32), + time_uncertainty_microseconds: 0, + } +} +pub fn imu(t: u64) -> FlightImuObservation { + FlightImuObservation { + acquisition_duration_us: 10_000, + context: context(t, t, 1, 10_000, FrameId::BODY_FRD), + acceleration: AccelerationFrd::metres_per_second_squared(0., 0., -GRAVITY_MPS2), + angular_rate: AngularRateFrd::radians_per_second(0., 0., 0.), + } +} +pub fn records(t: u64) -> Vec { + let mut result = vec![SensorRecord::Imu(imu(t - 1_000))]; + let m = t - 1_000; + if m % 50_000 == 30_000 { + result.push(SensorRecord::LocalPositionVelocity( + LocalPositionVelocityObservation { + context: context(m - 30_000, t, 2, 50_000, FrameId::LOCAL_NED), + position: PositionNed::metres(12., -7., 4.), + velocity: VelocityNed::metres_per_second(0., 0., 0.), + position_variance: [ScalarVariance::new(0.01).unwrap(); 3], + velocity_variance: [ScalarVariance::new(0.0025).unwrap(); 3], + reference: SensorReferenceId(1), + }, + )); + } + if m.is_multiple_of(50_000) { + result.push(SensorRecord::Magnetic(MagneticObservation { + context: context(m, t, 3, 50_000, FrameId::BODY_FRD), + microtesla_frd: Vector3::new(20., 0., 45.), + variance_microtesla_squared: [ScalarVariance::new(0.0004).unwrap(); 3], + })); + result.push(SensorRecord::Pressure(PressureObservation { + context: context(m, t, 4, 50_000, FrameId::SCALAR), + pascals: 101_325., + variance_pascals_squared: ScalarVariance::new(0.09).unwrap(), + })); + } + if m.is_multiple_of(20_000) { + result.push(SensorRecord::Range(RangeObservation { + context: context(m, t, 5, 20_000, FrameId::new(3)), + metres: 0.04, + variance: ScalarVariance::new(4e-6).unwrap(), + })); + } + result +} +#[derive(Default)] +pub struct Recording { + pub outputs: Vec, + pub commands: Vec, + pub fail_enabled: bool, + pub fail_inhibit: bool, + pub fail_notification: bool, +} +impl FlightOutput for Recording { + type Error = &'static str; + fn actuator(&mut self, r: OutputRecord) -> Result<(), Self::Error> { + self.outputs.push(r); + if r.permitted() && self.fail_enabled { + Err("enabled") + } else if !r.permitted() && self.fail_inhibit { + Err("inhibit") + } else { + Ok(()) + } + } + fn command_status(&mut self, s: CommandStatus) -> Result<(), Self::Error> { + self.commands.push(s); + if self.fail_notification { + Err("notification") + } else { + Ok(()) + } + } +} +pub fn feed_result( + ep: &mut FlightEndpoint, + out: &mut Recording, + t: u64, +) -> Result<(), EndpointError<&'static str>> { + for r in records(t) { + let result = ep.observe(ep.status().epoch, r, time(t), out)?; + if result == ObservationAdmission::Rejected(ObservationRejection::BehindFusionHorizon) { + // The first delayed GNSS delivery after initialization can predate + // t0. It must be refused rather than fused backwards or retimed. + assert!(ep.status().estimate.is_none()); + assert_eq!(r.kind(), 2); + assert!(r.context().measurement_time.as_microseconds() <= 2_020_000); + } else { + assert_eq!(result, ObservationAdmission::Queued); + } + } + ep.tick(time(t), out) +} +pub fn feed(ep: &mut FlightEndpoint, out: &mut Recording, t: u64) { + feed_result(ep, out, t).unwrap(); +} +pub fn request(ep: &FlightEndpoint, seq: u64, command: FlightCommand, t: u64) -> CommandRequest { + CommandRequest { + context: RequestContext { + epoch: ep.status().epoch, + owner: ep.status().owner, + sequence: RequestSequence(seq), + issued_at: time(t), + expires_at: time(t + 250_000), + time_uncertainty_microseconds: 0, + arm: if command == FlightCommand::Arm { + None + } else { + ep.status().arm + }, + }, + command, + } +} +pub fn ready() -> (FlightEndpoint, Recording, u64) { + let profile = profile_fixture::candidate() + .validate(&profile_fixture::capabilities()) + .unwrap(); + let mut ep = FlightEndpoint::new( + profile, + RunBinding { + session: TransportSessionId(123), + owner: CommandOwner(1), + }, + ) + .unwrap(); + let mut out = Recording::default(); + for m in (10_000..=4_000_000).step_by(10_000) { + feed(&mut ep, &mut out, m + 1_000); + } + assert!(ep.status().estimate.is_some(), "missing initialization"); + (ep, out, 4_001_000) +} +pub fn armed() -> (FlightEndpoint, Recording, u64) { + let (mut ep, mut out, now) = ready(); + let result = ep + .command( + request(&ep, 1, FlightCommand::Arm, now), + time(now), + &mut out, + ) + .unwrap(); + assert_eq!(result.lifecycle, CommandLifecycle::Accepted, "{result:?}"); + for t in (now + 10_000..=now + 600_000).step_by(10_000) { + feed(&mut ep, &mut out, t); + } + assert_eq!(ep.status().phase, VehiclePhase::ArmedIdle); + (ep, out, now + 600_000) +} diff --git a/firmware/libs/kaptan-i2c/Cargo.toml b/firmware/libs/kaptan-i2c/Cargo.toml index 383510d..e291f56 100644 --- a/firmware/libs/kaptan-i2c/Cargo.toml +++ b/firmware/libs/kaptan-i2c/Cargo.toml @@ -7,3 +7,6 @@ version.workspace = true [lints] workspace = true + +[dependencies] +kaptan-time = { path = "../kaptan-time" } diff --git a/firmware/libs/kaptan-i2c/src/lib.rs b/firmware/libs/kaptan-i2c/src/lib.rs index 3b1b9de..464f47d 100644 --- a/firmware/libs/kaptan-i2c/src/lib.rs +++ b/firmware/libs/kaptan-i2c/src/lib.rs @@ -6,6 +6,8 @@ #![no_std] +use kaptan_time::Timestamp; + /// Terminal outcome reported by an I²C controller. #[derive(Clone, Copy, Debug, Eq, PartialEq)] pub enum I2cCompletion { @@ -17,6 +19,18 @@ pub enum I2cCompletion { TransferFault, } +/// Completion admission or cancellation failure. Errors never copy read bytes. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum CompletionError { + NotPending, + NotActive, + InvalidLength, + TransferFault, + /// Physical STOP was not observed by the shutdown deadline. The controller + /// is halted and cannot accept another transaction during this boot. + StopUnproven, +} + /// Owns a non-blocking I²C controller route and its in-flight storage. /// /// Each operation sends a STOP on successful completion. Controllers copy a @@ -38,13 +52,25 @@ pub trait I2cBus { /// Releases controller ownership after a hardware ISR acknowledged the /// terminal event and, for a successful read, copies into `read`. /// - /// This must not poll or wait. `read` must match the length submitted to - /// [`Self::start_read`] for the completed read transaction. - fn complete_transaction(&mut self, completion: I2cCompletion, read: Option<&mut [u8]>); + /// This must not poll or wait. `NotPending` is entirely nonmutating. A + /// successful read requires a matching destination length and controller + /// proof of the current transaction's STOP, not just a caller notification. + /// Old IRQs/completions must not retire or copy out a newer transaction. + fn complete_transaction( + &mut self, + completion: I2cCompletion, + read: Option<&mut [u8]>, + ) -> Result<(), CompletionError>; /// Cancels an in-flight transaction after a platform-selected timeout. /// - /// Implementations must release controller-owned transaction state and - /// arrange for any required STOP or peripheral recovery without waiting. - fn cancel_transaction(&mut self); + /// Requests STOP without waiting. Admission remains closed until STOP is + /// proven. This is not permission to reuse or free in-flight storage. + fn cancel_transaction(&mut self, now: Timestamp); + + /// Services cancellation deadlines even without interrupts. The caller must + /// invoke this periodically with its monotonic boot clock. A bounded STOP + /// failure halts admission and returns `StopUnproven`. Storage must outlive + /// all hardware access, including owner drop/forget and failed cancellation. + fn service(&mut self, now: Timestamp) -> Result<(), CompletionError>; } diff --git a/firmware/libs/kaptan-math/src/kani_proofs.rs b/firmware/libs/kaptan-math/src/kani_proofs.rs new file mode 100644 index 0000000..edfabcd --- /dev/null +++ b/firmware/libs/kaptan-math/src/kani_proofs.rs @@ -0,0 +1,26 @@ +//! Classification of the production square root, not an accuracy proof. + +use super::sqrt; + +#[kani::proof] +#[kani::unwind(5)] +fn sqrt_preserves_domain_and_zero_sign() { + let value = f32::from_bits(kani::any()); + let root = sqrt(value); + + if !value.is_finite() || value < 0.0 { + assert!(root.is_nan()); + } else if value == 0.0 { + assert_eq!(root.to_bits(), value.to_bits()); + } else { + assert!(root.is_finite()); + assert!(root > 0.0); + } + + kani::cover!(value.is_nan()); + kani::cover!(value == f32::INFINITY); + kani::cover!(value < 0.0 && value.is_finite()); + kani::cover!(value.to_bits() == (-0.0_f32).to_bits()); + kani::cover!(value > 0.0 && value < f32::MIN_POSITIVE); + kani::cover!(value == f32::MAX); +} diff --git a/firmware/libs/kaptan-math/src/lib.rs b/firmware/libs/kaptan-math/src/lib.rs index 4ce0452..fb1533e 100644 --- a/firmware/libs/kaptan-math/src/lib.rs +++ b/firmware/libs/kaptan-math/src/lib.rs @@ -1,19 +1,18 @@ -//! Minimal `f32` math shared by Kaptan navigation and control. -//! -//! This crate deliberately exposes only fixed-size vectors and a unit -//! quaternion. Frame markers carry no runtime data and prevent accidental -//! body/local-vector interchange at public navigation and control boundaries. +//! Fixed-size `f32` vectors and unit quaternions with typed coordinate frames. #![no_std] +#![forbid(unsafe_code)] use core::marker::PhantomData; use kaptan_err_reportable::KaptanError; +#[cfg(kani)] +mod kani_proofs; + +/// Squared-radian domain of the bounded SO(3) exponential and Jacobians. +pub const MAXIMUM_ROTATION_VECTOR_NORM_SQUARED: f32 = 4.0; + /// Failure reported by a checked mathematical operation. -/// -/// The flight path must treat one of these as a rejected input or an estimator -/// failure. In particular, no quaternion constructor converts an error into -/// the identity rotation. #[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)] pub enum MathError { /// An operand or intermediate value was not finite. @@ -22,7 +21,7 @@ pub enum MathError { /// A vector or quaternion did not have a representable non-zero norm. #[kaptan_error(id = 6002)] ZeroNorm, - /// A small-angle quaternion was requested outside its reviewed domain. + /// A small-angle quaternion was requested outside its supported domain. #[kaptan_error(id = 6003)] RotationIncrementOutOfRange, /// Rotation-matrix axes were not a finite, right-handed orthonormal basis. @@ -36,11 +35,16 @@ pub enum Ned {} /// Aircraft forward-right-down body frame. #[derive(Clone, Copy, Debug, Eq, PartialEq)] pub enum Frd {} -/// A physical sensor's locally declared coordinate frame. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub enum Sensor {} /// Three `f32` components expressed in one explicit frame. +/// +/// Different coordinate frames cannot be added implicitly. +/// ```compile_fail,E0308 +/// use kaptan_math::{Frd, Ned, Vector3}; +/// let body = Vector3::::zero(); +/// let local = Vector3::::zero(); +/// let _ = body.add(local); +/// ``` #[derive(Debug, PartialEq)] pub struct Vector3 { x: f32, @@ -67,20 +71,16 @@ impl Vector3 { } } - /// Returns the x component. pub const fn x(self) -> f32 { self.x } - /// Returns the y component. pub const fn y(self) -> f32 { self.y } - /// Returns the z component. pub const fn z(self) -> f32 { self.z } - /// Returns the zero vector. pub const fn zero() -> Self { Self::new(0.0, 0.0, 0.0) } @@ -124,79 +124,56 @@ impl Vector3 { self.dot(self) } - /// Euclidean norm using a bounded Newton iteration. + /// Euclidean norm, scaled to avoid intermediate overflow and underflow. pub fn norm(self) -> f32 { - sqrt(self.norm_squared()) + if !self.is_finite() { + return f32::NAN; + } + let scale = self.x.abs().max(self.y.abs()).max(self.z.abs()); + if scale == 0.0 { + return 0.0; + } + let scaled = Self::new(self.x / scale, self.y / scale, self.z / scale); + scale * sqrt(scaled.norm_squared()) } /// Returns a unit vector, rejecting zero and non-finite input. - /// - /// Scaling before normalization prevents an otherwise finite, large vector - /// from overflowing while its direction is still perfectly representable. + /// Scaling prevents overflow when the direction is representable. pub fn normalized(self) -> Option { if !self.is_finite() { return None; } let scale = self.x.abs().max(self.y.abs()).max(self.z.abs()); - if scale <= 1.0e-8 { + if scale == 0.0 { return None; } - let scaled = self.scale(1.0 / scale); + let scaled = Self::new(self.x / scale, self.y / scale, self.z / scale); let norm = sqrt(scaled.norm_squared()); // `scale` is a finite non-zero component, so `scaled` contains one - // component with magnitude one. Its squared norm is therefore finite - // and at least one; keep that proof local rather than carrying an - // unreachable fallback after the public input validation above. + // component with magnitude one. The squared norm is in [1, 3], so + // multiplying components in [-1, 1] by its reciprocal root is finite. debug_assert!(norm.is_finite()); debug_assert!(norm > 0.0); let normalized = scaled.scale(1.0 / norm); - normalized.is_finite().then_some(normalized) - } -} - -/// Two `f32` components expressed in one explicit frame. -#[derive(Debug, PartialEq)] -pub struct Vector2 { - x: f32, - y: f32, - _frame: PhantomData, -} - -impl Copy for Vector2 {} -impl Clone for Vector2 { - fn clone(&self) -> Self { - *self - } -} - -impl Vector2 { - /// Creates a two-dimensional vector. - pub const fn new(x: f32, y: f32) -> Self { - Self { - x, - y, - _frame: PhantomData, - } - } - /// Returns the x component. - pub const fn x(self) -> f32 { - self.x - } - /// Returns the y component. - pub const fn y(self) -> f32 { - self.y - } - /// Returns whether both components are finite. - pub fn is_finite(self) -> bool { - self.x.is_finite() && self.y.is_finite() + debug_assert!(normalized.is_finite()); + Some(normalized) } } /// A normalized Hamilton rotation from `From` into `To`. /// -/// Source and destination frames are zero-sized type parameters, so a caller -/// cannot apply a valid quaternion to a vector in an unrelated frame. The -/// navigation convention is body-to-NED. +/// A rotation accepts vectors in its source frame. +/// ```compile_fail,E0308 +/// use kaptan_math::{Frd, Ned, UnitQuaternion, Vector3}; +/// let body_to_local = UnitQuaternion::::identity(); +/// let _ = body_to_local.rotate(Vector3::::zero()); +/// ``` +/// Composition requires the inner destination to match the outer source. +/// ```compile_fail,E0308 +/// use kaptan_math::{Frd, Ned, UnitQuaternion}; +/// let body_to_local = UnitQuaternion::::identity(); +/// let _ = body_to_local.compose(body_to_local); +/// ``` #[derive(Clone, Copy, Debug, PartialEq)] pub struct UnitQuaternion { w: f32, @@ -220,16 +197,13 @@ impl UnitQuaternion { } /// Normalizes raw Hamilton components, rejecting non-finite or zero input. - /// - /// Values are scaled before the norm is evaluated, so valid directions - /// with very large finite components do not overflow to an identity-like - /// result. + /// Scaling prevents intermediate norm overflow. pub fn from_components(w: f32, x: f32, y: f32, z: f32) -> Result { if !w.is_finite() | !x.is_finite() | !y.is_finite() | !z.is_finite() { return Err(MathError::NonFinite); } let scale = w.abs().max(x.abs()).max(y.abs()).max(z.abs()); - if scale <= 1.0e-8 { + if scale == 0.0 { return Err(MathError::ZeroNorm); } let sw = w / scale; @@ -314,10 +288,8 @@ impl UnitQuaternion { } impl UnitQuaternion { - /// Builds an incremental same-frame rotation from a rotation vector in - /// radians. The flight estimator admits increments no larger than two - /// radians before calling this bounded polynomial, avoiding a hidden - /// `libm` dependency in the rate path. + /// Builds a same-frame rotation using a polynomial valid up to two radians. + /// Larger rotation vectors are rejected. pub fn from_small_rotation_vector(rotation: Vector3) -> Result { if !rotation.is_finite() { return Err(MathError::NonFinite); @@ -326,13 +298,21 @@ impl UnitQuaternion { if !rotation_squared.is_finite() { return Err(MathError::NonFinite); } - if rotation_squared > 4.0 { + if rotation_squared > MAXIMUM_ROTATION_VECTOR_NORM_SQUARED { return Err(MathError::RotationIncrementOutOfRange); } let half_squared = 0.25 * rotation_squared; - let sin_over_angle = 0.5 * (1.0 - half_squared / 6.0 + half_squared * half_squared / 120.0); + let half_fourth = half_squared * half_squared; + let half_sixth = half_fourth * half_squared; + let half_eighth = half_fourth * half_fourth; + let sin_over_angle = 0.5 + * (1.0 - half_squared / 6.0 + half_squared * half_squared / 120.0 + - half_sixth / 5_040.0 + + half_eighth / 362_880.0); let cosine = 1.0 - half_squared / 2.0 + half_squared * half_squared / 24.0 - - half_squared * half_squared * half_squared / 720.0; + - half_squared * half_squared * half_squared / 720.0 + + half_eighth / 40_320.0 + - half_eighth * half_squared / 3_628_800.0; // The expression is close to unit length for its stated domain. A // normalization removes accumulated polynomial/rounding error. Self::from_components( @@ -407,10 +387,8 @@ impl UnitQuaternion { /// Bounded square root for positive finite `f32` values. /// -/// A scaled exponent estimate plus four Newton iterations provides bounded -/// vector/quaternion normalization precision across the finite `f32` range. -/// Negative/non-finite input is rejected as `NaN` rather than silently -/// converted. +/// Uses an exponent estimate and four Newton iterations. Returns `NaN` for +/// negative or non-finite input. pub fn sqrt(value: f32) -> f32 { if !value.is_finite() || value < 0.0 { return f32::NAN; @@ -418,16 +396,11 @@ pub fn sqrt(value: f32) -> f32 { if value == 0.0 { return value; } - // The exponent shortcut below assumes a normal input. Scale a - // subnormal by an exact power of two, then undo half that exponent after - // the same bounded iteration. This keeps the smallest finite positive - // `f32` in the reviewed accuracy envelope without a libm dependency. + // The exponent estimate requires normal input. Scale subnormals by 2^24 + // and divide their square root by 2^12. if value < f32::MIN_POSITIVE { return sqrt(value * 16_777_216.0) / 4_096.0; } - // A bit-level exponent estimate makes the fixed iteration count accurate - // for subnormal through maximum finite inputs. It avoids linking a - // platform libm implementation in the embedded rate path. let mut estimate = f32::from_bits((value.to_bits() >> 1) + 0x1fc0_0000); let mut iteration = 0; while iteration < 4 { @@ -439,10 +412,9 @@ pub fn sqrt(value: f32) -> f32 { /// Applies the SO(3) right Jacobian to a vector in one frame. /// -/// The same bounded two-radian increment domain as -/// [`UnitQuaternion::from_small_rotation_vector`] avoids a hidden `libm` -/// dependency. The coefficients are fixed Taylor polynomials for -/// `(1 - cos(theta)) / theta²` and `(theta - sin(theta)) / theta³`. +/// Uses the two-radian domain of [`UnitQuaternion::from_small_rotation_vector`] +/// and Taylor polynomials for `(1 - cos(theta)) / theta²` and +/// `(theta - sin(theta)) / theta³`. pub fn right_jacobian_apply( rotation: Vector3, vector: Vector3, @@ -473,18 +445,21 @@ fn jacobian_apply( if !theta_squared.is_finite() { return Err(MathError::NonFinite); } - if theta_squared > 4.0 { + if theta_squared > MAXIMUM_ROTATION_VECTOR_NORM_SQUARED { return Err(MathError::RotationIncrementOutOfRange); } let theta_fourth = theta_squared * theta_squared; let theta_sixth = theta_fourth * theta_squared; let theta_eighth = theta_fourth * theta_fourth; + let theta_tenth = theta_eighth * theta_squared; let first_coefficient = 0.5 - theta_squared / 24.0 + theta_fourth / 720.0 - theta_sixth / 40_320.0 - + theta_eighth / 3_628_800.0; + + theta_eighth / 3_628_800.0 + - theta_tenth / 479_001_600.0; let second_coefficient = 1.0 / 6.0 - theta_squared / 120.0 + theta_fourth / 5_040.0 - theta_sixth / 362_880.0 - + theta_eighth / 39_916_800.0; + + theta_eighth / 39_916_800.0 + - theta_tenth / 6_227_020_800.0; let cross = rotation.cross(vector); let result = vector .add(cross.scale(first_cross_sign * first_coefficient)) @@ -665,12 +640,6 @@ mod tests { ] { assert!(!invalid.is_finite()); } - - let two = super::Vector2::::new(1.0, -2.0); - assert_eq!(two.clone(), two); - assert_eq!((two.x(), two.y()), (1.0, -2.0)); - assert!(two.is_finite()); - assert!(!super::Vector2::::new(f32::NAN, 0.0).is_finite()); } #[test] @@ -750,75 +719,35 @@ mod tests { ] { assert!(!vector.is_finite()); } - for components in [ + for (w, x, y, z) in [ (f32::NAN, 1.0, 0.0, 0.0), (1.0, f32::NAN, 0.0, 0.0), (1.0, 0.0, f32::NAN, 0.0), (1.0, 0.0, 0.0, f32::NAN), + (f32::INFINITY, 0.0, 0.0, 0.0), + (1.0, f32::INFINITY, 0.0, 0.0), + (1.0, 0.0, f32::INFINITY, 0.0), + (1.0, 0.0, 0.0, f32::INFINITY), ] { assert_eq!( - UnitQuaternion::::from_components( - components.0, - components.1, - components.2, - components.3, - ), + UnitQuaternion::::from_components(w, x, y, z), Err(MathError::NonFinite) ); } - assert_eq!( - UnitQuaternion::::from_components(f32::INFINITY, 0.0, 0.0, 0.0), - Err(MathError::NonFinite) - ); - assert_eq!( - UnitQuaternion::::from_components(1.0, f32::INFINITY, 0.0, 0.0), - Err(MathError::NonFinite) - ); - assert_eq!( - UnitQuaternion::::from_components(1.0, 0.0, f32::INFINITY, 0.0), - Err(MathError::NonFinite) - ); - assert_eq!( - UnitQuaternion::::from_components(1.0, 0.0, 0.0, f32::INFINITY), - Err(MathError::NonFinite) - ); - for quaternion in [ - UnitQuaternion:: { - w: f32::NAN, - x: 0.0, - y: 0.0, - z: 0.0, - _frames: PhantomData, - }, - UnitQuaternion:: { - w: 1.0, - x: f32::NAN, - y: 0.0, - z: 0.0, - _frames: PhantomData, - }, - UnitQuaternion:: { - w: 1.0, - x: 0.0, - y: f32::NAN, - z: 0.0, - _frames: PhantomData, - }, - UnitQuaternion:: { - w: 1.0, - x: 0.0, - y: 0.0, - z: f32::NAN, - _frames: PhantomData, - }, - UnitQuaternion:: { - w: 2.0, - x: 0.0, - y: 0.0, - z: 0.0, - _frames: PhantomData, - }, + for (w, x, y, z) in [ + (f32::NAN, 0.0, 0.0, 0.0), + (1.0, f32::NAN, 0.0, 0.0), + (1.0, 0.0, f32::NAN, 0.0), + (1.0, 0.0, 0.0, f32::NAN), + (2.0, 0.0, 0.0, 0.0), ] { + let quaternion = UnitQuaternion:: { + w, + x, + y, + z, + _frames: PhantomData, + }; assert!(!quaternion.is_normalized()); } for axes in [ diff --git a/firmware/libs/kaptan-math/tests/so3_reference.rs b/firmware/libs/kaptan-math/tests/so3_reference.rs new file mode 100644 index 0000000..e57e964 --- /dev/null +++ b/firmware/libs/kaptan-math/tests/so3_reference.rs @@ -0,0 +1,122 @@ +use kaptan_math::{Frd, UnitQuaternion, Vector3, left_jacobian_apply, right_jacobian_apply}; + +fn exp(v: [f64; 3]) -> [f64; 4] { + let norm = v.iter().map(|x| x * x).sum::().sqrt(); + let scale = if norm == 0.0 { + 0.5 + } else { + (norm / 2.0).sin() / norm + }; + [(norm / 2.0).cos(), v[0] * scale, v[1] * scale, v[2] * scale] +} + +fn multiply(a: [f64; 4], b: [f64; 4]) -> [f64; 4] { + [ + a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3], + a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2], + a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1], + a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0], + ] +} + +#[test] +fn bounded_exponential_and_jacobians_match_f64_group_differentials() { + // Axial and oblique directions across the admitted domain. Central + // differences of Exp(phi + d) Exp(-phi) independently fix the left sign. + for direction in [ + [1.0_f64, 0.0, 0.0], + [0.0, 0.0, -1.0], + [2.0 / 3.0, -1.0 / 3.0, 2.0 / 3.0], + ] { + for magnitude in [0.0_f64, 1.0e-7, 0.001, 0.2, 1.0, 1.5, 1.999, 2.0] { + let phi = direction.map(|x| (x * magnitude) as f32); + let phi64 = phi.map(f64::from); + let expected = exp(phi64); + let rotation = Vector3::::new(phi[0], phi[1], phi[2]); + let actual = UnitQuaternion::from_small_rotation_vector(rotation).unwrap(); + for (actual, expected) in [actual.w(), actual.x(), actual.y(), actual.z()] + .into_iter() + .zip(expected) + { + // Below two f32 ulps, including the polynomial endpoint error. + assert!((f64::from(actual) - expected).abs() < 1.5e-7, "{phi:?}"); + } + let inverse = [expected[0], -expected[1], -expected[2], -expected[3]]; + for axis in 0..3 { + let mut basis = [0.0; 3]; + basis[axis] = 1.0; + let basis = Vector3::new(basis[0], basis[1], basis[2]); + let mut plus = phi64; + let mut minus = phi64; + const STEP: f64 = 1.0e-6; + plus[axis] += STEP; + minus[axis] -= STEP; + for (actual, positive, negative) in [ + ( + left_jacobian_apply(rotation, basis).unwrap(), + multiply(exp(plus), inverse), + multiply(exp(minus), inverse), + ), + ( + right_jacobian_apply(rotation, basis).unwrap(), + multiply(inverse, exp(plus)), + multiply(inverse, exp(minus)), + ), + ] { + for (row, value) in [actual.x(), actual.y(), actual.z()].into_iter().enumerate() + { + // The infinitesimal rotation is twice quaternion xyz. + let reference = (positive[row + 1] - negative[row + 1]) / STEP; + assert!( + (f64::from(value) - reference).abs() < 2.5e-6, + "{phi:?}, axis {axis}, row {row}" + ); + } + } + } + } + } +} + +#[test] +fn normalization_is_scale_invariant_across_f32_exponents() { + for scale in [ + f32::from_bits(1), + f32::MIN_POSITIVE, + 1.0e-30, + 1.0e-9, + 1.0, + 1.0e30, + f32::MAX, + ] { + let vector = Vector3::::new(scale, -scale, scale); + let normalized = vector.normalized().unwrap(); + for component in [normalized.x(), -normalized.y(), normalized.z()] { + assert!((f64::from(component) - 1.0 / 3.0_f64.sqrt()).abs() < 1.0e-7); + } + let quaternion = + UnitQuaternion::::from_components(scale, scale, -scale, -scale).unwrap(); + assert_eq!( + [ + quaternion.w(), + quaternion.x(), + quaternion.y(), + quaternion.z() + ], + [0.5, 0.5, -0.5, -0.5] + ); + assert_eq!(Vector3::::new(scale, 0.0, 0.0).norm(), scale); + } +} + +#[test] +fn norm_distinguishes_zero_invalid_input_and_unrepresentable_magnitude() { + assert_eq!(Vector3::::zero().norm(), 0.0); + for invalid in [f32::NAN, f32::INFINITY, f32::NEG_INFINITY] { + assert!(Vector3::::new(1.0, invalid, 2.0).norm().is_nan()); + } + let large = Vector3::::new(f32::MAX, f32::MAX, f32::MAX); + assert_eq!(large.norm(), f32::INFINITY); + // The direction is still representable when the magnitude is not. + assert!((f64::from(large.normalized().unwrap().norm()) - 1.0).abs() < 2.0e-7); +} diff --git a/firmware/libs/kaptan-mavlink/Cargo.toml b/firmware/libs/kaptan-mavlink/Cargo.toml index 18f39db..bd31005 100644 --- a/firmware/libs/kaptan-mavlink/Cargo.toml +++ b/firmware/libs/kaptan-mavlink/Cargo.toml @@ -9,14 +9,10 @@ version.workspace = true workspace = true [features] -# Sensor-domain adapters are physical telemetry only. sensor-telemetry = ["dep:kaptan-sensor"] [dependencies] -kaptan-err-reportable = { path = "../kaptan-err-reportable" } -kaptan-dma = { path = "../kaptan-dma" } kaptan-sensor = { path = "../kaptan-sensor", optional = true } -kaptan-tx = { path = "../kaptan-tx" } mavlink-core = { version = "0.18", default-features = false, features = [ "embedded", ] } diff --git a/firmware/libs/kaptan-mavlink/build.rs b/firmware/libs/kaptan-mavlink/build.rs index eff0bb9..7e99cdc 100644 --- a/firmware/libs/kaptan-mavlink/build.rs +++ b/firmware/libs/kaptan-mavlink/build.rs @@ -5,9 +5,7 @@ fn main() { let definitions = manifest_directory.join("definitions"); let dialect = definitions.join("kaptan.xml"); println!("cargo::rerun-if-changed={}", definitions.display()); - // The upstream generator conditionally emits derives for these optional - // ecosystem crates. Kaptan deliberately does not expose those incomplete - // features, so tell rustc the generated inactive cfgs are intentional. + // The generator emits these feature gates, but this crate leaves them disabled. println!("cargo::rustc-check-cfg=cfg(feature, values(\"serde\", \"arbitrary\", \"ts-rs\"))"); let output_directory = std::env::var("OUT_DIR").unwrap(); mavlink_bindgen::generate( @@ -15,9 +13,7 @@ fn main() { &output_directory, ) .unwrap(); - // Bindgen's index feature-gates every dialect module. Kaptan has exactly - // one unconditional dialect, so load its generated file as a normal Rust - // module instead of exposing a redundant Cargo feature. + // Bypass bindgen's feature-gated index to load the Kaptan dialect unconditionally. std::fs::write( PathBuf::from(output_directory).join("kaptan_module.rs"), "#[allow(non_camel_case_types, non_snake_case)]\n\ diff --git a/firmware/libs/kaptan-mavlink/definitions/kaptan.xml b/firmware/libs/kaptan-mavlink/definitions/kaptan.xml index 0375020..b827b74 100644 --- a/firmware/libs/kaptan-mavlink/definitions/kaptan.xml +++ b/firmware/libs/kaptan-mavlink/definitions/kaptan.xml @@ -1,7 +1,6 @@ - + publish/log.xml publish/error.xml publish/heartbeat.xml @@ -10,4 +9,5 @@ publish/optical_flow.xml transfer/file_control.xml transfer/file_data.xml + diff --git a/firmware/libs/kaptan-mavlink/definitions/publish/error.xml b/firmware/libs/kaptan-mavlink/definitions/publish/error.xml index eb14730..a2baf20 100644 --- a/firmware/libs/kaptan-mavlink/definitions/publish/error.xml +++ b/firmware/libs/kaptan-mavlink/definitions/publish/error.xml @@ -4,7 +4,7 @@ 0 - Reportable firmware error emitted when an Err leaves an annotated reporting point. This is distinct from every logging macro, which emits LOG. The stable ID is explicitly assigned to the Rust error variant. Protocol failures remain authoritative in their owning response. + Reportable firmware error emitted when an Err leaves an annotated function. Logging macros emit LOG instead. Protocol failures are reported in their operation responses. Stable explicitly assigned error catalog ID. Zero is reserved. Zero-padded compile-time file:line of the reporting point. diff --git a/firmware/libs/kaptan-mavlink/definitions/publish/heartbeat.xml b/firmware/libs/kaptan-mavlink/definitions/publish/heartbeat.xml index 2cd8a8d..135adcd 100644 --- a/firmware/libs/kaptan-mavlink/definitions/publish/heartbeat.xml +++ b/firmware/libs/kaptan-mavlink/definitions/publish/heartbeat.xml @@ -11,7 +11,7 @@ - Periodic liveness and discovery publication for the initialized output-inhibited firmware composition. + Liveness and discovery while initialized with outputs inhibited. diff --git a/firmware/libs/kaptan-mavlink/definitions/publish/imu.xml b/firmware/libs/kaptan-mavlink/definitions/publish/imu.xml index 3e5e8ba..ab67a0a 100644 --- a/firmware/libs/kaptan-mavlink/definitions/publish/imu.xml +++ b/firmware/libs/kaptan-mavlink/definitions/publish/imu.xml @@ -4,10 +4,10 @@ 0 - One accepted body-frame inertial observation in the native bounded sensor representation. + Body-frame inertial observation. Payload offsets are time_us 0, angular_rate 8, acceleration 20 and temperature 26. Payload length is 28 bytes, extra CRC 22. Flight-controller monotonic acquisition time in microseconds. Body-frame acceleration in milligravity. - Body-frame angular rate in milliradians per second. + Body-frame angular rate in milliradians per second. Preserves the full MPU6000 plus or minus 2000 degrees per second decoded range. Sensor temperature in centidegrees Celsius. diff --git a/firmware/libs/kaptan-mavlink/definitions/publish/log.xml b/firmware/libs/kaptan-mavlink/definitions/publish/log.xml index 735b51c..65e0c46 100644 --- a/firmware/libs/kaptan-mavlink/definitions/publish/log.xml +++ b/firmware/libs/kaptan-mavlink/definitions/publish/log.xml @@ -21,7 +21,7 @@ - Bounded human-readable runtime log emitted by every Kaptan logging macro, including error. The receiver needs only level, flags, and the valid UTF-8 text prefix. MAVLink framing, bounded admission, DMA, and retries remain platform-owned. + Runtime log emitted by Kaptan logging macros, including error. Kaptan wire level. TRUNCATED=1. Receivers ignore unrecognized bits. Number of valid UTF-8 bytes in message; at most 160. diff --git a/firmware/libs/kaptan-mavlink/definitions/publish/range.xml b/firmware/libs/kaptan-mavlink/definitions/publish/range.xml index 765d6e4..3ef3a48 100644 --- a/firmware/libs/kaptan-mavlink/definitions/publish/range.xml +++ b/firmware/libs/kaptan-mavlink/definitions/publish/range.xml @@ -5,7 +5,7 @@ One accepted downward range observation from the board's selected vertical sensor route. - Flight-controller monotonic measurement-request time in microseconds. + Producer timestamp in flight-controller monotonic microseconds: measurement request for LIDAR-Lite, frame reception for MTF-01. Measured downward distance in millimetres. diff --git a/firmware/libs/kaptan-mavlink/src/encoder.rs b/firmware/libs/kaptan-mavlink/src/encoder.rs index 6dac08a..97cd538 100644 --- a/firmware/libs/kaptan-mavlink/src/encoder.rs +++ b/firmware/libs/kaptan-mavlink/src/encoder.rs @@ -15,7 +15,7 @@ use crate::{ generated::kaptan::{HEARTBEAT_DATA, HeartbeatState, MavMessage}, }; -/// Serializes FC-domain data with MAVLink sender identity and sequence state. +/// Encodes Kaptan messages with one endpoint's sender identity and packet sequence. pub struct MavlinkEncoder { system_id: u8, component_id: u8, @@ -46,8 +46,7 @@ impl MavlinkEncoder { header } - /// Encodes one concrete generated Kaptan record through this endpoint's - /// sole header-sequence owner. + /// Encodes a Kaptan record using this endpoint's packet sequence. pub fn encode>(&mut self, data: &M) -> MavlinkFrame { let mut raw = MavlinkFrame::new(); raw.serialize_message_data(self.header(), data); @@ -155,7 +154,7 @@ mod tests { let snapshot = ImuSnapshot::new( Timestamp::from_microseconds(123), [1, 2, 3], - [4, 5, 6], + [-34_871, 34_870, -8_731], 2_500, ); @@ -166,7 +165,40 @@ mod tests { Some(MavMessage::IMU(data)) if data.time_us == 123 && data.acceleration_milligravity == [1, 2, 3] - && data.angular_rate_milliradians_per_second == [4, 5, 6] + && data.angular_rate_milliradians_per_second == [-34_871, 34_870, -8_731] + && data.temperature_centidegrees_celsius == 2_500 + )); + } + + #[cfg(feature = "sensor-telemetry")] + #[test] + fn imu_wire_layout_preserves_the_full_signed_gyro_representation() { + let mut encoder = MavlinkEncoder::new(1, 1); + let snapshot = ImuSnapshot::new( + Timestamp::from_microseconds(123), + [1, -2, 3], + [i32::MIN, i32::MAX, -34_871], + 2_500, + ); + let frame = encoder.imu_snapshot(&snapshot); + assert_eq!(frame.raw_bytes()[1], 28); + // MAVLink orders fields by scalar width. Widening gyro moves it ahead + // of acceleration. Literal little-endian bytes pin that new contract. + assert_eq!( + &frame.raw_bytes()[10..38], + &[ + 123, 0, 0, 0, 0, 0, 0, 0, // time_us: u64 + 0, 0, 0, 0x80, 0xff, 0xff, 0xff, 0x7f, 0xc9, 0x77, 0xff, 0xff, // gyro: i32[3] + 1, 0, 0xfe, 0xff, 3, 0, // acceleration: i16[3] + 0xc4, 0x09, // temperature: i16 + ] + ); + assert!(matches!( + parse_test_message(&frame), + Some(MavMessage::IMU(data)) + if data.time_us == 123 + && data.angular_rate_milliradians_per_second == [i32::MIN, i32::MAX, -34_871] + && data.acceleration_milligravity == [1, -2, 3] && data.temperature_centidegrees_celsius == 2_500 )); } @@ -219,8 +251,10 @@ mod tests { assert_eq!(HEARTBEAT_DATA::ENCODED_LEN, 1); assert_eq!(IMU_DATA::NAME, "IMU"); assert_eq!(IMU_DATA::ID, 51_028); - assert_eq!(IMU_DATA::EXTRA_CRC, 194); - assert_eq!(IMU_DATA::ENCODED_LEN, 22); + // Independently calculated X.25 extra CRC over IMU's wire-ordered + // field types/names/array lengths: u64, i32[3], i16[3], i16. + assert_eq!(IMU_DATA::EXTRA_CRC, 22); + assert_eq!(IMU_DATA::ENCODED_LEN, 28); assert_eq!(RANGE_DATA::NAME, "RANGE"); assert_eq!(RANGE_DATA::ID, 51_029); assert_eq!(RANGE_DATA::EXTRA_CRC, 44); diff --git a/firmware/libs/kaptan-mavlink/src/lib.rs b/firmware/libs/kaptan-mavlink/src/lib.rs index 333fd66..be3f4c6 100644 --- a/firmware/libs/kaptan-mavlink/src/lib.rs +++ b/firmware/libs/kaptan-mavlink/src/lib.rs @@ -2,16 +2,17 @@ #![no_std] -/// MAVLink frame encoding API for boards. +/// MAVLink frame encoding API. pub mod encoder; -/// Serial-byte MAVLink receive API for boards. +/// Incremental MAVLink byte-stream receive API. pub mod receiver; -/// UART DMA MAVLink frame transmission API for boards. -pub mod transmitter; -/// An owned MAVLink 2 frame suitable for UART transmission. +/// An owned MAVLink 2 frame. pub type MavlinkFrame = mavlink_core::MAVLinkV2MessageRaw; +/// Generated-message metadata and serialization contract. +pub use mavlink_core::MessageData; + /// Generated wire types for Kaptan's sole MAVLink dialect. pub mod generated { include!(concat!(env!("OUT_DIR"), "/kaptan_module.rs")); diff --git a/firmware/libs/kaptan-mavlink/src/transmitter.rs b/firmware/libs/kaptan-mavlink/src/transmitter.rs deleted file mode 100644 index 3e42b1d..0000000 --- a/firmware/libs/kaptan-mavlink/src/transmitter.rs +++ /dev/null @@ -1,313 +0,0 @@ -//! MAVLink frame encoding and UART transmission API. - -use kaptan_dma::{DmaTxBufferFactory, DmaTxEngine, TxCompletion}; -use kaptan_err_reportable::KaptanError; -use kaptan_tx::{PendingFrame, TransmitQueue}; - -use crate::MavlinkFrame; - -/// Delivery importance used for bounded MAVLink transmission admission. -pub use kaptan_tx::{EnqueueResult, FrameClass}; - -/// Failure while preparing or starting a MAVLink frame transfer. -#[derive(Debug, KaptanError)] -pub enum MavlinkTransmitFault { - /// DMA-visible frame preparation failed. - #[kaptan_error(id = 10001)] - Memory(MemoryError), - /// The UART DMA route rejected a transfer start. - #[kaptan_error(id = 10002)] - Engine(EngineError), -} - -/// Combined bounded-admission and DMA-start outcome. -/// -/// The outcomes stay distinct because admission decides whether the source may -/// acknowledge its frame, while a failed start keeps reserved work for retry. -#[must_use] -pub struct AdmissionStart { - /// Queue admission for the source frame. - pub admission: EnqueueResult, - /// One bounded attempt to start pending DMA work. - pub start: Result<(), MavlinkTransmitFault>, -} - -/// Owns MAVLink frame admission and UART DMA lifecycle. -/// -/// Exactly one worker invokes this type. It accepts local encoded frames and -/// received frames alike; the caller owns the policy that decides which frames -/// are transmitted. -pub struct MavlinkTransmitter { - transmit: TransmitQueue, - memory: Memory, - engine: Engine, -} - -impl MavlinkTransmitter { - /// Creates an empty MAVLink transmitter. - pub const fn new(memory: Memory, engine: Engine) -> Self { - Self { - transmit: TransmitQueue::new(), - memory, - engine, - } - } -} - -impl MavlinkTransmitter -where - Memory: DmaTxBufferFactory, - Engine: DmaTxEngine, -{ - /// Admits one frame and attempts one bounded DMA start while preserving - /// both outcomes. A start failure retains the selected frame in the queue - /// reservation for a later service-turn retry; it does not change whether - /// the newly submitted source frame was admitted. - pub fn admit_and_start( - &mut self, - class: FrameClass, - frame: MavlinkFrame, - ) -> AdmissionStart { - let admission = self.transmit.enqueue(PendingFrame { class, frame }); - let start = self.start_next(); - AdmissionStart { admission, start } - } - - /// Releases a terminal DMA frame and opportunistically starts the next. - /// - /// The returned class is the frame that reached a terminal DMA result, - /// not a class selected from newly pending work. A policy can therefore - /// distinguish completion classes without inferring them from newly - /// pending work. - pub fn on_dma_complete( - &mut self, - completion: TxCompletion, - ) -> Result, MavlinkTransmitFault> { - let completed = self.complete_dma(completion)?; - if completion == TxCompletion::Complete { - self.start_next()?; - } - Ok(completed) - } - - fn complete_dma( - &mut self, - completion: TxCompletion, - ) -> Result, MavlinkTransmitFault> { - match completion { - TxCompletion::NotPending => Ok(None), - TxCompletion::Complete => Ok(self.transmit.complete().map(|frame| frame.class)), - TxCompletion::TransferFault | TxCompletion::Halted => { - Ok(self.transmit.fail().map(|frame| frame.class)) - } - } - } - - fn start_next( - &mut self, - ) -> Result<(), MavlinkTransmitFault> { - let (transmit, memory, engine) = (&mut self.transmit, &mut self.memory, &mut self.engine); - transmit - .start_with(|frame| { - let buffer = memory - .prepare(frame.raw_bytes()) - .map_err(MavlinkTransmitFault::Memory)?; - engine.start(buffer).map_err(MavlinkTransmitFault::Engine) - }) - .map(|_| ()) - } -} - -#[cfg(test)] -mod tests { - use super::{ - EnqueueResult, FrameClass, MavlinkTransmitFault, MavlinkTransmitter, PendingFrame, - }; - use crate::encoder::MavlinkEncoder; - use kaptan_dma::{DmaReadableBuffer, DmaTxBufferFactory, DmaTxEngine, TxCompletion}; - - struct Memory; - - impl DmaTxBufferFactory for Memory { - type Error = core::convert::Infallible; - - fn prepare<'a>(&mut self, bytes: &'a [u8]) -> Result, Self::Error> { - // SAFETY: this host-only test keeps the queue's active frame alive. - Ok(unsafe { DmaReadableBuffer::new_unchecked(bytes) }) - } - } - - struct Engine; - - impl DmaTxEngine for Engine { - type StartError = core::convert::Infallible; - - fn start(&mut self, _: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> { - Ok(()) - } - } - - struct RecordingEngine { - started_sequences: [u8; 4], - starts: usize, - } - - #[derive(Clone, Copy, Debug, Eq, PartialEq)] - enum RetryError { - Busy, - } - - struct BusyOnceEngine { - busy_once: bool, - starts: usize, - } - - impl DmaTxEngine for BusyOnceEngine { - type StartError = RetryError; - - fn start(&mut self, _: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> { - if self.busy_once { - self.busy_once = false; - return Err(RetryError::Busy); - } - self.starts += 1; - Ok(()) - } - } - - impl DmaTxEngine for RecordingEngine { - type StartError = core::convert::Infallible; - - fn start(&mut self, buffer: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> { - self.started_sequences[self.starts] = buffer.bytes()[4]; - self.starts += 1; - Ok(()) - } - } - - #[test] - fn transfer_fault_drops_only_the_active_frame() { - let mut encoder = MavlinkEncoder::new(1, 1); - let mut transmitter = MavlinkTransmitter::::new(Memory, Engine); - assert!( - transmitter - .admit_and_start(FrameClass::BestEffort, encoder.heartbeat()) - .start - .is_ok() - ); - assert!( - transmitter - .admit_and_start(FrameClass::BestEffort, encoder.heartbeat()) - .start - .is_ok() - ); - assert!(transmitter.transmit.is_active()); - - transmitter - .on_dma_complete(TxCompletion::TransferFault) - .unwrap(); - assert!(!transmitter.transmit.is_active()); - } - - #[test] - fn reports_critical_queue_saturation_to_the_policy_owner() { - let mut encoder = MavlinkEncoder::new(1, 1); - let mut transmitter = MavlinkTransmitter::::new(Memory, Engine); - - assert_eq!( - transmitter - .admit_and_start(FrameClass::Critical, encoder.heartbeat()) - .admission, - EnqueueResult::Enqueued - ); - assert_eq!( - transmitter - .admit_and_start(FrameClass::Critical, encoder.heartbeat()) - .admission, - EnqueueResult::Enqueued - ); - assert_eq!( - transmitter - .admit_and_start(FrameClass::Critical, encoder.heartbeat()) - .admission, - EnqueueResult::CriticalQueueFull - ); - } - - #[test] - fn terminal_completion_can_defer_a_transient_engine_busy_without_losing_frame_order() { - let mut encoder = MavlinkEncoder::new(1, 1); - let mut transmitter = MavlinkTransmitter::::new( - Memory, - BusyOnceEngine { - busy_once: false, - starts: 0, - }, - ); - transmitter.transmit.enqueue(PendingFrame { - class: FrameClass::Critical, - frame: encoder.heartbeat(), - }); - transmitter.start_next().unwrap(); - assert_eq!(transmitter.engine.starts, 1); - transmitter.transmit.enqueue(PendingFrame { - class: FrameClass::Critical, - frame: encoder.heartbeat(), - }); - - // Model the STM32 case: TCIF is acknowledged but CR.EN still reports - // busy for the first attempt to start the successor. `complete_dma` - // deliberately does not start it in the terminal IRQ path. - assert_eq!( - transmitter.complete_dma(TxCompletion::Complete).unwrap(), - Some(FrameClass::Critical) - ); - assert_eq!(transmitter.engine.starts, 1); - transmitter.engine.busy_once = true; - assert!(matches!( - transmitter.start_next(), - Err(MavlinkTransmitFault::Engine(RetryError::Busy)) - )); - assert!(!transmitter.transmit.is_active()); - - transmitter.start_next().unwrap(); - assert!(transmitter.transmit.is_active()); - assert_eq!(transmitter.engine.starts, 2); - } - - #[test] - fn completion_retry_starts_pending_best_effort_before_later_critical_frame() { - let mut encoder = MavlinkEncoder::new(1, 1); - let mut transmitter = MavlinkTransmitter::::new( - Memory, - RecordingEngine { - started_sequences: [0; 4], - starts: 0, - }, - ); - // A critical frame is active. A best-effort sample arrives behind it - // before DMA completion. - transmitter.transmit.enqueue(PendingFrame { - class: FrameClass::Critical, - frame: encoder.heartbeat(), - }); - transmitter.start_next().unwrap(); - transmitter.transmit.enqueue(PendingFrame { - class: FrameClass::BestEffort, - frame: encoder.heartbeat(), - }); - - // The completion worker performs its bounded retry before the next - // periodic sender can enqueue fresh critical traffic. The existing - // best-effort frame therefore cannot starve behind later traffic. - transmitter.complete_dma(TxCompletion::Complete).unwrap(); - transmitter.start_next().unwrap(); - transmitter.transmit.enqueue(PendingFrame { - class: FrameClass::Critical, - frame: encoder.heartbeat(), - }); - - assert_eq!(transmitter.engine.starts, 2); - assert_eq!(&transmitter.engine.started_sequences[..2], &[0, 1]); - } -} diff --git a/firmware/libs/kaptan-navigation/Cargo.toml b/firmware/libs/kaptan-navigation/Cargo.toml index 36f8231..fd31064 100644 --- a/firmware/libs/kaptan-navigation/Cargo.toml +++ b/firmware/libs/kaptan-navigation/Cargo.toml @@ -10,11 +10,10 @@ workspace = true [features] default = [] -# Enables the provisional raw-rate estimator used only by the internal SITL -# executable. It is not part of a board build. -sitl = ["dep:kaptan-sensor"] +# Driver snapshot conversion into navigation observations. The estimator is +# available independently of physical sensor adapters. +sensor-snapshots = ["dep:kaptan-sensor"] [dependencies] -kaptan-err-reportable = { path = "../kaptan-err-reportable" } kaptan-math = { path = "../kaptan-math" } kaptan-sensor = { path = "../kaptan-sensor", optional = true } kaptan-time = { path = "../kaptan-time" } diff --git a/firmware/libs/kaptan-navigation/src/eskf.rs b/firmware/libs/kaptan-navigation/src/eskf.rs deleted file mode 100644 index 7be882f..0000000 --- a/firmware/libs/kaptan-navigation/src/eskf.rs +++ /dev/null @@ -1,4216 +0,0 @@ -//! Fixed-size multiplicative error-state Kalman filter. - -use kaptan_err_reportable::KaptanError; -use kaptan_math::{Frd, MathError, Ned, Sensor, UnitQuaternion, Vector3, sqrt}; -use kaptan_time::Timestamp; - -use crate::observation::{ImuNoise, ImuObservation}; -use crate::{ - AccelerationFrd, AngularRateFrd, CalibrationRevision, FrameId, GRAVITY_MPS2, - LocalPositionVelocityObservation, ObservationSequence, OpticalFlowObservation, PositionNed, - RangeObservation, ScalarVariance, SensorExtrinsics, SourceId, Validity, VelocityNed, -}; - -const INERTIAL_DIM: usize = 15; -const TERRAIN_DIM: usize = 16; -const MAX_DIM: usize = TERRAIN_DIM; -const MATRIX: usize = MAX_DIM * MAX_DIM; -const POSITION: usize = 0; -const VELOCITY: usize = 3; -const ATTITUDE: usize = 6; -const GYRO_BIAS: usize = 9; -const ACCEL_BIAS: usize = 12; -const TERRAIN: usize = 15; - -// `UnitQuaternion::from_small_rotation_vector` is a bounded polynomial. This -// keeps every estimator call in its accurate operating region even if a board -// deliberately configures an IMU timeout longer than the default. -const MAXIMUM_ROTATION_INCREMENT_RADIANS_SQUARED: f32 = 4.0; - -/// Configured process, integrity, and plausibility limits for one estimator. -#[derive(Clone, Copy, Debug, PartialEq)] -pub struct EstimatorConfig { - /// Down-positive gravity used in NED propagation, m/s². - pub gravity_mps2: f32, - /// Continuous gyro white-noise variance, (rad/s)²/s. - pub gyro_noise_variance: f32, - /// Continuous accelerometer white-noise variance, (m/s²)²/s. - pub acceleration_noise_variance: f32, - /// Gyro-bias random-walk variance, (rad/s)²/s. - pub gyro_bias_walk_variance: f32, - /// Accelerometer-bias random-walk variance, (m/s²)²/s. - pub acceleration_bias_walk_variance: f32, - /// Terrain-down random-walk variance, m²/s. - pub terrain_walk_variance: f32, - /// Largest accepted IMU interval. Larger gaps require reinitialization. - pub maximum_imu_interval_microseconds: u64, - /// Minimum cosine of a downward range ray in NED. - pub minimum_range_down_cosine: f32, - /// Inclusive physical range lower bound in metres. - pub minimum_range_metres: f32, - /// Inclusive physical range upper bound in metres. - pub maximum_range_metres: f32, - /// Minimum quality for range fusion. - pub minimum_range_quality: u8, - /// Minimum quality for optical-flow fusion. - pub minimum_flow_quality: u8, - /// Largest plausible ground speed accepted from optical flow, m/s. - pub maximum_flow_speed_metres_per_second: f32, - /// One-degree-of-freedom chi-square gate. - pub range_gate_chi_square: f32, - /// Two-degree-of-freedom chi-square gate. - pub flow_gate_chi_square: f32, - /// Largest local-NED coordinate magnitude accepted from a position source. - pub maximum_local_position_metres: f32, - /// Largest local-NED speed accepted from a position source. - pub maximum_local_speed_metres_per_second: f32, - /// One-degree-of-freedom chi-square gate for local position/velocity axes. - pub local_state_gate_chi_square: f32, -} - -impl Default for EstimatorConfig { - fn default() -> Self { - Self { - gravity_mps2: GRAVITY_MPS2, - gyro_noise_variance: 2.5e-5, - acceleration_noise_variance: 2.5e-3, - gyro_bias_walk_variance: 1.0e-8, - acceleration_bias_walk_variance: 1.0e-6, - terrain_walk_variance: 1.0e-4, - maximum_imu_interval_microseconds: 20_000, - minimum_range_down_cosine: 0.5, - minimum_range_metres: 0.05, - maximum_range_metres: 40.0, - minimum_range_quality: 1, - minimum_flow_quality: 30, - maximum_flow_speed_metres_per_second: 25.0, - range_gate_chi_square: 6.635, - flow_gate_chi_square: 9.210, - maximum_local_position_metres: 10_000.0, - maximum_local_speed_metres_per_second: 100.0, - local_state_gate_chi_square: 16.0, - } - } -} - -impl EstimatorConfig { - fn is_valid(self) -> bool { - self.gravity_mps2.is_finite() - && self.gravity_mps2 > 0.0 - && self.gyro_noise_variance.is_finite() - && self.gyro_noise_variance >= 0.0 - && self.acceleration_noise_variance.is_finite() - && self.acceleration_noise_variance >= 0.0 - && self.gyro_bias_walk_variance.is_finite() - && self.gyro_bias_walk_variance >= 0.0 - && self.acceleration_bias_walk_variance.is_finite() - && self.acceleration_bias_walk_variance >= 0.0 - && self.terrain_walk_variance.is_finite() - && self.terrain_walk_variance >= 0.0 - && self.maximum_imu_interval_microseconds > 0 - && self.minimum_range_down_cosine.is_finite() - && self.minimum_range_down_cosine > 0.0 - && self.minimum_range_down_cosine <= 1.0 - && self.minimum_range_metres.is_finite() - && self.minimum_range_metres > 0.0 - && self.maximum_range_metres.is_finite() - && self.maximum_range_metres >= self.minimum_range_metres - && self.maximum_flow_speed_metres_per_second.is_finite() - && self.maximum_flow_speed_metres_per_second > 0.0 - && self.range_gate_chi_square.is_finite() - && self.range_gate_chi_square > 0.0 - && self.flow_gate_chi_square.is_finite() - && self.flow_gate_chi_square > 0.0 - && self.maximum_local_position_metres.is_finite() - && self.maximum_local_position_metres > 0.0 - && self.maximum_local_speed_metres_per_second.is_finite() - && self.maximum_local_speed_metres_per_second > 0.0 - && self.local_state_gate_chi_square.is_finite() - && self.local_state_gate_chi_square > 0.0 - } -} - -/// Bounded source-isolation policy. Four sources cover the current supported -/// sensor paths without allocating a map in a flight path. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct IntegrityConfig { - /// Maximum receipt-time age for an aided capability. - pub maximum_age_microseconds: u64, - /// Consecutive invalid/plausibility/gate failures before isolation. - pub rejection_limit: u8, - /// Consecutive gated observations needed to re-admit an isolated source. - pub readmission_accepts: u8, -} - -impl Default for IntegrityConfig { - fn default() -> Self { - Self { - maximum_age_microseconds: 250_000, - rejection_limit: 3, - readmission_accepts: 2, - } - } -} - -impl IntegrityConfig { - fn is_valid(self) -> bool { - self.maximum_age_microseconds > 0 - && self.rejection_limit > 0 - && self.readmission_accepts > 0 - } -} - -/// Current acceptance state for a configured aiding source. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub enum SourceStatus { - /// Source has not yet published an accepted observation. - Unknown, - /// Source has a fresh accepted observation. - Healthy, - /// Source is older than its configured freshness bound. - Stale, - /// Source is withheld after repeated invalid or gated observations. - Isolated, -} - -/// Truthful estimator capability report. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct EstimatorCapabilities { - /// Attitude is initialized and its covariance is finite. - pub attitude: bool, - /// Terrain-relative vertical position has fresh accepted range aiding. - pub terrain_relative_height: bool, - /// Terrain-relative horizontal velocity has fresh accepted flow aiding. - pub terrain_relative_velocity: bool, - /// Absolute launch-local position has fresh aiding. - pub local_position: bool, - /// Absolute launch-local velocity has fresh aiding. - pub local_velocity: bool, -} - -/// Estimator state and integrity summary for safety/diagnostics. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct EstimatorHealth { - /// No state/covariance component is non-finite. - pub finite: bool, - /// Estimator has passed stationary initialization. - pub initialized: bool, - /// Source status for the most recent range source. - pub range: SourceStatus, - /// Source status for the most recent optical-flow source. - pub optical_flow: SourceStatus, - /// Source status for the most recent local position/velocity source. - pub local_position_velocity: SourceStatus, - /// Capabilities that are actually observable with fresh inputs. - pub capabilities: EstimatorCapabilities, -} - -/// Propagation failure that leaves the nominal state unchanged. -#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)] -pub enum PropagationError { - /// An IMU observation was invalid or was not calibrated into FRD. - #[kaptan_error(id = 8001)] - InvalidImu, - /// IMU source differs from the stream bound during stationary initialization. - #[kaptan_error(id = 8002)] - ImuSourceMismatch, - /// IMU calibration differs from the stream bound during stationary initialization. - #[kaptan_error(id = 8003)] - ImuCalibrationMismatch, - /// IMU sequence repeated or moved backwards within the bound stream. - #[kaptan_error(id = 8004)] - ReplayedImu, - /// IMU measurement time did not advance strictly. - #[kaptan_error(id = 8005)] - NonMonotonicTimestamp, - /// The interval exceeded the bounded propagation contract. - #[kaptan_error(id = 8006)] - ImuGapTooLarge, - /// The IMU rate and interval exceed the bounded quaternion increment. - #[kaptan_error(id = 8007)] - RotationIncrementTooLarge, - /// Input or resulting state was non-finite. - #[kaptan_error(id = 8008)] - NonFinite, - /// The candidate state or covariance failed validation; reinitialize before reuse. - #[kaptan_error(id = 8009)] - FilterInvalid, -} - -/// Aiding fusion refusal with no nominal-state update. -#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)] -pub enum FusionError { - /// Observation is not at this filter's current history time. - #[kaptan_error(id = 8101)] - TimestampMismatch, - /// Observation was device-invalid. - #[kaptan_error(id = 8102)] - InvalidObservation, - /// Observation source does not identify the configured physical device. - #[kaptan_error(id = 8103)] - SourceMismatch, - /// Observation frame does not match the configured extrinsics. - #[kaptan_error(id = 8104)] - FrameMismatch, - /// Observation calibration revision does not match the configured transform. - #[kaptan_error(id = 8105)] - CalibrationMismatch, - /// The fixed source-integrity table has no entry for this source. - #[kaptan_error(id = 8106)] - SourceCapacityExhausted, - /// A source repeated or moved backwards from its accepted sequence stream. - #[kaptan_error(id = 8107)] - ReplayedObservation, - /// This estimator profile has no terrain state. - #[kaptan_error(id = 8108)] - TerrainProfileRequired, - /// Optical flow from an offset sensor needs a correlated IMU-noise update - /// that this profile does not implement. - #[kaptan_error(id = 8109)] - UnsupportedOpticalFlowLeverArm, - /// Measurement fails physical range or velocity plausibility bounds. - #[kaptan_error(id = 8110)] - Implausible, - /// Range ray is too tilted or does not intersect terrain below the sensor. - #[kaptan_error(id = 8111)] - InvalidRangeGeometry, - /// Innovation covariance was singular/non-finite. - #[kaptan_error(id = 8112)] - SingularInnovation, - /// Innovation exceeds the configured chi-square gate. - #[kaptan_error(id = 8113)] - InnovationRejected, - /// The candidate state or covariance failed validation; reinitialize before reuse. - #[kaptan_error(id = 8114)] - FilterInvalid, -} - -/// Fusion outcome when the observation was valid and within its gate. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub enum FusionResult { - /// State and covariance were updated using Joseph form. - Fused, - /// Source remains isolated while it accumulates re-admission evidence. - SourceIsolated, -} - -#[derive(Clone, Copy)] -struct SourceEntry { - used: bool, - source: SourceId, - last_accepted_receipt: Option, - last_sequence: Option, - rejections: u8, - probes: u8, - isolated: bool, -} - -impl SourceEntry { - const EMPTY: Self = Self { - used: false, - source: SourceId::new(0), - last_accepted_receipt: None, - last_sequence: None, - rejections: 0, - probes: 0, - isolated: false, - }; -} - -#[derive(Clone, Copy)] -struct SourceIntegrity { - entries: [SourceEntry; 4], - config: IntegrityConfig, -} - -impl SourceIntegrity { - fn new(config: IntegrityConfig) -> Self { - Self { - entries: [SourceEntry::EMPTY; 4], - config, - } - } - - /// Reserves one static entry before source state is mutated. A full table - /// is a visible fusion refusal; it never replaces unrelated sensor state. - fn reserve(&mut self, source: SourceId) -> bool { - if self - .entries - .iter() - .any(|entry| entry.used && entry.source == source) - { - return true; - } - let Some(entry) = self.entries.iter_mut().find(|entry| !entry.used) else { - return false; - }; - *entry = SourceEntry { - used: true, - source, - ..SourceEntry::EMPTY - }; - true - } - - fn entry_mut(&mut self, source: SourceId) -> Option<&mut SourceEntry> { - self.entries - .iter_mut() - .find(|entry| entry.used && entry.source == source) - } - - fn accepts_sequence(&mut self, source: SourceId, sequence: ObservationSequence) -> bool { - let Some(entry) = self.entry_mut(source) else { - return false; - }; - let Some(previous) = entry.last_sequence else { - entry.last_sequence = Some(sequence); - return true; - }; - let distance = sequence.get().wrapping_sub(previous.get()); - if distance == 0 || distance >= (1 << 31) { - return false; - } - entry.last_sequence = Some(sequence); - true - } - - fn status(&self, source: Option, now: Timestamp) -> SourceStatus { - let Some(source) = source else { - return SourceStatus::Unknown; - }; - let Some(entry) = self - .entries - .iter() - .find(|entry| entry.used && entry.source == source) - else { - return SourceStatus::Unknown; - }; - if entry.isolated { - return SourceStatus::Isolated; - } - let Some(receipt) = entry.last_accepted_receipt else { - return SourceStatus::Unknown; - }; - if now - .as_microseconds() - .saturating_sub(receipt.as_microseconds()) - > self.config.maximum_age_microseconds - { - SourceStatus::Stale - } else { - SourceStatus::Healthy - } - } - - fn reject(&mut self, source: SourceId) { - let rejection_limit = self.config.rejection_limit; - let Some(entry) = self.entry_mut(source) else { - return; - }; - entry.probes = 0; - entry.rejections = entry.rejections.saturating_add(1); - if entry.rejections >= rejection_limit { - entry.isolated = true; - } - } - - /// Records a valid gated observation and reports whether it may update. - fn accept(&mut self, source: SourceId, receipt: Timestamp) -> bool { - let readmission_accepts = self.config.readmission_accepts; - let Some(entry) = self.entry_mut(source) else { - return false; - }; - entry.last_accepted_receipt = Some(receipt); - if entry.isolated { - entry.probes = entry.probes.saturating_add(1); - if entry.probes < readmission_accepts { - return false; - } - entry.isolated = false; - } - entry.probes = 0; - entry.rejections = 0; - true - } -} - -/// Nominal navigation state. Values have intentionally private storage so -/// state changes always pass through propagation or an error-state injection. -#[derive(Clone, Copy, Debug, PartialEq)] -pub struct NominalState { - position: PositionNed, - velocity: VelocityNed, - attitude: UnitQuaternion, - gyro_bias: AngularRateFrd, - acceleration_bias: AccelerationFrd, - terrain_down_metres: f32, -} - -impl NominalState { - /// Returns local NED position in metres. - pub const fn position(self) -> PositionNed { - self.position - } - /// Returns local NED velocity in metres per second. - pub const fn velocity(self) -> VelocityNed { - self.velocity - } - /// Returns the normalized body-to-NED attitude. - pub const fn attitude(self) -> UnitQuaternion { - self.attitude - } - /// Returns gyro bias in body rad/s. - pub const fn gyro_bias(self) -> AngularRateFrd { - self.gyro_bias - } - /// Returns accelerometer bias in body m/s². - pub const fn acceleration_bias(self) -> AccelerationFrd { - self.acceleration_bias - } - /// Returns terrain down position for the terrain profile only. - pub fn terrain_down_metres(self) -> Option { - if N == TERRAIN_DIM { - Some(self.terrain_down_metres) - } else { - None - } - } -} - -fn state_is_valid(state: NominalState) -> bool { - // UnitQuaternion keeps its representation private and every constructor - // normalizes finite input, so an unnormalized attitude has no safe path - // into NominalState. - state.position.vector().is_finite() - && state.velocity.vector().is_finite() - && state.gyro_bias.vector().is_finite() - && state.acceleration_bias.vector().is_finite() - && (N != TERRAIN_DIM || state.terrain_down_metres.is_finite()) -} - -/// Stationary initialization accumulator. It is bounded by `SAMPLES` and -/// assumes a level aircraft in FRD; arbitrary initial attitude needs an aiding -/// source and is intentionally not guessed. -const MAXIMUM_INITIAL_GYRO_BIAS_RAD_S: f32 = 0.1; -const MAXIMUM_INITIAL_ACCELERATION_RESIDUAL_MPS2: f32 = 0.75; -const MAXIMUM_INITIAL_GYRO_VARIANCE_RAD2_S2: f32 = 0.0025; -const MAXIMUM_INITIAL_ACCELERATION_VARIANCE_M2PS4: f32 = 0.16; - -pub struct StationaryInitializer { - count: usize, - gyro_sum: Vector3, - acceleration_sum: Vector3, - gyro_squared_sum: f32, - acceleration_squared_sum: f32, - source: Option, - calibration: Option, - last_time: Option, - last_sequence: Option, -} - -#[derive(Clone, Copy)] -struct ImuStream { - source: SourceId, - calibration: CalibrationRevision, - last_sequence: ObservationSequence, -} - -fn sequence_strictly_after(next: ObservationSequence, previous: ObservationSequence) -> bool { - let distance = next.get().wrapping_sub(previous.get()); - distance != 0 && distance < (1 << 31) -} - -impl StationaryInitializer { - /// Creates an empty level stationary initialization window. - pub const fn new() -> Self { - Self { - count: 0, - gyro_sum: Vector3::zero(), - acceleration_sum: Vector3::zero(), - gyro_squared_sum: 0.0, - acceleration_squared_sum: 0.0, - source: None, - calibration: None, - last_time: None, - last_sequence: None, - } - } - - /// Adds one source-bound, monotonic FRD IMU observation. Any malformed, - /// mixed-source, or non-monotonic sample resets the window so a level - /// estimate can never average unrelated or reordered motion. - pub fn push(&mut self, imu: ImuObservation) -> bool { - if self.count == SAMPLES { - return false; - } - if (imu.context.validity != Validity::Valid) - || (imu.context.frame != FrameId::BODY_FRD) - || !imu.acceleration.vector().is_finite() - || !imu.angular_rate.vector().is_finite() - { - self.reset(); - return false; - } - if self - .source - .is_some_and(|source| source != imu.context.source) - || self - .calibration - .is_some_and(|calibration| calibration != imu.context.calibration) - || self - .last_time - .is_some_and(|time| imu.context.measurement_time <= time) - || self - .last_sequence - .is_some_and(|sequence| !sequence_strictly_after(imu.context.sequence, sequence)) - { - self.reset(); - return false; - } - let gyro_squared = imu.angular_rate.vector().norm_squared(); - let acceleration_squared = imu.acceleration.vector().norm_squared(); - let gyro_sum = self.gyro_sum.add(imu.angular_rate.vector()); - let acceleration_sum = self.acceleration_sum.add(imu.acceleration.vector()); - let gyro_squared_sum = self.gyro_squared_sum + gyro_squared; - let acceleration_squared_sum = self.acceleration_squared_sum + acceleration_squared; - if !gyro_squared.is_finite() - || !acceleration_squared.is_finite() - || !gyro_sum.is_finite() - || !acceleration_sum.is_finite() - || !gyro_squared_sum.is_finite() - || !acceleration_squared_sum.is_finite() - { - self.reset(); - return false; - } - self.gyro_sum = gyro_sum; - self.acceleration_sum = acceleration_sum; - self.gyro_squared_sum = gyro_squared_sum; - self.acceleration_squared_sum = acceleration_squared_sum; - self.source = Some(imu.context.source); - self.calibration = Some(imu.context.calibration); - self.last_time = Some(imu.context.measurement_time); - self.last_sequence = Some(imu.context.sequence); - self.count += 1; - true - } - - /// Returns whether the fixed sample window is complete. - pub const fn is_complete(&self) -> bool { - self.count == SAMPLES - } - - fn reset(&mut self) { - *self = Self::new(); - } - - /// Produces a level initialized MEKF profile at the final accepted IMU - /// timestamp. The covariance starts diagonal and is deliberately - /// conservative for unobserved position. - pub fn finish( - self, - time: Timestamp, - config: EstimatorConfig, - integrity: IntegrityConfig, - terrain_down_metres: f32, - ) -> Option> { - if (SAMPLES == 0) - || (self.count != SAMPLES) - || (self.last_time != Some(time)) - || !config.is_valid() - || !terrain_down_metres.is_finite() - || !self.gyro_sum.is_finite() - || !self.acceleration_sum.is_finite() - || !self.gyro_squared_sum.is_finite() - || !self.acceleration_squared_sum.is_finite() - { - return None; - } - let inverse = 1.0 / self.count as f32; - let gyro_bias = self.gyro_sum.scale(inverse); - let average_acceleration = self.acceleration_sum.scale(inverse); - let acceleration_residual = - average_acceleration.sub(Vector3::new(0.0, 0.0, -config.gravity_mps2)); - let gyro_variance = self.gyro_squared_sum * inverse - gyro_bias.norm_squared(); - let acceleration_variance = - self.acceleration_squared_sum * inverse - average_acceleration.norm_squared(); - // The admission gate above proves finite sums and a nonzero sample - // count, so both averages are finite. Their residual and variances can - // still overflow and remain explicit fail-closed checks. - if !acceleration_residual.is_finite() - || !gyro_variance.is_finite() - || !acceleration_variance.is_finite() - || (gyro_bias.norm_squared() - > MAXIMUM_INITIAL_GYRO_BIAS_RAD_S * MAXIMUM_INITIAL_GYRO_BIAS_RAD_S) - || (acceleration_residual.norm_squared() - > MAXIMUM_INITIAL_ACCELERATION_RESIDUAL_MPS2 - * MAXIMUM_INITIAL_ACCELERATION_RESIDUAL_MPS2) - || (gyro_variance > MAXIMUM_INITIAL_GYRO_VARIANCE_RAD2_S2) - || (acceleration_variance > MAXIMUM_INITIAL_ACCELERATION_VARIANCE_M2PS4) - { - return None; - } - let acceleration_bias = acceleration_residual; - let stream = ImuStream { - source: self.source?, - calibration: self.calibration?, - last_sequence: self.last_sequence?, - }; - Eskf::new( - time, - NominalState { - position: PositionNed::metres(0.0, 0.0, 0.0), - velocity: VelocityNed::metres_per_second(0.0, 0.0, 0.0), - attitude: UnitQuaternion::identity(), - gyro_bias: AngularRateFrd::radians_per_second( - gyro_bias.x(), - gyro_bias.y(), - gyro_bias.z(), - ), - acceleration_bias: AccelerationFrd::metres_per_second_squared( - acceleration_bias.x(), - acceleration_bias.y(), - acceleration_bias.z(), - ), - terrain_down_metres, - }, - config, - integrity, - stream, - ) - .ok() - } -} - -impl Default for StationaryInitializer { - fn default() -> Self { - Self::new() - } -} - -#[derive(Clone, Copy)] -struct Covariance { - values: [f32; MATRIX], -} - -impl Covariance { - fn diagonal() -> Self { - let mut values = [0.0; MATRIX]; - let mut index = 0; - while index < MAX_DIM { - values[index * MAX_DIM + index] = match index { - 0..=2 => 100.0, - 3..=5 => 10.0, - 6..=8 => 0.25, - 9..=11 => 0.01, - 12..=14 => 0.25, - _ => 100.0, - }; - index += 1; - } - Self { values } - } - fn get(&self, row: usize, column: usize) -> f32 { - self.values[row * MAX_DIM + column] - } - fn set(&mut self, row: usize, column: usize, value: f32) { - self.values[row * MAX_DIM + column] = value; - } - fn symmetric(&mut self, n: usize) { - let mut row = 0; - while row < n { - let mut col = row + 1; - while col < n { - let value = 0.5 * (self.get(row, col) + self.get(col, row)); - self.set(row, col, value); - self.set(col, row, value); - col += 1; - } - row += 1; - } - } - fn finite(&self, n: usize) -> bool { - let mut row = 0; - while row < n { - let mut col = 0; - while col < n { - if !self.get(row, col).is_finite() { - return false; - } - col += 1; - } - row += 1; - } - true - } - - /// Strict fixed-size Cholesky validation. A finite, symmetric matrix is - /// not sufficient for a Kalman covariance: it must also be positive - /// definite. No diagonal values are repaired on failure. - fn positive_definite(&self, n: usize) -> bool { - if !self.finite(n) { - return false; - } - let mut lower = [0.0; MATRIX]; - for row in 0..n { - for col in 0..=row { - let mut value = self.get(row, col); - for k in 0..col { - value -= lower[row * MAX_DIM + k] * lower[col * MAX_DIM + k]; - } - if !value.is_finite() { - return false; - } - if row == col { - if value <= 0.0 { - return false; - } - let diagonal = sqrt(value); - // `value` is finite and strictly positive, and the - // reviewed bounded square root preserves that domain. - debug_assert!(diagonal.is_finite()); - debug_assert!(diagonal > 0.0); - lower[row * MAX_DIM + col] = diagonal; - } else { - let diagonal = lower[col * MAX_DIM + col]; - // Every lower diagonal was accepted in the corresponding - // earlier diagonal iteration. - debug_assert!(diagonal.is_finite()); - debug_assert!(diagonal > 0.0); - let entry = value / diagonal; - if !entry.is_finite() { - return false; - } - lower[row * MAX_DIM + col] = entry; - } - } - } - true - } -} - -/// Fixed-size MEKF profile. `N` is intentionally a const generic solely to -/// select one of the two reviewed covariance dimensions (15 inertial or 16 -/// local-terrain); no runtime profile switch exists. -#[derive(Clone, Copy)] -pub struct Eskf { - time: Timestamp, - state: NominalState, - covariance: Covariance, - config: EstimatorConfig, - integrity: SourceIntegrity, - imu_stream: ImuStream, - last_range_source: Option, - last_flow_source: Option, - last_local_state_source: Option, - failed: bool, -} - -/// Inertial profile: position, velocity, attitude, gyro bias, accelerometer bias. -pub type InertialEskf = Eskf; -/// Local-terrain profile: inertial state plus terrain down position. -pub type LocalTerrainEskf = Eskf; - -impl Eskf { - fn new( - time: Timestamp, - state: NominalState, - config: EstimatorConfig, - integrity: IntegrityConfig, - imu_stream: ImuStream, - ) -> Result { - if (N != INERTIAL_DIM && N != TERRAIN_DIM) - || !config.is_valid() - || !integrity.is_valid() - || !state_is_valid::(state) - { - return Err(()); - } - let covariance = Covariance::diagonal(); - // This fixed constructor is positive diagonal for both permitted - // dimensions. Candidate covariance validation remains mandatory on - // every propagation and fusion path. - debug_assert!(covariance.positive_definite(N)); - Ok(Self { - time, - state, - covariance, - config, - integrity: SourceIntegrity::new(integrity), - imu_stream, - last_range_source: None, - last_flow_source: None, - last_local_state_source: None, - failed: false, - }) - } - - /// Current filter time in the controller monotonic domain. - pub const fn time(&self) -> Timestamp { - self.time - } - /// Current nominal state. - pub const fn state(&self) -> NominalState { - self.state - } - /// Returns whether covariance remains finite, symmetric, and strictly - /// positive definite according to a fixed-size Cholesky factorization. - pub fn covariance_is_valid(&self) -> bool { - if !self.covariance.finite(N) { - return false; - } - let mut row = 0; - while row < N { - let mut col = row + 1; - while col < N { - if (self.covariance.get(row, col) - self.covariance.get(col, row)).abs() > 1.0e-4 { - return false; - } - col += 1; - } - row += 1; - } - self.covariance.positive_definite(N) - } - - /// Returns whether a numerical candidate has invalidated this filter and - /// made safe reinitialization mandatory. - pub const fn requires_reinitialization(&self) -> bool { - self.failed - } - - fn values_are_valid(&self) -> bool { - !self.failed && state_is_valid::(self.state) && self.covariance_is_valid() - } - - /// Propagates exactly one strictly newer IMU observation. - pub fn propagate(&mut self, imu: ImuObservation) -> Result<(), PropagationError> { - if self.failed { - return Err(PropagationError::FilterInvalid); - } - if (imu.context.validity != Validity::Valid) - || (imu.context.frame != FrameId::BODY_FRD) - || !imu.acceleration.vector().is_finite() - || !imu.angular_rate.vector().is_finite() - { - return Err(PropagationError::InvalidImu); - } - if imu.context.source != self.imu_stream.source { - return Err(PropagationError::ImuSourceMismatch); - } - if imu.context.calibration != self.imu_stream.calibration { - return Err(PropagationError::ImuCalibrationMismatch); - } - let measurement = imu.context.measurement_time.as_microseconds(); - let current = self.time.as_microseconds(); - if measurement <= current { - return Err(PropagationError::NonMonotonicTimestamp); - } - let dt_us = measurement - current; - if dt_us > self.config.maximum_imu_interval_microseconds { - return Err(PropagationError::ImuGapTooLarge); - } - if !sequence_strictly_after(imu.context.sequence, self.imu_stream.last_sequence) { - return Err(PropagationError::ReplayedImu); - } - let dt = dt_us as f32 / 1_000_000.0; - let rotation_increment = imu - .angular_rate - .vector() - .sub(self.state.gyro_bias.vector()) - .scale(dt); - if !rotation_increment.is_finite() { - return Err(PropagationError::NonFinite); - } - if rotation_increment.norm_squared() > MAXIMUM_ROTATION_INCREMENT_RADIANS_SQUARED { - return Err(PropagationError::RotationIncrementTooLarge); - } - // Compute into a fixed-size copy so the public error contract is true - // even if a finite input overflows a later covariance calculation. - let mut next = *self; - if next.propagate_delta(imu, dt).is_err() { - self.failed = true; - return Err(PropagationError::FilterInvalid); - } - next.time = imu.context.measurement_time; - next.imu_stream.last_sequence = imu.context.sequence; - if !next.values_are_valid() { - self.failed = true; - return Err(PropagationError::FilterInvalid); - } - *self = next; - Ok(()) - } - - fn propagate_delta(&mut self, imu: ImuObservation, dt: f32) -> Result<(), MathError> { - let gyro = imu.angular_rate.vector().sub(self.state.gyro_bias.vector()); - let force_body = imu - .acceleration - .vector() - .sub(self.state.acceleration_bias.vector()); - let delta = UnitQuaternion::from_small_rotation_vector(gyro.scale(dt))?; - let attitude = self.state.attitude.compose(delta)?; - let force_navigation = attitude.body_to_ned(force_body); - let acceleration = force_navigation.add(Vector3::new(0.0, 0.0, self.config.gravity_mps2)); - let position = self - .state - .position - .vector() - .add(self.state.velocity.vector().scale(dt)) - .add(acceleration.scale(0.5 * dt * dt)); - let velocity = self.state.velocity.vector().add(acceleration.scale(dt)); - if !acceleration.is_finite() || !position.is_finite() || !velocity.is_finite() { - return Err(MathError::NonFinite); - } - self.state.attitude = attitude; - self.state.position = PositionNed::metres(position.x(), position.y(), position.z()); - self.state.velocity = - VelocityNed::metres_per_second(velocity.x(), velocity.y(), velocity.z()); - self.propagate_covariance(force_navigation, dt, imu.noise); - Ok(()) - } - - fn propagate_covariance(&mut self, force_navigation: Vector3, dt: f32, noise: ImuNoise) { - let mut transition = [0.0; MATRIX]; - let mut i = 0; - while i < N { - transition[i * MAX_DIM + i] = 1.0; - i += 1; - } - for axis in 0..3 { - transition[(POSITION + axis) * MAX_DIM + VELOCITY + axis] = dt; - } - let skew = [ - [0.0, -force_navigation.z(), force_navigation.y()], - [force_navigation.z(), 0.0, -force_navigation.x()], - [-force_navigation.y(), force_navigation.x(), 0.0], - ]; - let rotation = rotation_matrix(self.state.attitude); - for row in 0..3 { - for col in 0..3 { - transition[(VELOCITY + row) * MAX_DIM + ATTITUDE + col] = -skew[row][col] * dt; - transition[(VELOCITY + row) * MAX_DIM + ACCEL_BIAS + col] = - -rotation[row][col] * dt; - transition[(ATTITUDE + row) * MAX_DIM + GYRO_BIAS + col] = -rotation[row][col] * dt; - } - } - let mut temporary = [0.0; MATRIX]; - for row in 0..N { - for col in 0..N { - let mut sum = 0.0; - for k in 0..N { - sum += transition[row * MAX_DIM + k] * self.covariance.get(k, col); - } - temporary[row * MAX_DIM + col] = sum; - } - } - let mut propagated = Covariance { - values: [0.0; MATRIX], - }; - for row in 0..N { - for col in 0..N { - let mut sum = 0.0; - for k in 0..N { - sum += temporary[row * MAX_DIM + k] * transition[col * MAX_DIM + k]; - } - propagated.set(row, col, sum); - } - } - // Continuous process spectral densities integrate once over `dt`. - // Per-sample sensor variances enter through their integrated angle or - // velocity increments and therefore scale with `dt²`. - let gyro_noise = self.config.gyro_noise_variance * dt + noise.gyro_rate.get() * dt * dt; - let accel_noise = - self.config.acceleration_noise_variance * dt + noise.acceleration.get() * dt * dt; - for axis in 0..3 { - propagated.set( - VELOCITY + axis, - VELOCITY + axis, - propagated.get(VELOCITY + axis, VELOCITY + axis) + accel_noise, - ); - propagated.set( - ATTITUDE + axis, - ATTITUDE + axis, - propagated.get(ATTITUDE + axis, ATTITUDE + axis) + gyro_noise, - ); - propagated.set( - GYRO_BIAS + axis, - GYRO_BIAS + axis, - propagated.get(GYRO_BIAS + axis, GYRO_BIAS + axis) - + self.config.gyro_bias_walk_variance * dt, - ); - propagated.set( - ACCEL_BIAS + axis, - ACCEL_BIAS + axis, - propagated.get(ACCEL_BIAS + axis, ACCEL_BIAS + axis) - + self.config.acceleration_bias_walk_variance * dt, - ); - } - if N == TERRAIN_DIM { - propagated.set( - TERRAIN, - TERRAIN, - propagated.get(TERRAIN, TERRAIN) + self.config.terrain_walk_variance * dt, - ); - } - propagated.symmetric(N); - self.covariance = propagated; - } - - /// Returns the co-located optical-flow prediction and its two-row ESKF - /// measurement Jacobian. Offset sensors are refused by the caller: their - /// rigid-body rate term is correlated with propagation IMU noise. - fn co_located_flow_model( - &self, - extrinsics: SensorExtrinsics, - ) -> ([f32; 2], [[f32; MAX_DIM]; 2]) { - let body_velocity = self - .state - .attitude - .ned_to_body(self.state.velocity.vector()); - let sensor_velocity: Vector3 = - extrinsics.sensor_to_body.inverse().rotate(body_velocity); - let predicted = [sensor_velocity.x(), sensor_velocity.y()]; - let mut h = [[0.0; MAX_DIM]; 2]; - for axis in 0..3 { - let basis = match axis { - 0 => Vector3::::new(1.0, 0.0, 0.0), - 1 => Vector3::::new(0.0, 1.0, 0.0), - _ => Vector3::::new(0.0, 0.0, 1.0), - }; - let body = self.state.attitude.ned_to_body(basis); - let sensor: Vector3 = extrinsics.sensor_to_body.inverse().rotate(body); - h[0][VELOCITY + axis] = sensor.x(); - h[1][VELOCITY + axis] = sensor.y(); - - // The filter's attitude error is a left-multiplicative NED - // tangent perturbation. d(Rᵀv)/dδθ = Rᵀ[v]×. - let attitude_body = self - .state - .attitude - .ned_to_body(self.state.velocity.vector().cross(basis)); - let attitude_sensor: Vector3 = - extrinsics.sensor_to_body.inverse().rotate(attitude_body); - h[0][ATTITUDE + axis] = attitude_sensor.x(); - h[1][ATTITUDE + axis] = attitude_sensor.y(); - } - (predicted, h) - } - - /// Fuses a range observation at the filter's current history time. - /// - /// Delayed observations are rejected. A future estimator must only admit - /// delayed aiding with an independently validated interval-splitting and - /// covariance-replay model; retaining a partial rewind layer here would - /// manufacture confidence without that proof. - pub fn fuse_range( - &mut self, - observation: RangeObservation, - extrinsics: SensorExtrinsics, - ) -> Result { - if self.failed { - return Err(FusionError::FilterInvalid); - } - if observation.context.measurement_time != self.time { - return Err(FusionError::TimestampMismatch); - } - if N != TERRAIN_DIM { - return Err(FusionError::TerrainProfileRequired); - } - if observation.context.source != extrinsics.source { - return Err(FusionError::SourceMismatch); - } - if observation.context.validity != Validity::Valid - || observation.context.quality < self.config.minimum_range_quality - { - self.integrity.reject(observation.context.source); - return Err(FusionError::InvalidObservation); - } - if observation.context.frame != extrinsics.sensor_frame { - self.integrity.reject(observation.context.source); - return Err(FusionError::FrameMismatch); - } - if observation.context.calibration != extrinsics.calibration { - self.integrity.reject(observation.context.source); - return Err(FusionError::CalibrationMismatch); - } - if !self.integrity.reserve(extrinsics.source) { - return Err(FusionError::SourceCapacityExhausted); - } - if !self - .integrity - .accepts_sequence(extrinsics.source, observation.context.sequence) - { - self.integrity.reject(extrinsics.source); - return Err(FusionError::ReplayedObservation); - } - if !observation.metres.is_finite() - || observation.metres < self.config.minimum_range_metres - || observation.metres > self.config.maximum_range_metres - { - self.integrity.reject(observation.context.source); - return Err(FusionError::Implausible); - } - let ray_body = extrinsics.sensor_to_body_vector(Vector3::::new(0.0, 0.0, 1.0)); - let ray = self.state.attitude.body_to_ned(ray_body); - let lever = self - .state - .attitude - .body_to_ned(extrinsics.position_body.vector()); - // The ray is a fixed finite unit vector transformed only by normalized - // quaternions. The public lever-arm value remains independently - // validated because it can carry a malformed configuration value. - if !lever.is_finite() || (ray.z() < self.config.minimum_range_down_cosine) { - self.integrity.reject(observation.context.source); - return Err(FusionError::InvalidRangeGeometry); - } - let numerator = - self.state.terrain_down_metres - self.state.position.vector().z() - lever.z(); - if numerator <= 0.0 { - self.integrity.reject(observation.context.source); - return Err(FusionError::InvalidRangeGeometry); - } - let predicted = numerator / ray.z(); - let mut h = [0.0; MAX_DIM]; - h[POSITION + 2] = -1.0 / ray.z(); - h[TERRAIN] = 1.0 / ray.z(); - for axis in 0..3 { - let lever_z = tangent_cross_z(axis, lever); - let ray_z = tangent_cross_z(axis, ray); - h[ATTITUDE + axis] = (-lever_z * ray.z() - numerator * ray_z) / (ray.z() * ray.z()); - } - let innovation = observation.metres - predicted; - if !predicted.is_finite() - || !innovation.is_finite() - || !h.iter().all(|value| value.is_finite()) - { - self.integrity.reject(observation.context.source); - return Err(FusionError::SingularInnovation); - } - let Some(gain) = self.scalar_gain(h, observation.variance) else { - self.integrity.reject(observation.context.source); - return Err(FusionError::SingularInnovation); - }; - let nis = innovation * innovation / gain.innovation_variance; - if !nis.is_finite() || nis > self.config.range_gate_chi_square { - self.integrity.reject(observation.context.source); - return Err(FusionError::InnovationRejected); - } - self.last_range_source = Some(extrinsics.source); - if !self - .integrity - .accept(extrinsics.source, observation.context.receipt_time) - { - return Ok(FusionResult::SourceIsolated); - } - let mut candidate = *self; - if candidate - .apply_scalar(gain, innovation, observation.variance.get()) - .is_err() - || !candidate.values_are_valid() - { - self.failed = true; - return Err(FusionError::FilterInvalid); - } - *self = candidate; - Ok(FusionResult::Fused) - } - - /// Fuses a ground-referenced sensor-frame optical-flow velocity at current time. - /// - /// The sensor must be co-located with the body origin. An offset sensor - /// has a rigid-body `ω × r` term correlated with the IMU sample used for - /// propagation. It is refused until the estimator implements that joint - /// measurement update instead of treating correlated noise as independent. - pub fn fuse_optical_flow( - &mut self, - observation: OpticalFlowObservation, - extrinsics: SensorExtrinsics, - ) -> Result { - if self.failed { - return Err(FusionError::FilterInvalid); - } - if observation.context.measurement_time != self.time { - return Err(FusionError::TimestampMismatch); - } - if observation.context.source != extrinsics.source { - return Err(FusionError::SourceMismatch); - } - if !extrinsics.position_body.vector().is_finite() - || extrinsics.position_body.vector() != Vector3::new(0.0, 0.0, 0.0) - { - return Err(FusionError::UnsupportedOpticalFlowLeverArm); - } - if (observation.context.validity != Validity::Valid) - || (observation.context.quality < self.config.minimum_flow_quality) - { - self.integrity.reject(observation.context.source); - return Err(FusionError::InvalidObservation); - } - if observation.context.frame != extrinsics.sensor_frame { - self.integrity.reject(observation.context.source); - return Err(FusionError::FrameMismatch); - } - if observation.context.calibration != extrinsics.calibration { - self.integrity.reject(observation.context.source); - return Err(FusionError::CalibrationMismatch); - } - if !self.integrity.reserve(extrinsics.source) { - return Err(FusionError::SourceCapacityExhausted); - } - if !self - .integrity - .accepts_sequence(extrinsics.source, observation.context.sequence) - { - self.integrity.reject(extrinsics.source); - return Err(FusionError::ReplayedObservation); - } - if !observation.velocity.is_finite() - || !observation.ground_distance_metres.is_finite() - || (observation.ground_distance_metres <= 0.0) - || (observation.velocity.x().abs() > self.config.maximum_flow_speed_metres_per_second) - || (observation.velocity.y().abs() > self.config.maximum_flow_speed_metres_per_second) - { - self.integrity.reject(observation.context.source); - return Err(FusionError::Implausible); - } - let (predicted, h) = self.co_located_flow_model(extrinsics); - if !predicted.iter().all(|value| value.is_finite()) { - self.integrity.reject(observation.context.source); - return Err(FusionError::Implausible); - } - if !h[0] - .iter() - .take(N) - .chain(h[1].iter().take(N)) - .all(|value| value.is_finite()) - { - self.integrity.reject(observation.context.source); - return Err(FusionError::SingularInnovation); - } - let innovation = [ - observation.velocity.x() - predicted[0], - observation.velocity.y() - predicted[1], - ]; - let Some(gain) = self.two_gain(h, observation.covariance) else { - self.integrity.reject(observation.context.source); - return Err(FusionError::SingularInnovation); - }; - let nis = innovation[0] - * (gain.inverse_s[0] * innovation[0] + gain.inverse_s[1] * innovation[1]) - + innovation[1] - * (gain.inverse_s[2] * innovation[0] + gain.inverse_s[3] * innovation[1]); - if !nis.is_finite() || nis > self.config.flow_gate_chi_square { - self.integrity.reject(observation.context.source); - return Err(FusionError::InnovationRejected); - } - self.last_flow_source = Some(extrinsics.source); - if !self - .integrity - .accept(extrinsics.source, observation.context.receipt_time) - { - return Ok(FusionResult::SourceIsolated); - } - let mut candidate = *self; - if candidate - .apply_two(gain, innovation, observation.covariance) - .is_err() - || !candidate.values_are_valid() - { - self.failed = true; - return Err(FusionError::FilterInvalid); - } - *self = candidate; - Ok(FusionResult::Fused) - } - - /// Fuses one complete local-NED position and velocity observation at the - /// filter's current history time. - /// - /// The configured source and calibration are explicit inputs because a - /// local-state producer is a vehicle integration choice (for example GPS, - /// VIO, or a simulator), not a hidden estimator mode. Position and - /// velocity axes are updated sequentially in a private candidate filter; - /// any failed gate leaves the live nominal state and covariance unchanged. - pub fn fuse_local_position_velocity( - &mut self, - observation: LocalPositionVelocityObservation, - expected_source: SourceId, - expected_calibration: CalibrationRevision, - ) -> Result { - if self.failed { - return Err(FusionError::FilterInvalid); - } - if observation.context.measurement_time != self.time { - return Err(FusionError::TimestampMismatch); - } - if observation.context.source != expected_source { - return Err(FusionError::SourceMismatch); - } - if (observation.context.validity != Validity::Valid) || (observation.context.quality == 0) { - self.integrity.reject(observation.context.source); - return Err(FusionError::InvalidObservation); - } - if observation.context.frame != FrameId::LOCAL_NED { - self.integrity.reject(observation.context.source); - return Err(FusionError::FrameMismatch); - } - if observation.context.calibration != expected_calibration { - self.integrity.reject(observation.context.source); - return Err(FusionError::CalibrationMismatch); - } - if !self.integrity.reserve(expected_source) { - return Err(FusionError::SourceCapacityExhausted); - } - if !self - .integrity - .accepts_sequence(expected_source, observation.context.sequence) - { - self.integrity.reject(expected_source); - return Err(FusionError::ReplayedObservation); - } - let position = observation.position.vector(); - let velocity = observation.velocity.vector(); - if !position.is_finite() - || !velocity.is_finite() - || (position.x().abs() > self.config.maximum_local_position_metres) - || (position.y().abs() > self.config.maximum_local_position_metres) - || (position.z().abs() > self.config.maximum_local_position_metres) - || (velocity.x().abs() > self.config.maximum_local_speed_metres_per_second) - || (velocity.y().abs() > self.config.maximum_local_speed_metres_per_second) - || (velocity.z().abs() > self.config.maximum_local_speed_metres_per_second) - { - self.integrity.reject(observation.context.source); - return Err(FusionError::Implausible); - } - let measurements = [ - (POSITION, position.x(), observation.position_variance), - (POSITION + 1, position.y(), observation.position_variance), - (POSITION + 2, position.z(), observation.position_variance), - (VELOCITY, velocity.x(), observation.velocity_variance), - (VELOCITY + 1, velocity.y(), observation.velocity_variance), - (VELOCITY + 2, velocity.z(), observation.velocity_variance), - ]; - let mut candidate = *self; - for (state_index, measurement, variance) in measurements { - let predicted = if state_index < VELOCITY { - let value = candidate.state.position.vector(); - match state_index - POSITION { - 0 => value.x(), - 1 => value.y(), - _ => value.z(), - } - } else { - let value = candidate.state.velocity.vector(); - match state_index - VELOCITY { - 0 => value.x(), - 1 => value.y(), - _ => value.z(), - } - }; - let mut h = [0.0; MAX_DIM]; - h[state_index] = 1.0; - let Some(gain) = candidate.scalar_gain(h, variance) else { - self.integrity.reject(observation.context.source); - return Err(FusionError::SingularInnovation); - }; - let innovation = measurement - predicted; - let nis = innovation * innovation / gain.innovation_variance; - if !nis.is_finite() || nis > self.config.local_state_gate_chi_square { - self.integrity.reject(observation.context.source); - return Err(FusionError::InnovationRejected); - } - if candidate - .apply_scalar(gain, innovation, variance.get()) - .is_err() - { - self.failed = true; - return Err(FusionError::FilterInvalid); - } - } - if !candidate.values_are_valid() { - self.failed = true; - return Err(FusionError::FilterInvalid); - } - self.last_local_state_source = Some(expected_source); - if !self - .integrity - .accept(expected_source, observation.context.receipt_time) - { - return Ok(FusionResult::SourceIsolated); - } - candidate.integrity = self.integrity; - candidate.last_local_state_source = self.last_local_state_source; - *self = candidate; - Ok(FusionResult::Fused) - } - - /// Returns only capabilities supported by fresh accepted aiding sources. - pub fn health(&self, now: Timestamp) -> EstimatorHealth { - let range = self.integrity.status(self.last_range_source, now); - let optical_flow = self.integrity.status(self.last_flow_source, now); - let local_position_velocity = self.integrity.status(self.last_local_state_source, now); - let finite = self.values_are_valid(); - EstimatorHealth { - finite, - initialized: !self.failed, - range, - optical_flow, - local_position_velocity, - capabilities: EstimatorCapabilities { - attitude: finite, - terrain_relative_height: N == TERRAIN_DIM && range == SourceStatus::Healthy, - terrain_relative_velocity: optical_flow == SourceStatus::Healthy, - local_position: local_position_velocity == SourceStatus::Healthy, - local_velocity: local_position_velocity == SourceStatus::Healthy, - }, - } - } - - fn scalar_gain(&self, h: [f32; MAX_DIM], variance: ScalarVariance) -> Option { - let mut ph = [0.0; MAX_DIM]; - for (row, ph_value) in ph.iter_mut().enumerate().take(N) { - for (col, h_value) in h.iter().enumerate().take(N) { - *ph_value += self.covariance.get(row, col) * h_value; - } - } - let mut innovation_variance = variance.get(); - for (h_value, ph_value) in h.iter().zip(ph).take(N) { - innovation_variance += h_value * ph_value; - } - if !innovation_variance.is_finite() || innovation_variance <= 1.0e-8 { - return None; - } - let mut gain = [0.0; MAX_DIM]; - for (gain_value, ph_value) in gain.iter_mut().zip(ph).take(N) { - *gain_value = ph_value / innovation_variance; - } - Some(ScalarGain { - h, - gain, - innovation_variance, - }) - } - - fn apply_scalar( - &mut self, - update: ScalarGain, - innovation: f32, - variance: f32, - ) -> Result<(), MathError> { - let mut error = [0.0; MAX_DIM]; - for (error_value, gain_value) in error.iter_mut().zip(update.gain).take(N) { - *error_value = gain_value * innovation; - } - if !innovation.is_finite() - || !variance.is_finite() - || (variance <= 0.0) - || !error.iter().take(N).all(|value| value.is_finite()) - { - return Err(MathError::NonFinite); - } - let attitude_error = self.inject(error)?; - let mut a = [0.0; MATRIX]; - for row in 0..N { - for col in 0..N { - a[row * MAX_DIM + col] = - if row == col { 1.0 } else { 0.0 } - update.gain[row] * update.h[col]; - } - } - self.joseph(a, &update.gain, variance); - self.reset_attitude_error(attitude_error); - Ok(()) - } - - fn two_gain(&self, h: [[f32; MAX_DIM]; 2], covariance: crate::Covariance2) -> Option { - let mut ph = [[0.0; 2]; MAX_DIM]; - for (row, ph_value) in ph.iter_mut().enumerate().take(N) { - for col in 0..N { - ph_value[0] += self.covariance.get(row, col) * h[0][col]; - ph_value[1] += self.covariance.get(row, col) * h[1][col]; - } - } - let mut s00 = covariance.xx.get(); - let mut s01 = covariance.xy; - let mut s11 = covariance.yy.get(); - for (row, ph_value) in ph.iter().enumerate().take(N) { - s00 += h[0][row] * ph_value[0]; - s01 += h[0][row] * ph_value[1]; - s11 += h[1][row] * ph_value[1]; - } - let determinant = s00 * s11 - s01 * s01; - // This is the fixed-size equivalent of accepting only a successful - // Cholesky factorization of the innovation covariance. A positive - // determinant alone would also accept a negative-definite 2x2 matrix. - if !s00.is_finite() - || !s01.is_finite() - || !s11.is_finite() - || (s00 <= 1.0e-8) - || (s11 <= 1.0e-8) - || !determinant.is_finite() - || (determinant <= 1.0e-8) - { - return None; - } - let inverse_s = [ - s11 / determinant, - -s01 / determinant, - -s01 / determinant, - s00 / determinant, - ]; - let mut gain = [[0.0; 2]; MAX_DIM]; - for row in 0..N { - gain[row][0] = ph[row][0] * inverse_s[0] + ph[row][1] * inverse_s[2]; - gain[row][1] = ph[row][0] * inverse_s[1] + ph[row][1] * inverse_s[3]; - } - Some(TwoGain { h, gain, inverse_s }) - } - - fn apply_two( - &mut self, - update: TwoGain, - innovation: [f32; 2], - covariance: crate::Covariance2, - ) -> Result<(), MathError> { - let mut error = [0.0; MAX_DIM]; - for (error_value, gain_value) in error.iter_mut().zip(update.gain).take(N) { - *error_value = gain_value[0] * innovation[0] + gain_value[1] * innovation[1]; - } - if !innovation.iter().all(|value| value.is_finite()) - || !error.iter().take(N).all(|value| value.is_finite()) - { - return Err(MathError::NonFinite); - } - let attitude_error = self.inject(error)?; - let mut a = [0.0; MATRIX]; - for row in 0..N { - for col in 0..N { - a[row * MAX_DIM + col] = (if row == col { 1.0 } else { 0.0 }) - - update.gain[row][0] * update.h[0][col] - - update.gain[row][1] * update.h[1][col]; - } - } - let mut first = [0.0; MAX_DIM]; - let mut second = [0.0; MAX_DIM]; - for row in 0..N { - first[row] = - update.gain[row][0] * covariance.xx.get() + update.gain[row][1] * covariance.xy; - second[row] = - update.gain[row][0] * covariance.xy + update.gain[row][1] * covariance.yy.get(); - } - self.joseph_two(a, first, second, &update.gain); - self.reset_attitude_error(attitude_error); - Ok(()) - } - - fn joseph(&mut self, a: [f32; MATRIX], gain: &[f32; MAX_DIM], variance: f32) { - let old = self.covariance; - let mut temporary = [0.0; MATRIX]; - for row in 0..N { - for col in 0..N { - let mut sum = 0.0; - for k in 0..N { - sum += a[row * MAX_DIM + k] * old.get(k, col); - } - temporary[row * MAX_DIM + col] = sum; - } - } - let mut next = Covariance { - values: [0.0; MATRIX], - }; - for row in 0..N { - for col in 0..N { - let mut sum = 0.0; - for k in 0..N { - sum += temporary[row * MAX_DIM + k] * a[col * MAX_DIM + k]; - } - next.set(row, col, sum + gain[row] * variance * gain[col]); - } - } - next.symmetric(N); - self.covariance = next; - } - - fn joseph_two( - &mut self, - a: [f32; MATRIX], - first: [f32; MAX_DIM], - second: [f32; MAX_DIM], - gain: &[[f32; 2]; MAX_DIM], - ) { - let old = self.covariance; - let mut temporary = [0.0; MATRIX]; - for row in 0..N { - for col in 0..N { - let mut sum = 0.0; - for k in 0..N { - sum += a[row * MAX_DIM + k] * old.get(k, col); - } - temporary[row * MAX_DIM + col] = sum; - } - } - let mut next = Covariance { - values: [0.0; MATRIX], - }; - for row in 0..N { - for col in 0..N { - let mut sum = 0.0; - for k in 0..N { - sum += temporary[row * MAX_DIM + k] * a[col * MAX_DIM + k]; - } - let krkt = first[row] * gain[col][0] + second[row] * gain[col][1]; - next.set(row, col, sum + krkt); - } - } - next.symmetric(N); - self.covariance = next; - } - - fn inject(&mut self, error: [f32; MAX_DIM]) -> Result, MathError> { - if !error.iter().take(N).all(|value| value.is_finite()) { - return Err(MathError::NonFinite); - } - let position = self - .state - .position - .vector() - .add(Vector3::new(error[0], error[1], error[2])); - let velocity = self - .state - .velocity - .vector() - .add(Vector3::new(error[3], error[4], error[5])); - let attitude_error = Vector3::::new(error[6], error[7], error[8]); - let attitude = UnitQuaternion::from_small_rotation_vector(attitude_error)? - .compose(self.state.attitude)?; - let gyro = self - .state - .gyro_bias - .vector() - .add(Vector3::new(error[9], error[10], error[11])); - let accel = self - .state - .acceleration_bias - .vector() - .add(Vector3::new(error[12], error[13], error[14])); - if !position.is_finite() - || !velocity.is_finite() - || !gyro.is_finite() - || !accel.is_finite() - || ((N == TERRAIN_DIM) - && !(self.state.terrain_down_metres + error[TERRAIN]).is_finite()) - { - return Err(MathError::NonFinite); - } - self.state.position = PositionNed::metres(position.x(), position.y(), position.z()); - self.state.velocity = - VelocityNed::metres_per_second(velocity.x(), velocity.y(), velocity.z()); - self.state.attitude = attitude; - self.state.gyro_bias = AngularRateFrd::radians_per_second(gyro.x(), gyro.y(), gyro.z()); - self.state.acceleration_bias = - AccelerationFrd::metres_per_second_squared(accel.x(), accel.y(), accel.z()); - if N == TERRAIN_DIM { - self.state.terrain_down_metres += error[TERRAIN]; - } - Ok(attitude_error) - } - - /// Applies the first-order MEKF reset after a left-multiplicative attitude - /// error is injected in NED coordinates. Without this transform the - /// covariance would remain expressed around the old tangent origin. - fn reset_attitude_error(&mut self, attitude_error: Vector3) { - let mut reset = [0.0; MATRIX]; - for axis in 0..N { - reset[axis * MAX_DIM + axis] = 1.0; - } - let skew = [ - [0.0, -attitude_error.z(), attitude_error.y()], - [attitude_error.z(), 0.0, -attitude_error.x()], - [-attitude_error.y(), attitude_error.x(), 0.0], - ]; - for row in 0..3 { - for col in 0..3 { - reset[(ATTITUDE + row) * MAX_DIM + ATTITUDE + col] += 0.5 * skew[row][col]; - } - } - let old = self.covariance; - let mut temporary = [0.0; MATRIX]; - for row in 0..N { - for col in 0..N { - for k in 0..N { - temporary[row * MAX_DIM + col] += reset[row * MAX_DIM + k] * old.get(k, col); - } - } - } - let mut next = Covariance { - values: [0.0; MATRIX], - }; - for row in 0..N { - for col in 0..N { - for k in 0..N { - next.set( - row, - col, - next.get(row, col) - + temporary[row * MAX_DIM + k] * reset[col * MAX_DIM + k], - ); - } - } - } - next.symmetric(N); - self.covariance = next; - } -} - -#[derive(Clone, Copy)] -struct ScalarGain { - h: [f32; MAX_DIM], - gain: [f32; MAX_DIM], - innovation_variance: f32, -} -#[derive(Clone, Copy)] -struct TwoGain { - h: [[f32; MAX_DIM]; 2], - gain: [[f32; 2]; MAX_DIM], - inverse_s: [f32; 4], -} - -fn rotation_matrix(attitude: UnitQuaternion) -> [[f32; 3]; 3] { - let x = attitude.body_to_ned(Vector3::::new(1.0, 0.0, 0.0)); - let y = attitude.body_to_ned(Vector3::::new(0.0, 1.0, 0.0)); - let z = attitude.body_to_ned(Vector3::::new(0.0, 0.0, 1.0)); - [ - [x.x(), y.x(), z.x()], - [x.y(), y.y(), z.y()], - [x.z(), y.z(), z.z()], - ] -} - -fn tangent_cross_z(axis: usize, vector: Vector3) -> f32 { - match axis { - 0 => vector.y(), - 1 => -vector.x(), - _ => 0.0, - } -} - -#[cfg(test)] -mod tests { - use super::*; - use crate::observation::ImuNoise; - use crate::{ - CalibrationRevision, Covariance2, FrameId, ObservationContext, ObservationSequence, - SourceId, - }; - use kaptan_sensor::imu::ImuSnapshot; - - fn context(time: u64, frame: FrameId, source: u8, quality: u8) -> ObservationContext { - ObservationContext::new( - Timestamp::from_microseconds(time), - Timestamp::from_microseconds(time + 1), - SourceId::new(source), - frame, - CalibrationRevision::new(1), - quality, - Validity::Valid, - ObservationSequence::new(time as u32), - ) - } - fn noise() -> ImuNoise { - ImuNoise { - gyro_rate: ScalarVariance::new(1.0e-4).unwrap(), - acceleration: ScalarVariance::new(1.0e-3).unwrap(), - } - } - - fn imu_stream() -> ImuStream { - ImuStream { - source: SourceId::new(1), - calibration: CalibrationRevision::new(1), - last_sequence: ObservationSequence::new(10), - } - } - fn imu(time: u64, acceleration_mg: [i16; 3], rate_mrad: [i16; 3]) -> ImuObservation { - ImuObservation::from_snapshot( - ImuSnapshot::new( - Timestamp::from_microseconds(time), - acceleration_mg, - rate_mrad, - 0, - ), - context(time, FrameId::BODY_FRD, 1, 255), - noise(), - ) - } - fn initialized() -> Eskf { - let mut init = StationaryInitializer::<8>::new(); - for time in 3..=10 { - assert!(init.push(imu(time, [0, 0, -1_000], [0; 3]))); - } - init.finish( - Timestamp::from_microseconds(10), - EstimatorConfig::default(), - IntegrityConfig::default(), - 2.0, - ) - .unwrap() - } - - fn range() -> RangeObservation { - RangeObservation { - context: context(10, FrameId::new(7), 2, 255), - metres: 2.0, - variance: ScalarVariance::new(0.1).unwrap(), - } - } - - fn range_extrinsics() -> SensorExtrinsics { - SensorExtrinsics { - source: SourceId::new(2), - sensor_frame: FrameId::new(7), - calibration: CalibrationRevision::new(1), - sensor_to_body: UnitQuaternion::identity(), - position_body: crate::PositionFrd::metres(0.0, 0.0, 0.0), - } - } - - fn flow() -> OpticalFlowObservation { - OpticalFlowObservation { - context: context(10, FrameId::new(8), 3, 255), - velocity: kaptan_math::Vector2::new(0.0, 0.0), - ground_distance_metres: 1.0, - covariance: Covariance2::new( - ScalarVariance::new(0.1).unwrap(), - 0.0, - ScalarVariance::new(0.1).unwrap(), - ) - .unwrap(), - } - } - - fn flow_extrinsics() -> SensorExtrinsics { - SensorExtrinsics { - source: SourceId::new(3), - sensor_frame: FrameId::new(8), - calibration: CalibrationRevision::new(1), - sensor_to_body: UnitQuaternion::identity(), - position_body: crate::PositionFrd::metres(0.0, 0.0, 0.0), - } - } - - fn local() -> LocalPositionVelocityObservation { - LocalPositionVelocityObservation { - context: context(10, FrameId::LOCAL_NED, 5, 255), - position: PositionNed::metres(0.0, 0.0, 0.0), - velocity: VelocityNed::metres_per_second(0.0, 0.0, 0.0), - position_variance: ScalarVariance::new(0.1).unwrap(), - velocity_variance: ScalarVariance::new(0.1).unwrap(), - } - } - - #[test] - fn stationary_initialization() { - let mut init = StationaryInitializer::<4>::new(); - for time in 0..4 { - init.push(imu(time, [0, 0, -990], [10, -20, 30])); - } - let filter: InertialEskf = init - .finish( - Timestamp::from_microseconds(3), - EstimatorConfig::default(), - IntegrityConfig::default(), - 0.0, - ) - .unwrap(); - assert!((filter.state().gyro_bias().vector().x() - 0.01).abs() < 1.0e-4); - assert!(filter.state().attitude().is_normalized()); - } - - #[test] - fn stationary_window_validation() { - let mut mixed = StationaryInitializer::<2>::new(); - assert!(mixed.push(imu(1, [0, 0, -1_000], [0; 3]))); - let mut wrong_source = imu(2, [0, 0, -1_000], [0; 3]); - wrong_source.context.source = SourceId::new(2); - assert!(!mixed.push(wrong_source)); - assert!(!mixed.is_complete()); - assert!(mixed.push(imu(3, [0, 0, -1_000], [0; 3]))); - assert!(mixed.push(imu(4, [0, 0, -1_000], [0; 3]))); - assert!( - mixed - .finish::<15>( - Timestamp::from_microseconds(4), - EstimatorConfig::default(), - IntegrityConfig::default(), - 0.0 - ) - .is_some() - ); - - let mut reordered = StationaryInitializer::<2>::new(); - assert!(reordered.push(imu(4, [0, 0, -1_000], [0; 3]))); - assert!(!reordered.push(imu(3, [0, 0, -1_000], [0; 3]))); - assert!(!reordered.is_complete()); - - let mut moving = StationaryInitializer::<2>::new(); - assert!(moving.push(imu(1, [0, 0, -1_000], [200, 0, 0]))); - assert!(moving.push(imu(2, [0, 0, -1_000], [200, 0, 0]))); - assert!( - moving - .finish::<15>( - Timestamp::from_microseconds(2), - EstimatorConfig::default(), - IntegrityConfig::default(), - 0.0 - ) - .is_none() - ); - } - - #[test] - fn rotation_normalizes() { - let mut filter: InertialEskf = initialized(); - for time in 11..1_011 { - filter - .propagate(imu(time * 1_000, [0, 0, -1_000], [0, 0, 1_000])) - .unwrap(); - } - assert!(filter.state().attitude().is_normalized()); - assert!(filter.covariance_is_valid()); - } - - #[test] - fn covariance_cholesky() { - let mut filter: InertialEskf = initialized(); - let mut seed = 0x4d59_5df4_u32; - for sample in 1..=2_000_u64 { - seed = seed.wrapping_mul(1_664_525).wrapping_add(1_013_904_223); - let x = (seed >> 16) as i16 % 200; - seed = seed.wrapping_mul(1_664_525).wrapping_add(1_013_904_223); - let y = (seed >> 16) as i16 % 200; - seed = seed.wrapping_mul(1_664_525).wrapping_add(1_013_904_223); - let yaw = (seed >> 16) as i16 % 100; - filter - .propagate(imu(10 + sample * 1_000, [x, y, -1_000], [0, 0, yaw])) - .unwrap(); - assert!(filter.state().attitude().is_normalized()); - assert!(filter.covariance_is_valid()); - } - } - - #[test] - fn propagation_uses_ned() { - let mut filter: InertialEskf = initialized(); - filter - .propagate(imu(10_010, [1_000, 0, -1_000], [0; 3])) - .unwrap(); - assert!(filter.state().velocity().vector().x() > 0.009); - assert!(filter.state().velocity().vector().z().abs() < 0.001); - } - - #[test] - fn propagation_matches_reference() { - let mut filter: InertialEskf = initialized(); - let dt = 0.01_f64; - for step in 1..=100_u64 { - filter - .propagate(imu(10 + step * 10_000, [1_000, 0, -1_000], [0; 3])) - .unwrap(); - } - let expected_velocity = GRAVITY_MPS2 as f64 * 100.0 * dt; - let expected_position = 0.5 * GRAVITY_MPS2 as f64 * (100.0 * dt).powi(2); - let state = filter.state(); - assert!((state.velocity().vector().x() as f64 - expected_velocity).abs() < 2.0e-4); - assert!((state.position().vector().x() as f64 - expected_position).abs() < 2.0e-4); - assert!(filter.covariance_is_valid()); - } - - #[test] - fn local_aiding_bounds_state() { - let mut filter: InertialEskf = initialized(); - let observation = LocalPositionVelocityObservation { - context: ObservationContext::new( - Timestamp::from_microseconds(10), - Timestamp::from_microseconds(11), - SourceId::new(5), - FrameId::LOCAL_NED, - CalibrationRevision::new(2), - 255, - Validity::Valid, - ObservationSequence::new(7), - ), - position: PositionNed::metres(2.0, -1.0, 0.5), - velocity: VelocityNed::metres_per_second(0.4, -0.2, 0.1), - position_variance: ScalarVariance::new(0.01).unwrap(), - velocity_variance: ScalarVariance::new(0.01).unwrap(), - }; - assert_eq!( - filter.fuse_local_position_velocity( - observation, - SourceId::new(5), - CalibrationRevision::new(2), - ), - Ok(FusionResult::Fused) - ); - let state = filter.state(); - assert!((state.position().vector().x() - 2.0).abs() < 0.01); - assert!((state.position().vector().y() + 1.0).abs() < 0.01); - assert!((state.velocity().vector().x() - 0.4).abs() < 0.01); - assert!( - filter - .health(Timestamp::from_microseconds(12)) - .capabilities - .local_position - ); - } - - #[test] - fn rejected_aiding_atomic() { - let mut filter: InertialEskf = initialized(); - let before_state = filter.state(); - let before_covariance = filter.covariance; - let observation = LocalPositionVelocityObservation { - context: ObservationContext::new( - Timestamp::from_microseconds(10), - Timestamp::from_microseconds(11), - SourceId::new(5), - FrameId::LOCAL_NED, - CalibrationRevision::new(2), - 255, - Validity::Valid, - ObservationSequence::new(8), - ), - position: PositionNed::metres(0.0, 9_999.0, 0.0), - velocity: VelocityNed::metres_per_second(0.0, 0.0, 0.0), - position_variance: ScalarVariance::new(0.01).unwrap(), - velocity_variance: ScalarVariance::new(0.01).unwrap(), - }; - assert_eq!( - filter.fuse_local_position_velocity( - observation, - SourceId::new(5), - CalibrationRevision::new(2), - ), - Err(FusionError::InnovationRejected) - ); - assert_eq!(filter.state(), before_state); - assert_eq!(filter.covariance.values, before_covariance.values); - let health = filter.health(Timestamp::from_microseconds(11)); - assert_eq!(health.local_position_velocity, SourceStatus::Unknown); - assert!(!health.capabilities.local_position); - assert!(!health.capabilities.local_velocity); - } - - #[test] - fn local_reject_does_not_refresh_health() { - let mut filter: InertialEskf = initialized(); - assert_eq!( - filter.fuse_local_position_velocity( - local(), - SourceId::new(5), - CalibrationRevision::new(1), - ), - Ok(FusionResult::Fused) - ); - - let mut rejected = local(); - rejected.context.sequence = ObservationSequence::new(11); - rejected.context.receipt_time = Timestamp::from_microseconds(250_010); - rejected.position = PositionNed::metres(9_999.0, 0.0, 0.0); - assert_eq!( - filter.fuse_local_position_velocity( - rejected, - SourceId::new(5), - CalibrationRevision::new(1), - ), - Err(FusionError::InnovationRejected) - ); - let health = filter.health(Timestamp::from_microseconds(250_012)); - assert_eq!(health.local_position_velocity, SourceStatus::Stale); - assert!(!health.capabilities.local_position); - assert!(!health.capabilities.local_velocity); - } - - #[test] - fn rejects_non_monotonic_imu_time() { - let mut filter: InertialEskf = initialized(); - assert_eq!( - filter.propagate(imu(10, [0, 0, -1_000], [0; 3])), - Err(PropagationError::NonMonotonicTimestamp) - ); - } - - #[test] - fn propagation_rejects_bad_imu() { - let mut filter: InertialEskf = initialized(); - let before = filter.state(); - let mut invalid = imu(20_000, [0, 0, -1_000], [0; 3]); - invalid.context.validity = Validity::Invalid; - assert_eq!(filter.propagate(invalid), Err(PropagationError::InvalidImu)); - assert_eq!( - filter.propagate(imu(40_001, [0, 0, -1_000], [0; 3])), - Err(PropagationError::ImuGapTooLarge) - ); - assert_eq!(filter.state(), before); - assert!(!filter.requires_reinitialization()); - } - - #[test] - fn propagation_binds_initialized_imu_stream_without_mutation_on_refusal() { - let mut source_filter: InertialEskf = initialized(); - let source_before = source_filter; - let mut foreign_source = imu(11, [0, 0, -1_000], [0; 3]); - foreign_source.context.source = SourceId::new(9); - assert_eq!( - source_filter.propagate(foreign_source), - Err(PropagationError::ImuSourceMismatch) - ); - assert_eq!(source_filter.time(), source_before.time()); - assert_eq!(source_filter.state(), source_before.state()); - assert_eq!( - source_filter.covariance.values, - source_before.covariance.values - ); - assert!(!source_filter.requires_reinitialization()); - assert_eq!( - source_filter.propagate(imu(11, [0, 0, -1_000], [0; 3])), - Ok(()) - ); - - let mut calibration_filter: InertialEskf = initialized(); - let calibration_before = calibration_filter; - let mut changed_calibration = imu(11, [0, 0, -1_000], [0; 3]); - changed_calibration.context.calibration = CalibrationRevision::new(2); - assert_eq!( - calibration_filter.propagate(changed_calibration), - Err(PropagationError::ImuCalibrationMismatch) - ); - assert_eq!(calibration_filter.time(), calibration_before.time()); - assert_eq!(calibration_filter.state(), calibration_before.state()); - assert_eq!( - calibration_filter.covariance.values, - calibration_before.covariance.values - ); - assert!(!calibration_filter.requires_reinitialization()); - assert_eq!( - calibration_filter.propagate(imu(11, [0, 0, -1_000], [0; 3])), - Ok(()) - ); - - let mut sequence_filter: InertialEskf = initialized(); - assert_eq!( - sequence_filter.propagate(imu(11, [0, 0, -1_000], [0; 3])), - Ok(()) - ); - let sequence_before = sequence_filter; - let mut replay = imu(12, [0, 0, -1_000], [0; 3]); - replay.context.sequence = ObservationSequence::new(11); - assert_eq!( - sequence_filter.propagate(replay), - Err(PropagationError::ReplayedImu) - ); - assert_eq!(sequence_filter.time(), sequence_before.time()); - assert_eq!(sequence_filter.state(), sequence_before.state()); - assert_eq!( - sequence_filter.covariance.values, - sequence_before.covariance.values - ); - assert!(!sequence_filter.requires_reinitialization()); - assert_eq!( - sequence_filter.propagate(imu(12, [0, 0, -1_000], [0; 3])), - Ok(()) - ); - } - - #[test] - fn covariance_fault_latch() { - let mut filter: InertialEskf = initialized(); - let before = filter.state(); - for row in 0..INERTIAL_DIM { - for col in 0..INERTIAL_DIM { - filter.covariance.set(row, col, f32::MAX); - } - } - assert_eq!( - filter.propagate(imu(20_000, [0, 0, -1_000], [0; 3])), - Err(PropagationError::FilterInvalid) - ); - assert_eq!(filter.state(), before); - assert!(filter.requires_reinitialization()); - let health = filter.health(Timestamp::from_microseconds(20_000)); - assert!(!health.finite); - assert!(!health.initialized); - assert_eq!( - filter.propagate(imu(20_001, [0, 0, -1_000], [0; 3])), - Err(PropagationError::FilterInvalid) - ); - } - - #[test] - fn rotation_limit_preserves_state() { - let mut filter: InertialEskf = initialized(); - filter.config.maximum_imu_interval_microseconds = 100_000; - let before = filter.state(); - - assert_eq!( - filter.propagate(imu(100_010, [0, 0, -1_000], [32_767, 0, 0])), - Err(PropagationError::RotationIncrementTooLarge) - ); - assert_eq!(filter.state(), before); - } - - #[test] - fn range_update_is_finite() { - let mut filter: LocalTerrainEskf = initialized(); - let extrinsics = SensorExtrinsics { - source: SourceId::new(2), - sensor_frame: FrameId::new(7), - calibration: CalibrationRevision::new(1), - sensor_to_body: UnitQuaternion::identity(), - position_body: crate::PositionFrd::metres(0.0, 0.0, 0.2), - }; - let observation = RangeObservation { - context: context(10, FrameId::new(7), 2, 255), - metres: 1.8, - variance: ScalarVariance::new(0.01).unwrap(), - }; - assert_eq!( - filter.fuse_range(observation, extrinsics), - Ok(FusionResult::Fused) - ); - assert!(filter.covariance_is_valid()); - assert!( - filter - .health(Timestamp::from_microseconds(11)) - .capabilities - .terrain_relative_height - ); - } - - #[test] - fn range_geometry_and_staleness() { - let mut filter: LocalTerrainEskf = initialized(); - let extrinsics = SensorExtrinsics { - source: SourceId::new(2), - sensor_frame: FrameId::new(7), - calibration: CalibrationRevision::new(1), - sensor_to_body: UnitQuaternion::identity(), - position_body: crate::PositionFrd::metres(0.0, 0.0, 0.0), - }; - filter.state.attitude = - UnitQuaternion::from_components(0.707_106_77, 0.0, 0.707_106_77, 0.0).unwrap(); - let observation = RangeObservation { - context: context(10, FrameId::new(7), 2, 255), - metres: 2.0, - variance: ScalarVariance::new(0.1).unwrap(), - }; - assert_eq!( - filter.fuse_range(observation, extrinsics), - Err(FusionError::InvalidRangeGeometry) - ); - - filter.state.attitude = UnitQuaternion::identity(); - let mut recovered = observation; - recovered.context.sequence = ObservationSequence::new(11); - assert_eq!( - filter.fuse_range(recovered, extrinsics), - Ok(FusionResult::Fused) - ); - let health = filter.health(Timestamp::from_microseconds(300_002)); - assert_eq!(health.range, SourceStatus::Stale); - assert!(!health.capabilities.terrain_relative_height); - } - - #[test] - fn flow_frame_and_quality() { - let mut filter: InertialEskf = initialized(); - let extrinsics = SensorExtrinsics { - source: SourceId::new(3), - sensor_frame: FrameId::new(8), - calibration: CalibrationRevision::new(1), - sensor_to_body: UnitQuaternion::from_components(0.0, 0.0, 0.0, 1.0).unwrap(), - position_body: crate::PositionFrd::metres(0.0, 0.0, 0.0), - }; - filter.state.velocity = VelocityNed::metres_per_second(1.0, 0.0, 0.0); - let covariance = Covariance2::new( - ScalarVariance::new(0.01).unwrap(), - 0.0, - ScalarVariance::new(0.01).unwrap(), - ) - .unwrap(); - let rejected = OpticalFlowObservation { - context: context(10, FrameId::new(8), 3, 1), - velocity: kaptan_math::Vector2::new(0.0, 0.0), - ground_distance_metres: 1.0, - covariance, - }; - assert_eq!( - filter.fuse_optical_flow(rejected, extrinsics), - Err(FusionError::InvalidObservation) - ); - let mut accepted = OpticalFlowObservation { - context: context(10, FrameId::new(8), 3, 255), - velocity: kaptan_math::Vector2::new(-1.0, 0.0), - ground_distance_metres: 1.0, - covariance, - }; - accepted.context.sequence = ObservationSequence::new(11); - assert_eq!( - filter.fuse_optical_flow(accepted, extrinsics), - Ok(FusionResult::Fused) - ); - } - - #[test] - fn two_axis_gain_validation() { - let mut filter: InertialEskf = initialized(); - filter.covariance.set(VELOCITY, VELOCITY, -0.02); - filter.covariance.set(VELOCITY + 1, VELOCITY + 1, -0.02); - let mut h = [[0.0; MAX_DIM]; 2]; - h[0][VELOCITY] = 1.0; - h[1][VELOCITY + 1] = 1.0; - let covariance = Covariance2::new( - ScalarVariance::new(0.01).unwrap(), - 0.0, - ScalarVariance::new(0.01).unwrap(), - ) - .unwrap(); - - assert!(filter.two_gain(h, covariance).is_none()); - } - - #[test] - fn range_source_recovery() { - let mut filter: LocalTerrainEskf = initialized(); - let extrinsics = SensorExtrinsics { - source: SourceId::new(4), - sensor_frame: FrameId::new(7), - calibration: CalibrationRevision::new(1), - sensor_to_body: UnitQuaternion::identity(), - position_body: crate::PositionFrd::metres(0.0, 0.0, 0.0), - }; - let bad = |sequence| RangeObservation { - context: ObservationContext::new( - Timestamp::from_microseconds(10), - Timestamp::from_microseconds(10 + u64::from(sequence)), - SourceId::new(4), - FrameId::new(7), - CalibrationRevision::new(1), - 255, - Validity::Valid, - ObservationSequence::new(sequence), - ), - metres: 40.0, - variance: ScalarVariance::new(0.001).unwrap(), - }; - for sequence in 0..3 { - assert_eq!( - filter.fuse_range(bad(sequence), extrinsics), - Err(FusionError::InnovationRejected) - ); - } - assert_eq!( - filter.health(Timestamp::from_microseconds(20)).range, - SourceStatus::Unknown - ); - let good = |sequence| RangeObservation { - context: ObservationContext::new( - Timestamp::from_microseconds(10), - Timestamp::from_microseconds(30 + u64::from(sequence)), - SourceId::new(4), - FrameId::new(7), - CalibrationRevision::new(1), - 255, - Validity::Valid, - ObservationSequence::new(sequence), - ), - metres: 2.0, - variance: ScalarVariance::new(0.1).unwrap(), - }; - assert_eq!( - filter.fuse_range(good(3), extrinsics), - Ok(FusionResult::SourceIsolated) - ); - assert_eq!( - filter.health(Timestamp::from_microseconds(34)).range, - SourceStatus::Isolated - ); - assert_eq!( - filter.fuse_range(good(4), extrinsics), - Ok(FusionResult::Fused) - ); - } - - #[test] - fn configuration_validation() { - let config = EstimatorConfig::default(); - for invalid in [ - EstimatorConfig { - gravity_mps2: 0.0, - ..config - }, - EstimatorConfig { - gravity_mps2: f32::NAN, - ..config - }, - EstimatorConfig { - gyro_noise_variance: -1.0, - ..config - }, - EstimatorConfig { - gyro_noise_variance: f32::NAN, - ..config - }, - EstimatorConfig { - acceleration_noise_variance: f32::NAN, - ..config - }, - EstimatorConfig { - acceleration_noise_variance: -1.0, - ..config - }, - EstimatorConfig { - gyro_bias_walk_variance: -1.0, - ..config - }, - EstimatorConfig { - gyro_bias_walk_variance: f32::NAN, - ..config - }, - EstimatorConfig { - acceleration_bias_walk_variance: -1.0, - ..config - }, - EstimatorConfig { - acceleration_bias_walk_variance: f32::NAN, - ..config - }, - EstimatorConfig { - terrain_walk_variance: -1.0, - ..config - }, - EstimatorConfig { - terrain_walk_variance: f32::NAN, - ..config - }, - EstimatorConfig { - maximum_imu_interval_microseconds: 0, - ..config - }, - EstimatorConfig { - minimum_range_down_cosine: 1.1, - ..config - }, - EstimatorConfig { - minimum_range_down_cosine: f32::NAN, - ..config - }, - EstimatorConfig { - minimum_range_down_cosine: 0.0, - ..config - }, - EstimatorConfig { - minimum_range_metres: 0.0, - ..config - }, - EstimatorConfig { - minimum_range_metres: f32::NAN, - ..config - }, - EstimatorConfig { - maximum_range_metres: 0.01, - ..config - }, - EstimatorConfig { - maximum_range_metres: f32::NAN, - ..config - }, - EstimatorConfig { - maximum_flow_speed_metres_per_second: 0.0, - ..config - }, - EstimatorConfig { - maximum_flow_speed_metres_per_second: f32::NAN, - ..config - }, - EstimatorConfig { - range_gate_chi_square: 0.0, - ..config - }, - EstimatorConfig { - range_gate_chi_square: f32::NAN, - ..config - }, - EstimatorConfig { - flow_gate_chi_square: 0.0, - ..config - }, - EstimatorConfig { - flow_gate_chi_square: f32::NAN, - ..config - }, - EstimatorConfig { - maximum_local_position_metres: 0.0, - ..config - }, - EstimatorConfig { - maximum_local_position_metres: f32::NAN, - ..config - }, - EstimatorConfig { - maximum_local_speed_metres_per_second: 0.0, - ..config - }, - EstimatorConfig { - maximum_local_speed_metres_per_second: f32::NAN, - ..config - }, - EstimatorConfig { - local_state_gate_chi_square: 0.0, - ..config - }, - EstimatorConfig { - local_state_gate_chi_square: f32::NAN, - ..config - }, - ] { - assert!(!invalid.is_valid()); - } - for invalid in [ - IntegrityConfig { - maximum_age_microseconds: 0, - ..IntegrityConfig::default() - }, - IntegrityConfig { - rejection_limit: 0, - ..IntegrityConfig::default() - }, - IntegrityConfig { - readmission_accepts: 0, - ..IntegrityConfig::default() - }, - ] { - assert!(!invalid.is_valid()); - } - } - - #[test] - fn initialization_validation() { - let config = EstimatorConfig::default(); - let mut initializer = StationaryInitializer::<1>::default(); - let mut invalid_imu = imu(1, [0, 0, -1_000], [0; 3]); - invalid_imu.context.validity = Validity::Invalid; - assert!(!initializer.push(invalid_imu)); - invalid_imu.context.validity = Validity::Valid; - invalid_imu.context.frame = FrameId::new(7); - assert!(!initializer.push(invalid_imu)); - invalid_imu.context.frame = FrameId::BODY_FRD; - invalid_imu.acceleration = AccelerationFrd::metres_per_second_squared(f32::NAN, 0.0, 0.0); - assert!(!initializer.push(invalid_imu)); - invalid_imu.acceleration = AccelerationFrd::metres_per_second_squared(0.0, 0.0, 0.0); - invalid_imu.angular_rate = AngularRateFrd::radians_per_second(f32::NAN, 0.0, 0.0); - assert!(!initializer.push(invalid_imu)); - assert!(initializer.push(imu(2, [0, 0, -1_000], [0; 3]))); - assert!(initializer.is_complete()); - assert!(!initializer.push(imu(3, [0, 0, -1_000], [0; 3]))); - assert!( - initializer - .finish::<15>( - Timestamp::from_microseconds(2), - config, - IntegrityConfig::default(), - f32::NAN - ) - .is_none() - ); - assert!( - StationaryInitializer::<0>::new() - .finish::<15>( - Timestamp::from_microseconds(4), - config, - IntegrityConfig::default(), - 0.0 - ) - .is_none() - ); - assert!( - StationaryInitializer::<1>::new() - .finish::<15>( - Timestamp::from_microseconds(10), - config, - IntegrityConfig::default(), - 0.0 - ) - .is_none() - ); - let mut invalid_finish = StationaryInitializer::<1>::new(); - assert!(invalid_finish.push(imu(4, [0, 0, -1_000], [0; 3]))); - assert!( - invalid_finish - .finish::<15>( - Timestamp::from_microseconds(4), - EstimatorConfig { - gravity_mps2: 0.0, - ..config - }, - IntegrityConfig::default(), - 0.0 - ) - .is_none() - ); - let mut invalid_sum = StationaryInitializer::<1>::new(); - invalid_sum.count = 1; - invalid_sum.last_time = Some(Timestamp::from_microseconds(10)); - invalid_sum.gyro_sum = Vector3::new(f32::NAN, 0.0, 0.0); - assert!( - invalid_sum - .finish::<15>( - Timestamp::from_microseconds(10), - config, - IntegrityConfig::default(), - 0.0 - ) - .is_none() - ); - let mut invalid_acceleration_sum = StationaryInitializer::<1>::new(); - invalid_acceleration_sum.count = 1; - invalid_acceleration_sum.last_time = Some(Timestamp::from_microseconds(10)); - invalid_acceleration_sum.acceleration_sum = Vector3::new(f32::NAN, 0.0, 0.0); - assert!( - invalid_acceleration_sum - .finish::<15>( - Timestamp::from_microseconds(10), - config, - IntegrityConfig::default(), - 0.0 - ) - .is_none() - ); - - let mut timestamp_mismatch = StationaryInitializer::<1>::new(); - assert!(timestamp_mismatch.push(imu(5, [0, 0, -1_000], [0; 3]))); - assert!( - timestamp_mismatch - .finish::<15>( - Timestamp::from_microseconds(6), - config, - IntegrityConfig::default(), - 0.0 - ) - .is_none() - ); - } - - #[test] - fn state_validation() { - let config = EstimatorConfig::default(); - let filter: LocalTerrainEskf = initialized(); - let state = filter.state(); - assert!(state.acceleration_bias().vector().norm() < 2.0e-6); - assert_eq!(state.terrain_down_metres::<16>(), Some(2.0)); - assert_eq!( - initialized::<15>().state().terrain_down_metres::<15>(), - None - ); - assert_eq!( - filter.health(Timestamp::from_microseconds(10)).range, - SourceStatus::Unknown - ); - assert!( - filter - .health(Timestamp::from_microseconds(10)) - .capabilities - .attitude - ); - let mut invalid_state = state; - invalid_state.position = PositionNed::metres(f32::NAN, 0.0, 0.0); - assert!(!state_is_valid::<16>(invalid_state)); - invalid_state = state; - invalid_state.velocity = VelocityNed::metres_per_second(f32::NAN, 0.0, 0.0); - assert!(!state_is_valid::<16>(invalid_state)); - invalid_state = state; - invalid_state.gyro_bias = AngularRateFrd::radians_per_second(f32::NAN, 0.0, 0.0); - assert!(!state_is_valid::<16>(invalid_state)); - invalid_state = state; - invalid_state.acceleration_bias = - AccelerationFrd::metres_per_second_squared(f32::NAN, 0.0, 0.0); - assert!(!state_is_valid::<16>(invalid_state)); - invalid_state = state; - invalid_state.terrain_down_metres = f32::NAN; - assert!(!state_is_valid::<16>(invalid_state)); - assert!( - Eskf::<15>::new( - Timestamp::from_microseconds(10), - state, - EstimatorConfig { - gravity_mps2: 0.0, - ..config - }, - IntegrityConfig::default(), - imu_stream(), - ) - .is_err() - ); - assert!( - Eskf::<15>::new( - Timestamp::from_microseconds(10), - state, - config, - IntegrityConfig { - rejection_limit: 0, - ..IntegrityConfig::default() - }, - imu_stream(), - ) - .is_err() - ); - } - - #[test] - fn covariance_validation() { - let config = EstimatorConfig::default(); - let state = initialized::<16>().state(); - let mut covariance = Covariance::diagonal(); - covariance.set(0, 1, f32::NAN); - assert!(!covariance.finite(2)); - assert!(!covariance.positive_definite(2)); - covariance = Covariance { - values: [0.0; MATRIX], - }; - assert!(!covariance.positive_definite(2)); - covariance.set(0, 0, 1.0); - covariance.set(1, 1, 1.0); - covariance.set(1, 0, 2.0); - covariance.set(0, 1, 2.0); - assert!(!covariance.positive_definite(2)); - covariance = Covariance { - values: [0.0; MATRIX], - }; - covariance.set(0, 0, f32::MIN_POSITIVE); - covariance.set(1, 0, f32::MAX); - covariance.set(1, 1, f32::MAX); - assert!(!covariance.positive_definite(2)); - - assert!( - Eskf::<14>::new( - Timestamp::from_microseconds(10), - state, - config, - IntegrityConfig::default(), - imu_stream(), - ) - .is_err() - ); - } - - #[test] - fn source_integrity_state_machine() { - let config = IntegrityConfig { - maximum_age_microseconds: 5, - rejection_limit: 2, - readmission_accepts: 2, - }; - let mut integrity = SourceIntegrity::new(config); - assert_eq!( - integrity.status(None, Timestamp::from_microseconds(0)), - SourceStatus::Unknown - ); - assert_eq!( - integrity.status(Some(SourceId::new(1)), Timestamp::from_microseconds(0)), - SourceStatus::Unknown - ); - for source in 1..=4 { - assert!(integrity.reserve(SourceId::new(source))); - } - assert!(integrity.reserve(SourceId::new(1))); - assert!(!integrity.reserve(SourceId::new(5))); - assert!(integrity.accepts_sequence(SourceId::new(1), ObservationSequence::new(10))); - assert!(!integrity.accepts_sequence(SourceId::new(1), ObservationSequence::new(10))); - assert!(!integrity.accepts_sequence(SourceId::new(1), ObservationSequence::new(9))); - assert!(integrity.accepts_sequence(SourceId::new(1), ObservationSequence::new(11))); - assert!(integrity.accept(SourceId::new(1), Timestamp::from_microseconds(9))); - integrity.reject(SourceId::new(1)); - assert_eq!( - integrity.status(Some(SourceId::new(1)), Timestamp::from_microseconds(10)), - SourceStatus::Healthy - ); - assert_eq!( - integrity.status(Some(SourceId::new(1)), Timestamp::from_microseconds(16)), - SourceStatus::Stale - ); - integrity.reject(SourceId::new(1)); - assert_eq!( - integrity.status(Some(SourceId::new(1)), Timestamp::from_microseconds(17)), - SourceStatus::Isolated - ); - assert!(!integrity.accept(SourceId::new(1), Timestamp::from_microseconds(18))); - assert!(integrity.accept(SourceId::new(1), Timestamp::from_microseconds(19))); - assert_eq!( - integrity.status(Some(SourceId::new(1)), Timestamp::from_microseconds(19)), - SourceStatus::Healthy - ); - } - - #[test] - fn range_metadata_validation() { - let range_extrinsics = range_extrinsics(); - let mut mismatch = range(); - mismatch.context.measurement_time = Timestamp::from_microseconds(11); - assert_eq!( - initialized::<16>().fuse_range(mismatch, range_extrinsics), - Err(FusionError::TimestampMismatch) - ); - assert_eq!( - initialized::<15>().fuse_range(range(), range_extrinsics), - Err(FusionError::TerrainProfileRequired) - ); - let mut invalid = range(); - invalid.context.validity = Validity::Invalid; - assert_eq!( - initialized::<16>().fuse_range(invalid, range_extrinsics), - Err(FusionError::InvalidObservation) - ); - let mut wrong_frame = range(); - wrong_frame.context.frame = FrameId::new(8); - assert_eq!( - initialized::<16>().fuse_range(wrong_frame, range_extrinsics), - Err(FusionError::FrameMismatch) - ); - let mut wrong_source = range(); - wrong_source.context.source = SourceId::new(3); - assert_eq!( - initialized::<16>().fuse_range(wrong_source, range_extrinsics), - Err(FusionError::SourceMismatch) - ); - let mut wrong_calibration = range(); - wrong_calibration.context.calibration = CalibrationRevision::new(2); - assert_eq!( - initialized::<16>().fuse_range(wrong_calibration, range_extrinsics), - Err(FusionError::CalibrationMismatch) - ); - let mut implausible = range(); - implausible.metres = 0.0; - assert_eq!( - initialized::<16>().fuse_range(implausible, range_extrinsics), - Err(FusionError::Implausible) - ); - let mut range_quality = range(); - range_quality.context.quality = 0; - assert_eq!( - initialized::<16>().fuse_range(range_quality, range_extrinsics), - Err(FusionError::InvalidObservation) - ); - let mut range_nan = range(); - range_nan.metres = f32::NAN; - assert_eq!( - initialized::<16>().fuse_range(range_nan, range_extrinsics), - Err(FusionError::Implausible) - ); - let mut range_high = range(); - range_high.metres = 41.0; - assert_eq!( - initialized::<16>().fuse_range(range_high, range_extrinsics), - Err(FusionError::Implausible) - ); - let mut replay = initialized::<16>(); - assert_eq!( - replay.fuse_range(range(), range_extrinsics), - Ok(FusionResult::Fused) - ); - assert_eq!( - replay.fuse_range(range(), range_extrinsics), - Err(FusionError::ReplayedObservation) - ); - } - - #[test] - fn rejected_aiding_is_unhealthy() { - let mut filter: LocalTerrainEskf = initialized(); - let mut wrong_source = range(); - wrong_source.context.source = SourceId::new(4); - assert_eq!( - filter.fuse_range(wrong_source, range_extrinsics()), - Err(FusionError::SourceMismatch) - ); - assert!(filter.integrity.entries.iter().all(|entry| !entry.used)); - - let mut rejected = range(); - rejected.metres = 40.0; - rejected.context.sequence = ObservationSequence::new(10); - assert_eq!( - filter.fuse_range(rejected, range_extrinsics()), - Err(FusionError::InnovationRejected) - ); - assert_eq!( - filter.health(Timestamp::from_microseconds(10)).range, - SourceStatus::Unknown - ); - - let mut accepted = range(); - accepted.context.sequence = ObservationSequence::new(11); - accepted.context.receipt_time = Timestamp::from_microseconds(11); - assert_eq!( - filter.fuse_range(accepted, range_extrinsics()), - Ok(FusionResult::Fused) - ); - assert_eq!( - filter.health(Timestamp::from_microseconds(11)).range, - SourceStatus::Healthy - ); - } - - #[test] - fn bad_metadata_cannot_fill_sources() { - let mut filter: LocalTerrainEskf = initialized(); - for source in 1..=4 { - let mut malformed = range(); - malformed.context.source = SourceId::new(source); - malformed.context.frame = FrameId::new(8); - let mut extrinsics = range_extrinsics(); - extrinsics.source = SourceId::new(source); - assert_eq!( - filter.fuse_range(malformed, extrinsics), - Err(FusionError::FrameMismatch) - ); - } - assert!(filter.integrity.entries.iter().all(|entry| !entry.used)); - - let mut valid = range(); - valid.context.source = SourceId::new(5); - let mut extrinsics = range_extrinsics(); - extrinsics.source = SourceId::new(5); - assert_eq!( - filter.fuse_range(valid, extrinsics), - Ok(FusionResult::Fused) - ); - } - - #[test] - fn reserved_source_isolates_on_bad_metadata() { - let mut filter: LocalTerrainEskf = initialized(); - assert_eq!( - filter.fuse_range(range(), range_extrinsics()), - Ok(FusionResult::Fused) - ); - for _ in 0..IntegrityConfig::default().rejection_limit { - let mut malformed = range(); - malformed.context.frame = FrameId::new(8); - assert_eq!( - filter.fuse_range(malformed, range_extrinsics()), - Err(FusionError::FrameMismatch) - ); - } - assert_eq!( - filter.health(Timestamp::from_microseconds(11)).range, - SourceStatus::Isolated - ); - } - - #[test] - fn flow_metadata_validation() { - let flow_extrinsics = flow_extrinsics(); - let mut flow_invalid = flow(); - flow_invalid.context.quality = 0; - assert_eq!( - initialized::<15>().fuse_optical_flow(flow_invalid, flow_extrinsics), - Err(FusionError::InvalidObservation) - ); - let mut flow_validity = flow(); - flow_validity.context.validity = Validity::Invalid; - assert_eq!( - initialized::<15>().fuse_optical_flow(flow_validity, flow_extrinsics), - Err(FusionError::InvalidObservation) - ); - let mut flow_frame = flow(); - flow_frame.context.frame = FrameId::new(9); - assert_eq!( - initialized::<15>().fuse_optical_flow(flow_frame, flow_extrinsics), - Err(FusionError::FrameMismatch) - ); - let mut flow_source = flow(); - flow_source.context.source = SourceId::new(4); - assert_eq!( - initialized::<15>().fuse_optical_flow(flow_source, flow_extrinsics), - Err(FusionError::SourceMismatch) - ); - let mut flow_calibration = flow(); - flow_calibration.context.calibration = CalibrationRevision::new(2); - assert_eq!( - initialized::<15>().fuse_optical_flow(flow_calibration, flow_extrinsics), - Err(FusionError::CalibrationMismatch) - ); - let mut flow_implausible = flow(); - flow_implausible.ground_distance_metres = 0.0; - assert_eq!( - initialized::<15>().fuse_optical_flow(flow_implausible, flow_extrinsics), - Err(FusionError::Implausible) - ); - let mut flow_nan_velocity = flow(); - flow_nan_velocity.velocity = kaptan_math::Vector2::new(f32::NAN, 0.0); - assert_eq!( - initialized::<15>().fuse_optical_flow(flow_nan_velocity, flow_extrinsics), - Err(FusionError::Implausible) - ); - let mut flow_nan_distance = flow(); - flow_nan_distance.ground_distance_metres = f32::NAN; - assert_eq!( - initialized::<15>().fuse_optical_flow(flow_nan_distance, flow_extrinsics), - Err(FusionError::Implausible) - ); - let mut flow_fast_x = flow(); - flow_fast_x.velocity = kaptan_math::Vector2::new(26.0, 0.0); - assert_eq!( - initialized::<15>().fuse_optical_flow(flow_fast_x, flow_extrinsics), - Err(FusionError::Implausible) - ); - let mut flow_fast_y = flow(); - flow_fast_y.velocity = kaptan_math::Vector2::new(0.0, 26.0); - assert_eq!( - initialized::<15>().fuse_optical_flow(flow_fast_y, flow_extrinsics), - Err(FusionError::Implausible) - ); - } - - #[test] - fn optical_flow_refuses_offset_sensor_without_mutating_integrity() { - let mut filter: InertialEskf = initialized(); - let mut lever_extrinsics = flow_extrinsics(); - lever_extrinsics.position_body = crate::PositionFrd::metres(1.0, 0.0, 0.0); - assert_eq!( - filter.fuse_optical_flow(flow(), lever_extrinsics), - Err(FusionError::UnsupportedOpticalFlowLeverArm) - ); - assert!(filter.integrity.entries.iter().all(|entry| !entry.used)); - } - - #[test] - fn optical_flow_models_attitude_error_for_a_colocated_sensor() { - let mut attitude_filter: InertialEskf = initialized(); - attitude_filter.state.velocity = VelocityNed::metres_per_second(2.0, 0.0, 0.0); - let mut attitude_flow = flow(); - attitude_flow.velocity = kaptan_math::Vector2::new(2.0, 0.0); - let before_attitude = attitude_filter.covariance.get(ATTITUDE + 2, ATTITUDE + 2); - assert_eq!( - attitude_filter.fuse_optical_flow(attitude_flow, flow_extrinsics()), - Ok(FusionResult::Fused) - ); - assert!(attitude_filter.covariance.get(ATTITUDE + 2, ATTITUDE + 2) < before_attitude); - - let mut finite_difference: InertialEskf = initialized(); - finite_difference.state.velocity = VelocityNed::metres_per_second(1.2, -0.7, 0.4); - finite_difference.state.attitude = - UnitQuaternion::from_components(0.962_425_2, 0.087_155_7, -0.130_526_2, 0.217_376_3) - .unwrap(); - let mut rotated_sensor = flow_extrinsics(); - rotated_sensor.sensor_to_body = - UnitQuaternion::from_components(0.965_925_8, 0.0, 0.0, 0.258_819).unwrap(); - let (_, jacobian) = finite_difference.co_located_flow_model(rotated_sensor); - let base_attitude = finite_difference.state.attitude; - const EPSILON: f32 = 1.0e-3; - for axis in 0..3 { - let basis = match axis { - 0 => Vector3::::new(1.0, 0.0, 0.0), - 1 => Vector3::::new(0.0, 1.0, 0.0), - _ => Vector3::::new(0.0, 0.0, 1.0), - }; - let mut plus = finite_difference; - plus.state.attitude = UnitQuaternion::from_small_rotation_vector(basis.scale(EPSILON)) - .unwrap() - .compose(base_attitude) - .unwrap(); - let mut minus = finite_difference; - minus.state.attitude = - UnitQuaternion::from_small_rotation_vector(basis.scale(-EPSILON)) - .unwrap() - .compose(base_attitude) - .unwrap(); - let (positive, _) = plus.co_located_flow_model(rotated_sensor); - let (negative, _) = minus.co_located_flow_model(rotated_sensor); - for row in 0..2 { - let numerical = (positive[row] - negative[row]) / (2.0 * EPSILON); - assert!((numerical - jacobian[row][ATTITUDE + axis]).abs() < 2.0e-3); - } - } - } - - #[test] - fn left_mekf_reset_uses_positive_half_skew() { - let mut filter: InertialEskf = initialized(); - filter.covariance.set(ATTITUDE + 1, POSITION, 2.0); - filter.covariance.set(POSITION, ATTITUDE + 1, 2.0); - filter.reset_attitude_error(Vector3::new(0.0, 0.0, 0.2)); - assert!((filter.covariance.get(ATTITUDE, POSITION) + 0.2).abs() < 1.0e-6); - assert!((filter.covariance.get(POSITION, ATTITUDE) + 0.2).abs() < 1.0e-6); - } - - #[test] - fn local_metadata_validation() { - let mut local_invalid = local(); - local_invalid.context.quality = 0; - assert_eq!( - initialized::<15>().fuse_local_position_velocity( - local_invalid, - SourceId::new(5), - CalibrationRevision::new(1) - ), - Err(FusionError::InvalidObservation) - ); - let mut local_validity = local(); - local_validity.context.validity = Validity::Invalid; - assert_eq!( - initialized::<15>().fuse_local_position_velocity( - local_validity, - SourceId::new(5), - CalibrationRevision::new(1) - ), - Err(FusionError::InvalidObservation) - ); - let mut local_frame = local(); - local_frame.context.frame = FrameId::BODY_FRD; - assert_eq!( - initialized::<15>().fuse_local_position_velocity( - local_frame, - SourceId::new(5), - CalibrationRevision::new(1) - ), - Err(FusionError::FrameMismatch) - ); - assert_eq!( - initialized::<15>().fuse_local_position_velocity( - local(), - SourceId::new(6), - CalibrationRevision::new(1) - ), - Err(FusionError::SourceMismatch) - ); - assert_eq!( - initialized::<15>().fuse_local_position_velocity( - local(), - SourceId::new(5), - CalibrationRevision::new(2) - ), - Err(FusionError::CalibrationMismatch) - ); - let mut local_implausible = local(); - local_implausible.position = PositionNed::metres(20_000.0, 0.0, 0.0); - assert_eq!( - initialized::<15>().fuse_local_position_velocity( - local_implausible, - SourceId::new(5), - CalibrationRevision::new(1) - ), - Err(FusionError::Implausible) - ); - let mut local_nan_position = local(); - local_nan_position.position = PositionNed::metres(f32::NAN, 0.0, 0.0); - assert_eq!( - initialized::<15>().fuse_local_position_velocity( - local_nan_position, - SourceId::new(5), - CalibrationRevision::new(1) - ), - Err(FusionError::Implausible) - ); - let mut local_nan_velocity = local(); - local_nan_velocity.velocity = VelocityNed::metres_per_second(f32::NAN, 0.0, 0.0); - assert_eq!( - initialized::<15>().fuse_local_position_velocity( - local_nan_velocity, - SourceId::new(5), - CalibrationRevision::new(1) - ), - Err(FusionError::Implausible) - ); - let mut local_position_y = local(); - local_position_y.position = PositionNed::metres(0.0, 20_000.0, 0.0); - assert_eq!( - initialized::<15>().fuse_local_position_velocity( - local_position_y, - SourceId::new(5), - CalibrationRevision::new(1) - ), - Err(FusionError::Implausible) - ); - let mut local_position_z = local(); - local_position_z.position = PositionNed::metres(0.0, 0.0, 20_000.0); - assert_eq!( - initialized::<15>().fuse_local_position_velocity( - local_position_z, - SourceId::new(5), - CalibrationRevision::new(1) - ), - Err(FusionError::Implausible) - ); - let mut local_velocity_x = local(); - local_velocity_x.velocity = VelocityNed::metres_per_second(200.0, 0.0, 0.0); - assert_eq!( - initialized::<15>().fuse_local_position_velocity( - local_velocity_x, - SourceId::new(5), - CalibrationRevision::new(1) - ), - Err(FusionError::Implausible) - ); - let mut local_velocity_y = local(); - local_velocity_y.velocity = VelocityNed::metres_per_second(0.0, 200.0, 0.0); - assert_eq!( - initialized::<15>().fuse_local_position_velocity( - local_velocity_y, - SourceId::new(5), - CalibrationRevision::new(1) - ), - Err(FusionError::Implausible) - ); - let mut local_velocity_z = local(); - local_velocity_z.velocity = VelocityNed::metres_per_second(0.0, 0.0, 200.0); - assert_eq!( - initialized::<15>().fuse_local_position_velocity( - local_velocity_z, - SourceId::new(5), - CalibrationRevision::new(1) - ), - Err(FusionError::Implausible) - ); - } - - #[test] - fn propagation_faults_fail_closed() { - let mut no_receipt = SourceIntegrity::new(IntegrityConfig::default()); - assert!(no_receipt.reserve(SourceId::new(1))); - assert_eq!( - no_receipt.status(Some(SourceId::new(1)), Timestamp::from_microseconds(10)), - SourceStatus::Unknown - ); - let mut wrong_frame_imu = imu(11, [0, 0, -1_000], [0; 3]); - wrong_frame_imu.context.frame = FrameId::new(7); - assert_eq!( - initialized::<15>().propagate(wrong_frame_imu), - Err(PropagationError::InvalidImu) - ); - let mut nonfinite_acceleration = imu(11, [0, 0, -1_000], [0; 3]); - nonfinite_acceleration.acceleration = - AccelerationFrd::metres_per_second_squared(f32::NAN, 0.0, 0.0); - assert_eq!( - initialized::<15>().propagate(nonfinite_acceleration), - Err(PropagationError::InvalidImu) - ); - let mut nonfinite_rate = imu(11, [0, 0, -1_000], [0; 3]); - nonfinite_rate.angular_rate = AngularRateFrd::radians_per_second(f32::NAN, 0.0, 0.0); - assert_eq!( - initialized::<15>().propagate(nonfinite_rate), - Err(PropagationError::InvalidImu) - ); - } - - #[test] - fn range_faults_fail_closed() { - let range_extrinsics = range_extrinsics(); - let mut range_capacity = initialized::<16>(); - for source in 1..=4 { - assert!(range_capacity.integrity.reserve(SourceId::new(source))); - } - let mut over_capacity = range(); - over_capacity.context.source = SourceId::new(5); - let mut capacity_extrinsics = range_extrinsics; - capacity_extrinsics.source = SourceId::new(5); - assert_eq!( - range_capacity.fuse_range(over_capacity, capacity_extrinsics), - Err(FusionError::SourceCapacityExhausted) - ); - - let mut below_terrain = initialized::<16>(); - below_terrain.state.terrain_down_metres = 0.0; - assert_eq!( - below_terrain.fuse_range(range(), range_extrinsics), - Err(FusionError::InvalidRangeGeometry) - ); - let mut singular_range = initialized::<16>(); - singular_range - .covariance - .set(POSITION + 2, POSITION + 2, -0.1); - singular_range.covariance.set(TERRAIN, TERRAIN, 0.0); - assert_eq!( - singular_range.fuse_range(range(), range_extrinsics), - Err(FusionError::SingularInnovation) - ); - let mut infinite_range_nis = initialized::<16>(); - infinite_range_nis.config.maximum_range_metres = f32::MAX; - let mut enormous_range = range(); - enormous_range.metres = f32::MAX; - assert_eq!( - infinite_range_nis.fuse_range(enormous_range, range_extrinsics), - Err(FusionError::InnovationRejected) - ); - let mut nonfinite_prediction = initialized::<16>(); - nonfinite_prediction.state.terrain_down_metres = f32::MAX; - nonfinite_prediction.state.position = PositionNed::metres(0.0, 0.0, -f32::MAX); - assert_eq!( - nonfinite_prediction.fuse_range(range(), range_extrinsics), - Err(FusionError::SingularInnovation) - ); - let mut nonfinite_innovation = initialized::<16>(); - nonfinite_innovation.config.maximum_range_metres = f32::MAX; - nonfinite_innovation.state.terrain_down_metres = f32::MAX; - let mut matching_maximum = range(); - matching_maximum.metres = f32::MAX; - assert_eq!( - nonfinite_innovation.fuse_range(matching_maximum, range_extrinsics), - Ok(FusionResult::Fused) - ); - let mut invalid_range_candidate = initialized::<16>(); - invalid_range_candidate.state.gyro_bias = - AngularRateFrd::radians_per_second(f32::NAN, 0.0, 0.0); - assert_eq!( - invalid_range_candidate.fuse_range(range(), range_extrinsics), - Err(FusionError::FilterInvalid) - ); - let mut failed_range = initialized::<16>(); - failed_range.failed = true; - assert_eq!( - failed_range.fuse_range(range(), range_extrinsics), - Err(FusionError::FilterInvalid) - ); - } - - #[test] - fn flow_faults_fail_closed() { - let flow_extrinsics = flow_extrinsics(); - let mut flow_timestamp = flow(); - flow_timestamp.context.measurement_time = Timestamp::from_microseconds(11); - assert_eq!( - initialized::<15>().fuse_optical_flow(flow_timestamp, flow_extrinsics), - Err(FusionError::TimestampMismatch) - ); - let mut flow_capacity = initialized::<15>(); - for source in 1..=4 { - assert!(flow_capacity.integrity.reserve(SourceId::new(source))); - } - let mut flow_over_capacity = flow(); - flow_over_capacity.context.source = SourceId::new(5); - let mut capacity_extrinsics = flow_extrinsics; - capacity_extrinsics.source = SourceId::new(5); - assert_eq!( - flow_capacity.fuse_optical_flow(flow_over_capacity, capacity_extrinsics), - Err(FusionError::SourceCapacityExhausted) - ); - let mut flow_replay = initialized::<15>(); - assert_eq!( - flow_replay.fuse_optical_flow(flow(), flow_extrinsics), - Ok(FusionResult::Fused) - ); - assert_eq!( - flow_replay.fuse_optical_flow(flow(), flow_extrinsics), - Err(FusionError::ReplayedObservation) - ); - let mut flow_singular = initialized::<15>(); - flow_singular.covariance.set(VELOCITY, VELOCITY, -0.1); - flow_singular - .covariance - .set(VELOCITY + 1, VELOCITY + 1, -0.1); - assert_eq!( - flow_singular.fuse_optical_flow(flow(), flow_extrinsics), - Err(FusionError::SingularInnovation) - ); - let mut flow_gate = flow(); - flow_gate.velocity = kaptan_math::Vector2::new(24.0, 0.0); - assert_eq!( - initialized::<15>().fuse_optical_flow(flow_gate, flow_extrinsics), - Err(FusionError::InnovationRejected) - ); - let mut infinite_flow_nis = initialized::<15>(); - infinite_flow_nis - .config - .maximum_flow_speed_metres_per_second = f32::MAX; - let mut enormous_flow = flow(); - enormous_flow.velocity = kaptan_math::Vector2::new(f32::MAX, 0.0); - assert_eq!( - infinite_flow_nis.fuse_optical_flow(enormous_flow, flow_extrinsics), - Err(FusionError::InnovationRejected) - ); - let mut flow_isolated = initialized::<15>(); - assert!(flow_isolated.integrity.reserve(SourceId::new(3))); - flow_isolated - .integrity - .entry_mut(SourceId::new(3)) - .expect("test reserved source") - .isolated = true; - assert_eq!( - flow_isolated.fuse_optical_flow(flow(), flow_extrinsics), - Ok(FusionResult::SourceIsolated) - ); - let mut failed_flow = initialized::<15>(); - failed_flow.failed = true; - assert_eq!( - failed_flow.fuse_optical_flow(flow(), flow_extrinsics), - Err(FusionError::FilterInvalid) - ); - let mut invalid_flow_candidate = initialized::<15>(); - invalid_flow_candidate.state.gyro_bias = - AngularRateFrd::radians_per_second(f32::NAN, 0.0, 0.0); - assert_eq!( - invalid_flow_candidate.fuse_optical_flow(flow(), flow_extrinsics), - Err(FusionError::FilterInvalid) - ); - } - - #[test] - fn candidate_validation_and_nonfinite_extrinsics_fail_closed() { - let mut invalid_nominal = initialized::(); - invalid_nominal.state.gyro_bias = AngularRateFrd::radians_per_second(f32::NAN, 0.0, 0.0); - assert!(!invalid_nominal.values_are_valid()); - - // The candidate update must be committed atomically. These entries - // are unobserved by the range/flow Jacobians, so gain construction and - // the update itself succeed while final covariance validation rejects - // the private candidate. - let mut range_candidate = initialized::(); - range_candidate.covariance.set(0, 0, -1.0); - let range_state = range_candidate.state; - let range_covariance = range_candidate.covariance; - assert_eq!( - range_candidate.fuse_range(range(), range_extrinsics()), - Err(FusionError::FilterInvalid) - ); - assert_eq!(range_candidate.state, range_state); - assert_eq!(range_candidate.covariance.values, range_covariance.values); - assert!(range_candidate.requires_reinitialization()); - - let mut flow_candidate = initialized::(); - flow_candidate.covariance.set(0, 0, -1.0); - let flow_state = flow_candidate.state; - let flow_covariance = flow_candidate.covariance; - assert_eq!( - flow_candidate.fuse_optical_flow(flow(), flow_extrinsics()), - Err(FusionError::FilterInvalid) - ); - assert_eq!(flow_candidate.state, flow_state); - assert_eq!(flow_candidate.covariance.values, flow_covariance.values); - assert!(flow_candidate.requires_reinitialization()); - - let mut invalid_extrinsics = flow_extrinsics(); - invalid_extrinsics.position_body = crate::PositionFrd::metres(f32::NAN, 0.0, 0.0); - assert_eq!( - initialized::().fuse_optical_flow(flow(), invalid_extrinsics), - Err(FusionError::UnsupportedOpticalFlowLeverArm) - ); - } - - #[test] - fn stationary_initializer_internal_numeric_faults_fail_closed() { - let complete = || { - let mut initializer = StationaryInitializer::<1>::new(); - initializer.count = 1; - initializer.source = Some(SourceId::new(1)); - initializer.calibration = Some(CalibrationRevision::new(1)); - initializer.last_time = Some(Timestamp::from_microseconds(10)); - initializer.last_sequence = Some(ObservationSequence::new(1)); - initializer - }; - let config = EstimatorConfig::default(); - let integrity = IntegrityConfig::default(); - - for mutator in [ - |initializer: &mut StationaryInitializer<2>| { - initializer.gyro_sum = Vector3::new(f32::NAN, 0.0, 0.0) - }, - |initializer: &mut StationaryInitializer<2>| { - initializer.acceleration_sum = Vector3::new(f32::NAN, 0.0, 0.0) - }, - |initializer: &mut StationaryInitializer<2>| initializer.gyro_squared_sum = f32::NAN, - |initializer: &mut StationaryInitializer<2>| { - initializer.acceleration_squared_sum = f32::NAN - }, - ] { - let mut initializer = StationaryInitializer::<2>::new(); - mutator(&mut initializer); - assert!(!initializer.push(imu(10, [0, 0, -1_000], [0; 3]))); - assert!(!initializer.is_complete()); - } - - let mut nonfinite_gyro_square = complete(); - nonfinite_gyro_square.gyro_squared_sum = f32::NAN; - assert!( - nonfinite_gyro_square - .finish::(Timestamp::from_microseconds(10), config, integrity, 0.0) - .is_none() - ); - - let mut nonfinite_acceleration_square = complete(); - nonfinite_acceleration_square.acceleration_squared_sum = f32::NAN; - assert!( - nonfinite_acceleration_square - .finish::(Timestamp::from_microseconds(10), config, integrity, 0.0) - .is_none() - ); - - let mut nonfinite_residual = complete(); - nonfinite_residual.acceleration_sum = Vector3::new(0.0, 0.0, f32::MAX); - assert!( - nonfinite_residual - .finish::( - Timestamp::from_microseconds(10), - EstimatorConfig { - gravity_mps2: f32::MAX, - ..config - }, - integrity, - 0.0, - ) - .is_none() - ); - - let mut nonfinite_gyro_variance = complete(); - nonfinite_gyro_variance.gyro_sum = Vector3::new(f32::MAX, 0.0, 0.0); - assert!( - nonfinite_gyro_variance - .finish::(Timestamp::from_microseconds(10), config, integrity, 0.0) - .is_none() - ); - - let mut nonfinite_acceleration_variance = complete(); - nonfinite_acceleration_variance.acceleration_sum = Vector3::new(f32::MAX, 0.0, 0.0); - assert!( - nonfinite_acceleration_variance - .finish::(Timestamp::from_microseconds(10), config, integrity, 0.0) - .is_none() - ); - - let mut residual_limit = complete(); - residual_limit.acceleration_sum = Vector3::new(1.0, 0.0, -GRAVITY_MPS2); - assert!( - residual_limit - .finish::(Timestamp::from_microseconds(10), config, integrity, 0.0) - .is_none() - ); - - let mut gyro_variance_limit = complete(); - gyro_variance_limit.acceleration_sum = Vector3::new(0.0, 0.0, -GRAVITY_MPS2); - gyro_variance_limit.acceleration_squared_sum = GRAVITY_MPS2 * GRAVITY_MPS2; - gyro_variance_limit.gyro_squared_sum = 0.01; - assert!( - gyro_variance_limit - .finish::(Timestamp::from_microseconds(10), config, integrity, 0.0) - .is_none() - ); - - let mut acceleration_variance_limit = complete(); - acceleration_variance_limit.acceleration_sum = Vector3::new(0.0, 0.0, -GRAVITY_MPS2); - acceleration_variance_limit.acceleration_squared_sum = GRAVITY_MPS2 * GRAVITY_MPS2 + 1.0; - assert!( - acceleration_variance_limit - .finish::(Timestamp::from_microseconds(10), config, integrity, 0.0) - .is_none() - ); - } - - #[test] - fn range_numerical_boundary_faults_fail_closed() { - let mut invalid_lever = range_extrinsics(); - invalid_lever.position_body = crate::PositionFrd::metres(f32::NAN, 0.0, 0.0); - assert_eq!( - initialized::().fuse_range(range(), invalid_lever), - Err(FusionError::InvalidRangeGeometry) - ); - - let mut nonfinite_innovation = initialized::(); - nonfinite_innovation.config.minimum_range_metres = -f32::MAX; - nonfinite_innovation.config.maximum_range_metres = f32::MAX; - nonfinite_innovation.state.terrain_down_metres = f32::MAX; - let mut negative_maximum_range = range(); - negative_maximum_range.metres = -f32::MAX; - assert_eq!( - nonfinite_innovation.fuse_range(negative_maximum_range, range_extrinsics()), - Err(FusionError::SingularInnovation) - ); - - let mut nonfinite_jacobian = initialized::(); - let near_horizontal = - UnitQuaternion::::from_components(0.707_142_1, 0.0, 0.707_071_4, 0.0) - .unwrap(); - nonfinite_jacobian.state.attitude = nonfinite_jacobian - .state - .attitude - .compose(near_horizontal) - .unwrap(); - nonfinite_jacobian.state.terrain_down_metres = 1.0e34; - nonfinite_jacobian.config.minimum_range_down_cosine = 1.0e-5; - nonfinite_jacobian.config.maximum_range_metres = f32::MAX; - assert_eq!( - nonfinite_jacobian.fuse_range(range(), range_extrinsics()), - Err(FusionError::SingularInnovation) - ); - } - - #[test] - fn finite_flow_innovation_covariance_overflow_is_rejected() { - let mut filter = initialized::(); - filter.config.gyro_noise_variance = 1.0e31; - let mut high_acceleration = imu(11, [0, 0, -1_000], [0; 3]); - high_acceleration.acceleration = - AccelerationFrd::metres_per_second_squared(1.0e13, 0.0, -GRAVITY_MPS2); - assert!(filter.propagate(high_acceleration).is_ok()); - assert!(filter.covariance_is_valid()); - let (_, h) = filter.co_located_flow_model(flow_extrinsics()); - assert!(filter.two_gain(h, flow().covariance).is_none()); - - let before_state = filter.state; - let before_covariance = filter.covariance.values; - let mut observation = flow(); - observation.context = context(11, FrameId::new(8), 3, 255); - assert_eq!( - filter.fuse_optical_flow(observation, flow_extrinsics()), - Err(FusionError::SingularInnovation) - ); - assert_eq!(filter.state, before_state); - assert_eq!(filter.covariance.values, before_covariance); - } - - #[test] - fn local_faults_fail_closed() { - let mut local_timestamp = local(); - local_timestamp.context.measurement_time = Timestamp::from_microseconds(11); - assert_eq!( - initialized::<15>().fuse_local_position_velocity( - local_timestamp, - SourceId::new(5), - CalibrationRevision::new(1) - ), - Err(FusionError::TimestampMismatch) - ); - let mut local_capacity = initialized::<15>(); - for source in 1..=4 { - assert!(local_capacity.integrity.reserve(SourceId::new(source))); - } - assert_eq!( - local_capacity.fuse_local_position_velocity( - local(), - SourceId::new(5), - CalibrationRevision::new(1) - ), - Err(FusionError::SourceCapacityExhausted) - ); - let mut local_replay = initialized::<15>(); - assert_eq!( - local_replay.fuse_local_position_velocity( - local(), - SourceId::new(5), - CalibrationRevision::new(1) - ), - Ok(FusionResult::Fused) - ); - assert_eq!( - local_replay.fuse_local_position_velocity( - local(), - SourceId::new(5), - CalibrationRevision::new(1) - ), - Err(FusionError::ReplayedObservation) - ); - let mut local_singular = initialized::<15>(); - local_singular.covariance.set(POSITION, POSITION, -0.1); - assert_eq!( - local_singular.fuse_local_position_velocity( - local(), - SourceId::new(5), - CalibrationRevision::new(1) - ), - Err(FusionError::SingularInnovation) - ); - let mut infinite_local_nis = initialized::<15>(); - infinite_local_nis.config.maximum_local_position_metres = f32::MAX; - let mut enormous_local = local(); - enormous_local.position = PositionNed::metres(f32::MAX, 0.0, 0.0); - assert_eq!( - infinite_local_nis.fuse_local_position_velocity( - enormous_local, - SourceId::new(5), - CalibrationRevision::new(1) - ), - Err(FusionError::InnovationRejected) - ); - let mut local_isolated = initialized::<15>(); - assert!(local_isolated.integrity.reserve(SourceId::new(5))); - local_isolated - .integrity - .entry_mut(SourceId::new(5)) - .expect("test reserved source") - .isolated = true; - assert_eq!( - local_isolated.fuse_local_position_velocity( - local(), - SourceId::new(5), - CalibrationRevision::new(1) - ), - Ok(FusionResult::SourceIsolated) - ); - let mut failed_local = initialized::<15>(); - failed_local.failed = true; - assert_eq!( - failed_local.fuse_local_position_velocity( - local(), - SourceId::new(5), - CalibrationRevision::new(1) - ), - Err(FusionError::FilterInvalid) - ); - let mut invalid_local_candidate = initialized::<15>(); - invalid_local_candidate.state.gyro_bias = - AngularRateFrd::radians_per_second(f32::NAN, 0.0, 0.0); - assert_eq!( - invalid_local_candidate.fuse_local_position_velocity( - local(), - SourceId::new(5), - CalibrationRevision::new(1) - ), - Err(FusionError::FilterInvalid) - ); - } - - #[test] - fn numeric_helpers_fail_closed() { - let mut asymmetric = initialized::<15>(); - asymmetric.covariance.set(0, 1, 0.5); - assert!(!asymmetric.covariance_is_valid()); - let mut overflow_propagation = initialized::<15>(); - overflow_propagation - .config - .maximum_imu_interval_microseconds = u64::MAX; - overflow_propagation.state.velocity = VelocityNed::metres_per_second(f32::MAX, 0.0, 0.0); - let mut overflow_imu = imu(u64::MAX - 1, [0, 0, -1_000], [0; 3]); - overflow_imu.context.sequence = ObservationSequence::new(11); - assert_eq!( - overflow_propagation.propagate(overflow_imu), - Err(PropagationError::FilterInvalid) - ); - let mut nonfinite_rotation = initialized::<15>(); - nonfinite_rotation.config.maximum_imu_interval_microseconds = u64::MAX; - let mut extreme_rate = imu(u64::MAX - 1, [0, 0, -1_000], [0; 3]); - extreme_rate.context.sequence = ObservationSequence::new(11); - extreme_rate.angular_rate = AngularRateFrd::radians_per_second(f32::MAX, 0.0, 0.0); - assert_eq!( - nonfinite_rotation.propagate(extreme_rate), - Err(PropagationError::NonFinite) - ); - let mut nonfinite_acceleration_delta = initialized::<15>(); - nonfinite_acceleration_delta.config.gravity_mps2 = f32::MAX; - let mut large_acceleration = imu(11, [0, 0, -1_000], [0; 3]); - large_acceleration.acceleration = - AccelerationFrd::metres_per_second_squared(f32::MAX, 0.0, f32::MAX); - assert!( - nonfinite_acceleration_delta - .propagate_delta(large_acceleration, 0.001) - .is_err() - ); - let mut nonfinite_velocity_delta = initialized::<15>(); - nonfinite_velocity_delta.state.position = PositionNed::metres(-f32::MAX, 0.0, 0.0); - let mut velocity_overflow_imu = imu(11, [0, 0, -1_000], [0; 3]); - velocity_overflow_imu.acceleration = - AccelerationFrd::metres_per_second_squared(f32::MAX, 0.0, -GRAVITY_MPS2); - assert!( - nonfinite_velocity_delta - .propagate_delta(velocity_overflow_imu, 1.1) - .is_err() - ); - - let mut gain_filter = initialized::<15>(); - let scalar = ScalarGain { - h: [0.0; MAX_DIM], - gain: [0.0; MAX_DIM], - innovation_variance: 1.0, - }; - assert!( - gain_filter - .scalar_gain([0.0; MAX_DIM], ScalarVariance::new(0.1).unwrap()) - .is_some() - ); - gain_filter.covariance.set(0, 0, -0.1); - let mut scalar_h = [0.0; MAX_DIM]; - scalar_h[0] = 1.0; - assert!( - gain_filter - .scalar_gain(scalar_h, ScalarVariance::new(0.1).unwrap()) - .is_none() - ); - let mut nonfinite_gain_filter = initialized::<15>(); - nonfinite_gain_filter.covariance.set(0, 0, f32::NAN); - assert!( - nonfinite_gain_filter - .scalar_gain(scalar_h, ScalarVariance::new(0.1).unwrap()) - .is_none() - ); - assert!(gain_filter.apply_scalar(scalar, f32::NAN, 0.1).is_err()); - assert!(gain_filter.apply_scalar(scalar, 0.0, f32::NAN).is_err()); - assert!(gain_filter.apply_scalar(scalar, 0.0, 0.0).is_err()); - let overflowing_scalar = ScalarGain { - h: [0.0; MAX_DIM], - gain: [f32::MAX; MAX_DIM], - innovation_variance: 1.0, - }; - assert!( - gain_filter - .apply_scalar(overflowing_scalar, f32::MAX, 0.1) - .is_err() - ); - let two = TwoGain { - h: [[0.0; MAX_DIM]; 2], - gain: [[0.0; 2]; MAX_DIM], - inverse_s: [1.0, 0.0, 0.0, 1.0], - }; - assert!( - gain_filter - .apply_two(two, [f32::NAN, 0.0], flow().covariance) - .is_err() - ); - let overflowing_two = TwoGain { - h: [[0.0; MAX_DIM]; 2], - gain: [[f32::MAX; 2]; MAX_DIM], - inverse_s: [1.0, 0.0, 0.0, 1.0], - }; - assert!( - gain_filter - .apply_two(overflowing_two, [f32::MAX, f32::MAX], flow().covariance) - .is_err() - ); - let mut bad_two_gain = initialized::<15>(); - let mut two_h = [[0.0; MAX_DIM]; 2]; - two_h[0][VELOCITY] = 1.0; - two_h[1][VELOCITY + 1] = 1.0; - assert!( - bad_two_gain - .two_gain( - two_h, - Covariance2 { - xx: ScalarVariance::new(0.1).unwrap(), - xy: f32::NAN, - yy: ScalarVariance::new(0.1).unwrap() - } - ) - .is_none() - ); - bad_two_gain.covariance.set(VELOCITY, VELOCITY, f32::MAX); - assert!(bad_two_gain.two_gain(two_h, flow().covariance).is_none()); - - let mut nonfinite_s00 = initialized::<15>(); - nonfinite_s00.covariance.set(VELOCITY, VELOCITY, f32::NAN); - assert!(nonfinite_s00.two_gain(two_h, flow().covariance).is_none()); - - let mut nonfinite_s11 = initialized::<15>(); - nonfinite_s11 - .covariance - .set(VELOCITY + 1, VELOCITY + 1, f32::NAN); - assert!(nonfinite_s11.two_gain(two_h, flow().covariance).is_none()); - - let mut nonpositive_s11 = initialized::<15>(); - nonpositive_s11 - .covariance - .set(VELOCITY + 1, VELOCITY + 1, -flow().covariance.yy.get()); - assert!(nonpositive_s11.two_gain(two_h, flow().covariance).is_none()); - - let mut singular_innovation_covariance = initialized::<15>(); - singular_innovation_covariance - .covariance - .set(VELOCITY, VELOCITY, 0.0); - singular_innovation_covariance - .covariance - .set(VELOCITY + 1, VELOCITY + 1, 0.0); - assert!( - singular_innovation_covariance - .two_gain( - two_h, - Covariance2 { - xx: ScalarVariance::new(0.1).unwrap(), - xy: 0.1, - yy: ScalarVariance::new(0.1).unwrap(), - }, - ) - .is_none() - ); - let mut error = [0.0; MAX_DIM]; - error[0] = f32::NAN; - assert!(gain_filter.inject(error).is_err()); - let mut position_overflow = initialized::<15>(); - position_overflow.state.position = PositionNed::metres(f32::MAX, 0.0, 0.0); - let mut error = [0.0; MAX_DIM]; - error[0] = f32::MAX; - assert!(position_overflow.inject(error).is_err()); - let mut velocity_overflow = initialized::<15>(); - velocity_overflow.state.velocity = VelocityNed::metres_per_second(f32::MAX, 0.0, 0.0); - let mut error = [0.0; MAX_DIM]; - error[3] = f32::MAX; - assert!(velocity_overflow.inject(error).is_err()); - let mut gyro_overflow = initialized::<15>(); - gyro_overflow.state.gyro_bias = AngularRateFrd::radians_per_second(f32::MAX, 0.0, 0.0); - let mut error = [0.0; MAX_DIM]; - error[9] = f32::MAX; - assert!(gyro_overflow.inject(error).is_err()); - let mut acceleration_overflow = initialized::<15>(); - acceleration_overflow.state.acceleration_bias = - AccelerationFrd::metres_per_second_squared(f32::MAX, 0.0, 0.0); - let mut error = [0.0; MAX_DIM]; - error[12] = f32::MAX; - assert!(acceleration_overflow.inject(error).is_err()); - let mut terrain_overflow = initialized::<16>(); - terrain_overflow.state.terrain_down_metres = f32::MAX; - let mut error = [0.0; MAX_DIM]; - error[TERRAIN] = f32::MAX; - assert!(terrain_overflow.inject(error).is_err()); - } - - #[test] - fn defensive_estimator_paths_are_explicitly_fail_closed() { - let source = SourceId::new(9); - let mut integrity = SourceIntegrity::new(IntegrityConfig::default()); - assert!(!integrity.accepts_sequence(source, ObservationSequence::new(1))); - assert!(!integrity.accept(source, Timestamp::from_microseconds(1))); - - let mut initializer = StationaryInitializer::<1>::new(); - let mut overflowing_sample = imu(1, [0, 0, -1_000], [0; 3]); - overflowing_sample.acceleration = - AccelerationFrd::metres_per_second_squared(f32::MAX, 0.0, 0.0); - assert!(!initializer.push(overflowing_sample)); - assert!(!initializer.is_complete()); - - // A finite covariance can still overflow while constructing its - // Cholesky factor. It must be rejected rather than repaired. - let mut covariance = Covariance::diagonal(); - covariance.set(0, 0, 1.0); - covariance.set(1, 0, f32::MAX); - covariance.set(0, 1, f32::MAX); - covariance.set(1, 1, 1.0); - assert!(!covariance.positive_definite(2)); - - let mut terrain = initialized::(); - terrain.covariance.set(TERRAIN, TERRAIN, 1.0); - let before = terrain.covariance.get(TERRAIN, TERRAIN); - terrain - .propagate(imu(10_010, [0, 0, -1_000], [0; 3])) - .unwrap(); - assert!(terrain.covariance.get(TERRAIN, TERRAIN) > before); - - // A candidate whose unobserved terrain covariance is invalid must not - // be committed by an otherwise valid six-axis local-state update. - let mut invalid_candidate = initialized::(); - invalid_candidate.covariance.set(TERRAIN, TERRAIN, -1.0); - assert_eq!( - invalid_candidate.fuse_local_position_velocity( - local(), - SourceId::new(5), - CalibrationRevision::new(1), - ), - Err(FusionError::FilterInvalid) - ); - assert!(invalid_candidate.requires_reinitialization()); - } - - #[test] - fn optical_flow_numerical_model_guards_reject_before_fusion() { - let mut nonfinite_prediction = initialized::(); - let yaw_delta = - UnitQuaternion::::from_small_rotation_vector(Vector3::new(0.0, 0.0, 0.785)) - .unwrap(); - nonfinite_prediction.state.attitude = nonfinite_prediction - .state - .attitude - .compose(yaw_delta) - .unwrap(); - nonfinite_prediction.state.velocity = - VelocityNed::metres_per_second(f32::MAX, f32::MAX, 0.0); - nonfinite_prediction - .config - .maximum_flow_speed_metres_per_second = f32::MAX; - assert_eq!( - nonfinite_prediction.fuse_optical_flow(flow(), flow_extrinsics()), - Err(FusionError::Implausible) - ); - - // The measurement only observes the sensor x/y axes. This finite - // velocity projects into the unobserved sensor z axis without an - // error, while its attitude Jacobian overflows a measured component. - let mut nonfinite_jacobian = initialized::(); - nonfinite_jacobian.state.velocity = - VelocityNed::metres_per_second(f32::MAX, 0.0, -f32::MAX); - nonfinite_jacobian - .config - .maximum_flow_speed_metres_per_second = f32::MAX; - let mut rotated_extrinsics = flow_extrinsics(); - rotated_extrinsics.sensor_to_body = - UnitQuaternion::from_components(1.0, 0.0, -0.5, 0.0).unwrap(); - assert_eq!( - nonfinite_jacobian.fuse_optical_flow(flow(), rotated_extrinsics), - Err(FusionError::SingularInnovation) - ); - } - - #[test] - fn initializer_and_constructor_rejection_matrix_is_atomic() { - let state = initialized::().state; - assert_eq!(state.terrain_down_metres::(), None); - assert_eq!(state.terrain_down_metres::(), Some(2.0)); - assert!(state_is_valid::(state)); - assert!(state_is_valid::(state)); - - let mut nonfinite_position = state; - nonfinite_position.position = PositionNed::metres(f32::NAN, 0.0, 0.0); - assert!(!state_is_valid::(nonfinite_position)); - let mut nonfinite_velocity = state; - nonfinite_velocity.velocity = VelocityNed::metres_per_second(f32::NAN, 0.0, 0.0); - assert!(!state_is_valid::(nonfinite_velocity)); - let mut nonfinite_gyro = state; - nonfinite_gyro.gyro_bias = AngularRateFrd::radians_per_second(f32::NAN, 0.0, 0.0); - assert!(!state_is_valid::(nonfinite_gyro)); - let mut nonfinite_acceleration = state; - nonfinite_acceleration.acceleration_bias = - AccelerationFrd::metres_per_second_squared(f32::NAN, 0.0, 0.0); - assert!(!state_is_valid::(nonfinite_acceleration)); - let mut nonfinite_terrain = state; - nonfinite_terrain.terrain_down_metres = f32::NAN; - assert!(!state_is_valid::(nonfinite_terrain)); - - let mut full = StationaryInitializer::<1>::new(); - assert!(full.push(imu(10, [0, 0, -1_000], [0; 3]))); - assert!(!full.push(imu(11, [0, 0, -1_000], [0; 3]))); - - for malformed in [ - { - let mut value = imu(10, [0, 0, -1_000], [0; 3]); - value.context.validity = Validity::Invalid; - value - }, - { - let mut value = imu(10, [0, 0, -1_000], [0; 3]); - value.context.frame = FrameId::new(9); - value - }, - { - let mut value = imu(10, [0, 0, -1_000], [0; 3]); - value.acceleration = AccelerationFrd::metres_per_second_squared(f32::NAN, 0.0, 0.0); - value - }, - { - let mut value = imu(10, [0, 0, -1_000], [0; 3]); - value.angular_rate = AngularRateFrd::radians_per_second(f32::NAN, 0.0, 0.0); - value - }, - ] { - let mut accumulator = StationaryInitializer::<1>::new(); - assert!(!accumulator.push(malformed)); - assert!(!accumulator.is_complete()); - } - - for mismatched in [ - { - let mut value = imu(11, [0, 0, -1_000], [0; 3]); - value.context.source = SourceId::new(9); - value - }, - { - let mut value = imu(11, [0, 0, -1_000], [0; 3]); - value.context.calibration = CalibrationRevision::new(9); - value - }, - imu(10, [0, 0, -1_000], [0; 3]), - { - let mut value = imu(11, [0, 0, -1_000], [0; 3]); - value.context.sequence = ObservationSequence::new(10); - value - }, - ] { - let mut accumulator = StationaryInitializer::<2>::new(); - assert!(accumulator.push(imu(10, [0, 0, -1_000], [0; 3]))); - assert!(!accumulator.push(mismatched)); - assert!(!accumulator.is_complete()); - } - - let mut numeric = StationaryInitializer::<2>::new(); - assert!(numeric.push(imu(10, [0, 0, -1_000], [0; 3]))); - numeric.gyro_squared_sum = f32::MAX; - let mut huge_rate = imu(11, [0, 0, -1_000], [0; 3]); - huge_rate.angular_rate = AngularRateFrd::radians_per_second(f32::MAX, 0.0, 0.0); - assert!(!numeric.push(huge_rate)); - assert!(!numeric.is_complete()); - - let complete = || { - let mut accumulator = StationaryInitializer::<1>::new(); - assert!(accumulator.push(imu(10, [0, 0, -1_000], [0; 3]))); - accumulator - }; - assert!( - StationaryInitializer::<0>::new() - .finish::( - Timestamp::from_microseconds(10), - EstimatorConfig::default(), - IntegrityConfig::default(), - 2.0, - ) - .is_none() - ); - assert!( - complete() - .finish::( - Timestamp::from_microseconds(11), - EstimatorConfig::default(), - IntegrityConfig::default(), - 2.0, - ) - .is_none() - ); - assert!( - complete() - .finish::( - Timestamp::from_microseconds(10), - EstimatorConfig { - gravity_mps2: 0.0, - ..EstimatorConfig::default() - }, - IntegrityConfig::default(), - 2.0, - ) - .is_none() - ); - assert!( - complete() - .finish::( - Timestamp::from_microseconds(10), - EstimatorConfig::default(), - IntegrityConfig::default(), - f32::NAN, - ) - .is_none() - ); - - let stream = imu_stream(); - assert!( - Eskf::<14>::new( - Timestamp::from_microseconds(10), - state, - EstimatorConfig::default(), - IntegrityConfig::default(), - stream, - ) - .is_err() - ); - assert!( - Eskf::::new( - Timestamp::from_microseconds(10), - nonfinite_position, - EstimatorConfig::default(), - IntegrityConfig::default(), - stream, - ) - .is_err() - ); - assert!( - Eskf::::new( - Timestamp::from_microseconds(10), - state, - EstimatorConfig::default(), - IntegrityConfig { - rejection_limit: 0, - ..IntegrityConfig::default() - }, - stream, - ) - .is_err() - ); - } -} diff --git a/firmware/libs/kaptan-navigation/src/filter.rs b/firmware/libs/kaptan-navigation/src/filter.rs new file mode 100644 index 0000000..4c9878e --- /dev/null +++ b/firmware/libs/kaptan-navigation/src/filter.rs @@ -0,0 +1,1140 @@ +//! Sixteen-state left-NED ESKF. + +use crate::{ + BarometricReference, FlightImuObservation, GRAVITY_MPS2, NavigationConfig, NavigationFailure, + NoiseDensities, +}; +use kaptan_math::{Frd, Ned, UnitQuaternion, Vector3, left_jacobian_apply, sqrt}; + +pub(crate) const N: usize = 16; +// Each discretization column represents its nonzero rows in one u16 mask. +const _: () = assert!(N <= u16::BITS as usize); +pub(crate) type Matrix = [[f32; N]; N]; +pub(crate) const ZERO: Matrix = [[0.0; N]; N]; + +pub(crate) fn identity() -> Matrix { + let mut a = ZERO; + for (i, row) in a.iter_mut().enumerate() { + row[i] = 1.0; + } + a +} +fn product(a: &Matrix, b: &Matrix) -> Matrix { + let mut out = ZERO; + for (i, arow) in a.iter().enumerate() { + for (k, brow) in b.iter().enumerate() { + if arow[k] != 0.0 { + for (output, value) in out[i].iter_mut().zip(brow) { + *output += arow[k] * value; + } + } + } + } + out +} +pub(crate) fn sandwich(a: &Matrix, p: &Matrix) -> Matrix { + let ap = product(a, p); + let mut out = ZERO; + if ap.iter().flatten().all(|value| value.is_finite()) { + // The transition/reset matrices are sparse. Visit each coefficient + // once while retaining ascending-k addition for every output element. + // Finite AP makes the omitted products exact zeros; a nonfinite AP + // must retain 0*Inf/NaN so numerical failure cannot be hidden. + for (j, row) in a.iter().enumerate() { + for (k, right) in row.iter().enumerate() { + if *right != 0.0 { + for (i, output) in out.iter_mut().enumerate() { + output[j] += ap[i][k] * right; + } + } + } + } + } else { + for i in 0..N { + for (j, row) in a.iter().enumerate() { + for (left, right) in ap[i].iter().zip(row) { + out[i][j] += left * right; + } + } + } + } + out +} +fn symmetric(p: &mut Matrix) { + for i in 0..N { + for j in 0..i { + let v = p[i][j] * 0.5 + p[j][i] * 0.5; + p[i][j] = v; + p[j][i] = v; + } + } +} +pub(crate) fn covariance_valid(p: &Matrix) -> bool { + let mut l = ZERO; + for i in 0..N { + for j in 0..=i { + if !p[i][j].is_finite() || !p[j][i].is_finite() || (p[i][j] - p[j][i]).abs() > 1e-4 { + return false; + } + let mut s = p[i][j]; + for k in 0..j { + s -= l[i][k] * l[j][k]; + } + if !s.is_finite() { + return false; + } + if i == j { + if s <= 0.0 { + return false; + } + l[i][j] = sqrt(s); + } else { + l[i][j] = s / l[j][j]; + if !l[i][j].is_finite() { + return false; + } + } + } + } + true +} +pub(crate) fn rotation(q: UnitQuaternion) -> [[f32; 3]; 3] { + let axes = [ + Vector3::new(1., 0., 0.), + Vector3::new(0., 1., 0.), + Vector3::new(0., 0., 1.), + ] + .map(|v| q.body_to_ned(v)); + core::array::from_fn(|i| core::array::from_fn(|j| components(axes[j])[i])) +} +pub(crate) fn components(v: Vector3) -> [f32; 3] { + [v.x(), v.y(), v.z()] +} +pub(crate) fn vector(v: [f32; 3]) -> Vector3 { + Vector3::new(v[0], v[1], v[2]) +} +pub(crate) fn skew(v: [f32; 3]) -> [[f32; 3]; 3] { + [[0., -v[2], v[1]], [v[2], 0., -v[0]], [-v[1], v[0], 0.]] +} + +/// Exact frozen-F discretization. F^4=0; isotropic independent drives make C +/// diagonal in either body or NED. No per-sample variance is added. +pub(crate) fn discretize( + r: [[f32; 3]; 3], + force: [f32; 3], + dt: f32, + noise: NoiseDensities, +) -> (Matrix, Matrix) { + let mut ft = ZERO; + let s = skew(force); + for i in 0..3 { + ft[i][i + 3] = dt; + for j in 0..3 { + ft[i + 3][j + 6] = -s[i][j] * dt; + ft[i + 3][j + 12] = -r[i][j] * dt; + ft[i + 6][j + 9] = -r[i][j] * dt; + } + } + let mut powers = [ZERO; 4]; + powers[0] = identity(); + powers[1] = ft; + for k in 2..4 { + powers[k] = product(&ft, &powers[k - 1]); + for row in &mut powers[k] { + for v in row { + *v /= k as f32; + } + } + } + let mut phi = ZERO; + for a in &powers { + for i in 0..N { + for j in 0..N { + phi[i][j] += a[i][j]; + } + } + } + // Only states 3..N have independent noise drives. Reuse the existing exact + // zero predicate once per immutable column instead of rescanning all rows + // for every pair of powers. Set-bit traversal keeps the original order. + let columns: [[u16; N - 3]; 4] = core::array::from_fn(|power| { + core::array::from_fn(|column| { + let mut mask = 0; + for (i, row) in powers[power].iter().enumerate() { + if row[column + 3] != 0.0 { + mask |= 1 << i; + } + } + mask + }) + }); + let mut q = ZERO; + for (a, pa) in powers.iter().enumerate() { + for (b, pb) in powers.iter().enumerate() { + for k in 3..N { + let density = match k { + 3..=5 => noise.acceleration, + 6..=8 => noise.gyro, + 9..=11 => noise.gyro_bias, + 12..=14 => noise.acceleration_bias, + _ => noise.barometric_datum, + }; + let weight = density * dt / (a + b + 1) as f32; + let mut rows_a = columns[a][k - 3]; + while rows_a != 0 { + let i = rows_a.trailing_zeros() as usize; + rows_a &= rows_a - 1; + let mut rows_b = columns[b][k - 3]; + while rows_b != 0 { + let j = rows_b.trailing_zeros() as usize; + rows_b &= rows_b - 1; + q[i][j] += pa[i][k] * weight * pb[j][k]; + } + } + } + } + } + (phi, q) +} + +#[derive(Clone, Copy)] +pub(crate) struct InputTiming { + pub start: u64, + pub end: u64, + pub force_jump: Vector3, + pub rate_jump: Vector3, +} + +#[derive(Clone, Copy)] +pub(crate) struct Filter { + pub time: u64, + pub p: Vector3, + pub v: Vector3, + pub q: UnitQuaternion, + pub bg: Vector3, + pub ba: Vector3, + pub datum: f32, + pub covariance: Matrix, + pub timing: crate::timing::TimingEnvelope, +} +impl Filter { + pub fn valid(&self) -> bool { + // UnitQuaternion has private storage and every safe constructor and + // composition normalizes or returns an error. There is no safe path + // for an unnormalized attitude into this state. + self.p.is_finite() + && self.v.is_finite() + && self.bg.is_finite() + && self.ba.is_finite() + && self.datum.is_finite() + && covariance_valid(&self.covariance) + && self.timing.valid() + } + #[cfg(test)] + pub fn propagate( + &mut self, + imu: FlightImuObservation, + to: u64, + config: NavigationConfig, + ) -> Result<(), NavigationFailure> { + self.propagate_timed(imu, to, config, core::iter::empty()) + } + pub fn propagate_timed( + &mut self, + imu: FlightImuObservation, + to: u64, + config: NavigationConfig, + timing: impl Iterator, + ) -> Result<(), NavigationFailure> { + let us = to.checked_sub(self.time).ok_or(NavigationFailure::ImuGap)?; + if us == 0 { + return Ok(()); + } + if us > config.maximum_imu_interval_microseconds { + return Err(NavigationFailure::ImuGap); + } + let dt = us as f32 * 1e-6; + let omega = imu.angular_rate.vector().sub(self.bg); + let force = imu.acceleration.vector().sub(self.ba); + let half_rotation = UnitQuaternion::from_small_rotation_vector(omega.scale(dt * 0.5))?; + let mid = self.q.compose(half_rotation)?; + let f = mid.body_to_ned(force); + let a = f.add(Vector3::new(0., 0., GRAVITY_MPS2)); + let (phi, qd) = discretize(rotation(mid), components(f), dt, config.noise); + let mut next = *self; + next.timing.transform(&phi)?; + for window in timing { + // The shifted reconstruction boundary can affect a segment before + // the nominal switch. Integrate its possible support, not an impulse + // delayed until the new average is selected. + let length = (window.end - window.start) as f32 * 1e-6; + let young = (to - window.end) as f32 * 1e-6; + let old = (to - window.start) as f32 * 1e-6; + let moments = [ + length, + length * (old + young) * 0.5, + length * (old * old + old * young + young * young) / 6.0, + ]; + let dv = mid.body_to_ned(window.force_jump); + let da = mid.body_to_ned(window.rate_jump); + let coupling = f.cross(da).scale(-1.0); + // For input direction d=[0,dv,da,0,0,0], F^3*d=0. Each + // signed time moment is bounded by the integral over the support. + // These are correlated vector generators, combined as bounded sets, + // never independent Gaussian noise or additional filter states. + let mut direct = [0.0; N]; + let mut first = [0.0; N]; + let mut second = [0.0; N]; + for i in 0..3 { + direct[i + 3] = components(dv)[i] * moments[0]; + direct[i + 6] = components(da)[i] * moments[0]; + first[i] = components(dv)[i] * moments[1]; + first[i + 3] = components(coupling)[i] * moments[1]; + second[i] = components(coupling)[i] * moments[2]; + } + for generator in [direct, first, second] { + next.timing.add_generator(generator)?; + } + } + next.p = self.p.add(self.v.scale(dt)).add(a.scale(0.5 * dt * dt)); + next.v = self.v.add(a.scale(dt)); + let full_rotation = UnitQuaternion::from_small_rotation_vector(omega.scale(dt))?; + next.q = self.q.compose(full_rotation)?; + next.covariance = sandwich(&phi, &self.covariance); + for (row, noise) in next.covariance.iter_mut().zip(qd) { + for (value, addition) in row.iter_mut().zip(noise) { + *value += addition; + } + } + symmetric(&mut next.covariance); + next.time = to; + if !next.valid() { + return Err(NavigationFailure::Numerical); + } + *self = next; + Ok(()) + } + /// False means innovation rejection. Numerical failure and live state are + /// distinct: callers apply all axes to one candidate before record commit. + #[cfg(test)] + pub fn scalar( + &mut self, + h: [f32; N], + residual: f32, + variance: f32, + ) -> Result { + self.scalar_timed(h, residual, variance, 0.0, 0.0, &mut [0.0; N]) + } + pub fn scalar_timed( + &mut self, + h: [f32; N], + residual: f32, + variance: f32, + clock_effect: f32, + remainder: f32, + generator: &mut [f32; N], + ) -> Result { + if !residual.is_finite() || !variance.is_finite() || variance <= 0.0 { + return Err(NavigationFailure::Numerical); + } + let mut ph = [0.; N]; + for (value, row) in ph.iter_mut().zip(&self.covariance) { + for (p, hj) in row.iter().zip(h) { + *value += p * hj; + } + } + let mut s = variance; + for i in 0..N { + s += h[i] * ph[i]; + } + if !s.is_finite() || s <= 0.0 { + return Err(NavigationFailure::Numerical); + } + if !clock_effect.is_finite() || !remainder.is_finite() || remainder < 0.0 { + return Err(NavigationFailure::Numerical); + } + let correlated: f32 = h.iter().zip(generator.iter()).map(|(a, b)| a * b).sum(); + let timing_innovation = + self.timing.projected(&h)? + (clock_effect - correlated).abs() + remainder; + if !timing_innovation.is_finite() { + return Err(NavigationFailure::Numerical); + } + let gated_residual = (residual.abs() - timing_innovation).max(0.0); + let nis = gated_residual * gated_residual / s; + if !nis.is_finite() || nis > 16.0 { + return Ok(false); + } + let k = ph.map(|v| v / s); + let e = k.map(|v| v * residual); + let mut a = identity(); + for (row, ki) in a.iter_mut().zip(k) { + for (value, hj) in row.iter_mut().zip(h) { + *value -= ki * hj; + } + } + let mut p = sandwich(&a, &self.covariance); + for i in 0..N { + for j in 0..N { + p[i][j] += k[i] * variance * k[j]; + } + } + let mut next = *self; + next.timing.transform(&a)?; + next.timing + .add_generator(k.map(|value| value * remainder))?; + let mut next_generator = [0.0; N]; + for i in 0..N { + for (coefficient, error) in a[i].iter().zip(generator.iter()) { + next_generator[i] += coefficient * error; + } + next_generator[i] += k[i] * clock_effect; + } + next.p = next.p.add(vector([e[0], e[1], e[2]])); + next.v = next.v.add(vector([e[3], e[4], e[5]])); + let angle = Vector3::::new(e[6], e[7], e[8]); + next.q = + UnitQuaternion::from_small_rotation_vector(angle).and_then(|dq| dq.compose(next.q))?; + next.bg = next.bg.add(vector([e[9], e[10], e[11]])); + next.ba = next.ba.add(vector([e[12], e[13], e[14]])); + next.datum += e[15]; + let mut reset = identity(); + for j in 0..3 { + let mut basis = [0.; 3]; + basis[j] = 1.; + let column = components(left_jacobian_apply(angle, vector(basis))?); + for i in 0..3 { + reset[i + 6][j + 6] = column[i]; + } + } + next.covariance = sandwich(&reset, &p); + next.timing.transform(&reset)?; + let mut reset_generator = [0.0; N]; + for i in 0..N { + for (coefficient, error) in reset[i].iter().zip(next_generator) { + reset_generator[i] += coefficient * error; + } + } + if !reset_generator.iter().all(|v| v.is_finite()) { + return Err(NavigationFailure::Numerical); + } + symmetric(&mut next.covariance); + if !next.valid() { + return Err(NavigationFailure::Numerical); + } + *self = next; + *generator = reset_generator; + Ok(true) + } +} + +/// Requires a validated reference. Five terms bound log error for ratio 0.95..1.05. +pub(crate) fn pressure_height(p: f32, reference: BarometricReference) -> Option<(f32, f32)> { + if !p.is_finite() + || !(0.95 * reference.pressure_pascals..=1.05 * reference.pressure_pascals).contains(&p) + { + return None; + } + let h = reference.height_scale(); + let x = (reference.pressure_pascals - p) / (reference.pressure_pascals + p); + let xx = x * x; + let mut term = x; + let mut sum = 0.; + for k in 0..5 { + sum += term / (2 * k + 1) as f32; + term *= xx; + } + Some((2. * h * sum, h / p)) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn sparse_sandwich_preserves_dense_accumulation_and_nonfinite_failures() { + // Retain the former dense second multiplication as a bit-level + // regression oracle. The independent analytic/semigroup/enclosure + // tests below still establish the numerical model separately. + fn dense(a: &Matrix, p: &Matrix) -> Matrix { + let ap = product(a, p); + core::array::from_fn(|i| { + core::array::from_fn(|j| (0..N).fold(0.0, |sum, k| sum + ap[i][k] * a[j][k])) + }) + } + fn compare(a: &Matrix, p: &Matrix) { + let actual = sandwich(a, p); + let expected = dense(a, p); + for (value, reference) in actual.iter().flatten().zip(expected.iter().flatten()) { + if reference.is_nan() { + assert!(value.is_nan()); + } else { + assert_eq!(value.to_bits(), reference.to_bits()); + } + } + } + for scale in [ + 0.0, + -0.0, + f32::from_bits(1), + f32::MIN_POSITIVE, + 1e-20, + 0.25, + 1e20, + f32::MAX, + ] { + for stride in [1, 2, 5, 17] { + let a = core::array::from_fn(|i| { + core::array::from_fn(|j| { + if (i * N + j).is_multiple_of(stride) { + ((i * 7 + j * 3) as f32 - 47.0) * 0.03125 + } else { + -0.0 + } + }) + }); + let p = core::array::from_fn(|i| { + core::array::from_fn(|j| if i == j { scale } else { -scale * 0.125 }) + }); + compare(&a, &p); + } + } + for bad in [f32::NAN, f32::INFINITY, f32::NEG_INFINITY, f32::MAX] { + let mut a = identity(); + a[0][0] = 2.0; + let mut p = identity(); + p[0][0] = bad; + compare(&a, &p); + // Nonfinite AP times a zero coefficient must remain NaN. Dropping + // this contribution would hide the old numerical failure path. + assert!(sandwich(&a, &p)[0][1].is_nan()); + } + let mut a = identity(); + a[0][0] = 2.0; + let mut p = identity(); + p[0][0] = f32::MAX * 0.5; + assert!(product(&a, &p).iter().flatten().all(|v| v.is_finite())); + compare(&a, &p); // Only the second multiplication overflows. + assert_eq!(sandwich(&a, &p)[0][0], f32::INFINITY); + assert_eq!(sandwich(&a, &p)[0][1].to_bits(), 0.0_f32.to_bits()); + } + + #[test] + fn column_masks_preserve_the_former_noise_integral_order() { + // This reference retains the former scan order. It is a compatibility + // check, distinct from the independent f64 integral/semigroup tests. + fn former_discretize( + r: [[f32; 3]; 3], + force: [f32; 3], + dt: f32, + noise: NoiseDensities, + ) -> (Matrix, Matrix) { + let mut ft = ZERO; + let s = skew(force); + for i in 0..3 { + ft[i][i + 3] = dt; + for j in 0..3 { + ft[i + 3][j + 6] = -s[i][j] * dt; + ft[i + 3][j + 12] = -r[i][j] * dt; + ft[i + 6][j + 9] = -r[i][j] * dt; + } + } + let mut powers = [ZERO; 4]; + powers[0] = identity(); + powers[1] = ft; + for k in 2..4 { + powers[k] = product(&ft, &powers[k - 1]); + for row in &mut powers[k] { + for v in row { + *v /= k as f32; + } + } + } + let mut phi = ZERO; + for a in &powers { + for i in 0..N { + for j in 0..N { + phi[i][j] += a[i][j]; + } + } + } + let mut q = ZERO; + for (a, pa) in powers.iter().enumerate() { + for (b, pb) in powers.iter().enumerate() { + for k in 3..N { + let density = match k { + 3..=5 => noise.acceleration, + 6..=8 => noise.gyro, + 9..=11 => noise.gyro_bias, + 12..=14 => noise.acceleration_bias, + _ => noise.barometric_datum, + }; + let weight = density * dt / (a + b + 1) as f32; + for i in 0..N { + if pa[i][k] != 0.0 { + for (j, row) in pb.iter().enumerate() { + if row[k] != 0.0 { + q[i][j] += pa[i][k] * weight * row[k]; + } + } + } + } + } + } + } + (phi, q) + } + + let zero = NoiseDensities { + gyro: 0.0, + acceleration: 0.0, + gyro_bias: 0.0, + acceleration_bias: 0.0, + barometric_datum: 0.0, + }; + let rotations = [ + [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]], + [[0., -1., 0.], [1., 0., -0.], [0., 0., 1.]], + [[0.8, 0., 0.6], [0., 1., 0.], [-0.6, 0., 0.8]], + ]; + let forces = [ + [0.; 3], + [0., 0., -GRAVITY_MPS2], + [2., -1., -9.], + [f32::MAX, f32::NAN, f32::INFINITY], + ]; + let densities = [ + zero, + noise(), + NoiseDensities { + gyro: f32::from_bits(1), + ..zero + }, + NoiseDensities { + acceleration_bias: f32::MAX, + ..zero + }, + NoiseDensities { + barometric_datum: f32::NAN, + ..zero + }, + NoiseDensities { + acceleration: f32::INFINITY, + ..zero + }, + ]; + for r in rotations { + for force in forces { + for dt in [f32::from_bits(1), 1e-6, 0.007, 0.020, f32::MAX, f32::NAN] { + for density in densities { + let (actual_phi, actual_q) = discretize(r, force, dt, density); + let (expected_phi, expected_q) = former_discretize(r, force, dt, density); + for (actual, expected) in + [(actual_phi, expected_phi), (actual_q, expected_q)] + { + for (value, reference) in + actual.iter().flatten().zip(expected.iter().flatten()) + { + if reference.is_nan() { + assert!(value.is_nan()); + } else { + assert_eq!(value.to_bits(), reference.to_bits()); + } + } + } + } + } + } + } + } + + fn state() -> Filter { + Filter { + time: 0, + p: Vector3::zero(), + v: Vector3::zero(), + q: UnitQuaternion::identity(), + bg: Vector3::zero(), + ba: Vector3::zero(), + datum: 0.0, + covariance: identity(), + timing: crate::timing::TimingEnvelope::ZERO, + } + } + fn stationary_imu() -> FlightImuObservation { + FlightImuObservation { + acquisition_duration_us: 10_000, + context: crate::ObservationContext { + measurement_time: kaptan_time::Timestamp::from_microseconds(0), + receipt_time: kaptan_time::Timestamp::from_microseconds(0), + source: crate::SourceId::new(1), + frame: crate::FrameId::BODY_FRD, + calibration: crate::CalibrationRevision::new(1), + quality: 255, + validity: crate::Validity::Valid, + sequence: crate::ObservationSequence::new(1), + time_uncertainty_microseconds: 0, + }, + acceleration: crate::AccelerationFrd::metres_per_second_squared( + 0.0, + 0.0, + -GRAVITY_MPS2, + ), + angular_rate: crate::AngularRateFrd::radians_per_second(0.0, 0.0, 0.0), + } + } + #[test] + fn covariance_screen_rejects_asymmetry_singular_and_unrepresentable_factors() { + for (row, col, value) in [ + (0, 0, f32::NAN), + (1, 0, f32::NAN), + (0, 1, f32::NAN), + (1, 0, 0.1), + (0, 0, 0.0), + ] { + let mut p = identity(); + p[row][col] = value; + assert!(!covariance_valid(&p)); + } + let mut p = identity(); + p[0][1] = f32::MAX / 2.0; + p[1][0] = p[0][1]; + assert!(!covariance_valid(&p)); // finite off-diagonal factor, overflowed pivot + p[0][0] = f32::MIN_POSITIVE; + assert!(!covariance_valid(&p)); // division cannot represent the factor + for field in 0..5 { + let mut f = state(); + match field { + 0 => f.p = Vector3::new(f32::NAN, 0.0, 0.0), + 1 => f.v = Vector3::new(f32::NAN, 0.0, 0.0), + 2 => f.bg = Vector3::new(f32::NAN, 0.0, 0.0), + 3 => f.ba = Vector3::new(f32::NAN, 0.0, 0.0), + _ => f.datum = f32::NAN, + } + assert!(!f.valid()); + } + } + #[test] + fn left_mekf_reset_matches_trigonometric_covariance() { + for angle in [0.0_f32, 1e-5, 0.2, 1.999, 2.0] { + let mut f = state(); + // Symmetric, strictly diagonally dominant positive covariance. + // Correlations exercise both unchanged and measurement-updated + // position/attitude blocks through the real correction/reset path. + f.covariance[0][0] = 2.0; + f.covariance[1][1] = 3.0; + f.covariance[2][2] = 4.0; + f.covariance[7][7] = 2.0; + f.covariance[8][8] = 4.0; + for (i, j, value) in [(0, 7, 0.25), (1, 6, -0.2), (2, 8, 0.4), (6, 7, 0.1)] { + f.covariance[i][j] = value; + f.covariance[j][i] = value; + } + assert!(f.valid()); + let prior = f.covariance; + let mut h = [0.0; N]; + h[8] = 1.0; + // P_zz=R=4 gives gain 1/2. Residual 2*angle produces the + // desired correction with NIS=angle^2/2 <= 2, inside the gate. + assert!(f.scalar(h, 2.0 * angle, 4.0).unwrap()); + assert!(f.valid()); + let theta = f64::from(angle); + let (s, c) = if angle == 0.0 { + (1.0, 0.0) + } else { + (theta.sin() / theta, (1.0 - theta.cos()) / theta) + }; + let mut reset = [[0.0_f64; N]; N]; + for (i, row) in reset.iter_mut().enumerate() { + row[i] = 1.0; + } + reset[6][6] = s; + reset[6][7] = -c; + reset[7][6] = c; + reset[7][7] = s; + // Independent Gaussian conditioning, then exact trigonometric J_l. + // This does not call the production Joseph/reset math helpers. + for i in 0..N { + for j in 0..N { + let mut expected = 0.0; + for k in 0..N { + for l in 0..N { + let posterior = f64::from(prior[k][l]) + - f64::from(prior[k][8]) * f64::from(prior[8][l]) / 8.0; + expected += reset[i][k] * posterior * reset[j][l]; + } + } + assert!((f64::from(f.covariance[i][j]) - expected).abs() < 3e-6); + } + } + assert!((f64::from(f.q.w()) - (theta / 2.0).cos()).abs() < 2e-6); + assert!((f64::from(f.q.z()) - (theta / 2.0).sin()).abs() < 2e-6); + assert!((f64::from(f.p.z()) - 0.1 * theta).abs() < 1e-7); + } + } + + #[test] + fn propagation_time_and_late_numeric_failures_are_atomic() { + let config = NavigationConfig::engineering_fixture(); + let mut f = state(); + let imu = stationary_imu(); + f.propagate(imu, 0, config).unwrap(); + assert_eq!(f.covariance, identity()); + f.time = 10; + assert_eq!(f.propagate(imu, 9, config), Err(NavigationFailure::ImuGap)); + assert_eq!( + f.propagate(imu, 20_011, config), + Err(NavigationFailure::ImuGap) + ); + let mut bad = imu; + bad.angular_rate = crate::AngularRateFrd::radians_per_second(1000.0, 0.0, 0.0); + assert_eq!( + f.propagate(bad, 10_010, config), + Err(NavigationFailure::Numerical) + ); + bad = imu; + bad.acceleration = crate::AccelerationFrd::metres_per_second_squared(f32::MAX, 0.0, 0.0); + assert_eq!( + f.propagate(bad, 10_010, config), + Err(NavigationFailure::Numerical) + ); + assert_eq!(f.time, 10); + assert_eq!(f.p, Vector3::zero()); + assert_eq!(f.covariance, identity()); + } + #[test] + fn innovation_and_candidate_failures_retain_the_prior() { + let mut h = [0.0; N]; + h[0] = 1.0; + for (residual, variance) in [(f32::NAN, 1.0), (0.0, 0.0), (0.0, -1.0)] { + let mut f = state(); + assert_eq!( + f.scalar(h, residual, variance), + Err(NavigationFailure::Numerical) + ); + assert_eq!(f.covariance, identity()); + } + let mut f = state(); + f.covariance[0][0] = f32::MAX; + let before = f.covariance; + let mut large_h = h; + large_h[0] = 2.0; + assert_eq!( + f.scalar(large_h, 1.0, 1.0), + Err(NavigationFailure::Numerical) + ); + assert_eq!(f.covariance, before); + f = state(); + f.covariance[0][0] = -1.0; + assert_eq!(f.scalar(h, 0.0, 1.0), Err(NavigationFailure::Numerical)); + f = state(); + assert_eq!(f.scalar(h, f32::MAX, 1.0), Ok(false)); + assert_eq!(f.p, Vector3::zero()); + f = state(); + f.covariance[15][15] = -1.0; + let before = f.covariance; + assert_eq!(f.scalar(h, 0.1, 1.0), Err(NavigationFailure::Numerical)); + assert_eq!(f.covariance, before); + f = state(); + f.covariance[6][6] = 16.0; + let before = f.covariance; + h = [0.0; N]; + h[6] = 1.0; + assert_eq!(f.scalar(h, 4.0, 1.0), Err(NavigationFailure::Numerical)); + assert_eq!(f.covariance, before); + } + fn noise() -> NoiseDensities { + NoiseDensities { + gyro: 0.03, + acceleration: 0.2, + gyro_bias: 0.04, + acceleration_bias: 0.05, + barometric_datum: 0.06, + } + } + #[test] + fn invalid_timing_updates_leave_gaussian_state_and_clock_generator_untouched() { + let mut h = [0.0; N]; + h[0] = 1.0; + for (effect, remainder) in [(f32::NAN, 0.0), (0.0, f32::NAN), (0.0, -0.001)] { + let mut f = state(); + let mut generator = [0.0; N]; + assert_eq!( + f.scalar_timed(h, 0.0, 1.0, effect, remainder, &mut generator), + Err(NavigationFailure::Numerical) + ); + assert_eq!(f.covariance, identity()); + assert_eq!(generator, [0.0; N]); + } + let mut f = state(); + let mut generator = [0.0; N]; + generator[0] = -f32::MAX; + let before = generator; + assert_eq!( + f.scalar_timed(h, 0.0, 1.0, f32::MAX, 0.0, &mut generator), + Err(NavigationFailure::Numerical) + ); + assert_eq!(generator, before); + assert_eq!(f.covariance, identity()); + + // A finite but unrepresentable gain/model product must fail before + // either Gaussian state or the caller-owned correlated error is committed. + f.covariance[1][0] = f32::MAX; + let prior = f.covariance; + generator[0] = 4.0; + let before = generator; + assert_eq!( + f.scalar_timed(h, 0.0, 1.0, 0.0, 0.0, &mut generator), + Err(NavigationFailure::Numerical) + ); + assert_eq!(generator, before); + assert_eq!(f.covariance, prior); + assert_eq!(f.p, Vector3::zero()); + } + #[test] + fn persistent_clock_bias_does_not_average_into_gaussian_noise() { + let mut f = state(); + let mut h = [0.0; N]; + h[0] = 1.0; + let mut early = 0.0; + for sample in 0..1000 { + let mut clock = [0.0; N]; + assert!( + f.scalar_timed(h, 0.002 - f.p.x(), 1e-6, 0.002, 0.0, &mut clock) + .unwrap() + ); + f.timing.add_generator(clock).unwrap(); + if sample == 19 { + early = f.timing.radius(0); + } + } + assert!(f.p.x() > 3.0 * sqrt(f.covariance[0][0])); + assert!(f.p.x() <= f.timing.radius(0)); + assert!(f.timing.radius(0) >= early * 0.95); + } + #[test] + fn one_record_clock_generator_keeps_cross_axis_correlation() { + let mut f = state(); + let mut clock = [0.0; N]; + for (axis, delta) in [(0, 0.002), (1, 0.004)] { + let mut h = [0.0; N]; + h[axis] = 1.0; + assert!( + f.scalar_timed(h, delta, 1e-6, delta, 0.0, &mut clock) + .unwrap() + ); + } + f.timing.add_generator(clock).unwrap(); + let mut cancel = [0.0; N]; + cancel[0] = 2.0; + cancel[1] = -1.0; + assert!(f.timing.projected(&cancel).unwrap() < 0.0001); + assert!(f.timing.radius(0) > 0.0019); + } + #[test] + fn exact_white_acceleration_and_bias_integrals() { + let n = noise(); + let t = 0.02; + let (phi, q) = discretize([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]], [0.; 3], t, n); + let close = |a: f32, b: f64| { + let error = (f64::from(a) - b).abs(); + let tolerance = b.abs() * 2e-6 + 1e-14; + assert!(error < tolerance, "{a} != {b}") + }; + let t = f64::from(t); + let a = f64::from(n.acceleration); + let b = f64::from(n.acceleration_bias); + close(q[0][0], a * t.powi(3) / 3. + b * t.powi(5) / 20.); + close(q[0][3], a * t * t / 2. + b * t.powi(4) / 8.); + close(q[3][3], a * t + b * t.powi(3) / 3.); + close(q[0][12], -b * t.powi(3) / 6.); + close(q[3][12], -b * t * t / 2.); + close(q[6][9], -f64::from(n.gyro_bias) * t * t / 2.); + close( + q[6][6], + f64::from(n.gyro) * t + f64::from(n.gyro_bias) * t.powi(3) / 3., + ); + close(phi[0][12], -t * t / 2.); + } + #[test] + fn frozen_split_semigroup_including_initial_covariance() { + let r = [[0., -1., 0.], [1., 0., 0.], [0., 0., 1.]]; + let f = [1., 2., -9.]; + let (a, qa) = discretize(r, f, 0.007, noise()); + let (b, qb) = discretize(r, f, 0.013, noise()); + let (ab, qab) = discretize(r, f, 0.020, noise()); + let composed = product(&b, &a); + let mut p = identity(); + p[0][3] = 0.2; + p[3][0] = 0.2; + let mut first = sandwich(&a, &p); + for i in 0..N { + for j in 0..N { + first[i][j] += qa[i][j]; + } + } + let mut split = sandwich(&b, &first); + let mut whole = sandwich(&ab, &p); + for i in 0..N { + for j in 0..N { + split[i][j] += qb[i][j]; + whole[i][j] += qab[i][j]; + assert!((split[i][j] - whole[i][j]).abs() < 3e-7); + assert!((ab[i][j] - composed[i][j]).abs() < 3e-8); + } + } + assert!(covariance_valid(&split)); + } + #[test] + fn pressure_reference_oracle() { + for (pressure_pascals, temperature_kelvin) in + [(80_000.0, 273.15), (101_325.0, 288.15), (110_000.0, 305.0)] + { + let reference = BarometricReference { + pressure_pascals, + temperature_kelvin, + }; + for ratio in [0.95, 0.975, 1.0, 1.025, 1.05] { + let p = pressure_pascals * ratio; + let (height, derivative) = pressure_height(p, reference).unwrap(); + let scale = 287.05_f64 * f64::from(temperature_kelvin) / f64::from(GRAVITY_MPS2); + let expected = scale * (f64::from(pressure_pascals) / f64::from(p)).ln(); + assert!((f64::from(height) - expected).abs() < 0.0002); + assert!((f64::from(derivative) - scale / f64::from(p)).abs() < 2e-8); + } + for p in [ + f32::NAN, + 0.0, + f32::INFINITY, + (0.95 * pressure_pascals).next_down(), + (1.05 * pressure_pascals).next_up(), + ] { + assert!(pressure_height(p, reference).is_none()); + } + } + } + #[test] + fn gravity_coupled_gyro_and_bias_noise_match_analytic_high_order_integrals() { + let n = NoiseDensities { + gyro: 0.02, + gyro_bias: 0.04, + acceleration: 0., + acceleration_bias: 0., + barometric_datum: 0., + }; + let t = 0.02_f32; + let g = 9.8_f32; + let (_, q) = discretize( + [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]], + [0., 0., -g], + t, + n, + ); + let (t, g, qg, qb) = ( + f64::from(t), + f64::from(g), + f64::from(n.gyro), + f64::from(n.gyro_bias), + ); + let close = |a: f32, b: f64| { + let error = (f64::from(a) - b).abs(); + let tolerance = b.abs() * 3e-6 + 1e-18; + assert!(error <= tolerance, "{a} != {b}") + }; + // A gyro-bias Brownian drive integrates through tilt, horizontal + // velocity, then position: its position kernel is g*t³/6. + close( + q[0][0], + g * g * (qg * t.powi(5) / 20. + qb * t.powi(7) / 252.), + ); + close( + q[0][3], + g * g * (qg * t.powi(4) / 8. + qb * t.powi(6) / 72.), + ); + close( + q[3][3], + g * g * (qg * t.powi(3) / 3. + qb * t.powi(5) / 20.), + ); + close(q[0][7], -g * (qg * t.powi(3) / 6. + qb * t.powi(5) / 30.)); + close(q[0][10], g * qb * t.powi(4) / 24.); + } + + #[test] + fn midpoint_nominal_converges_to_rotating_force_known_answer() { + let run = |step: u64| { + let mut f = Filter { + time: 0, + p: Vector3::zero(), + v: Vector3::zero(), + q: UnitQuaternion::identity(), + bg: Vector3::zero(), + ba: Vector3::zero(), + datum: 0., + covariance: identity(), + timing: crate::timing::TimingEnvelope::ZERO, + }; + let imu = FlightImuObservation { + acquisition_duration_us: 10_000, + context: crate::ObservationContext { + measurement_time: kaptan_time::Timestamp::from_microseconds(0), + receipt_time: kaptan_time::Timestamp::from_microseconds(0), + source: crate::SourceId::new(1), + frame: crate::FrameId::BODY_FRD, + calibration: crate::CalibrationRevision::new(1), + quality: 255, + validity: crate::Validity::Valid, + sequence: crate::ObservationSequence::new(1), + time_uncertainty_microseconds: 0, + }, + acceleration: crate::AccelerationFrd::metres_per_second_squared( + 1., + 0., + -GRAVITY_MPS2, + ), + angular_rate: crate::AngularRateFrd::radians_per_second(0., 0., 1.), + }; + for t in (step..=200_000).step_by(step as usize) { + f.propagate(imu, t, NavigationConfig::engineering_fixture()) + .unwrap(); + } + let expected_y = 0.2_f64 - 0.2_f64.sin(); + assert!((f64::from(f.v.x()) - 0.2_f64.sin()).abs() < 5e-6); + assert!((f64::from(f.p.x()) - (1. - 0.2_f64.cos())).abs() < 5e-6); + (f64::from(f.p.y()) - expected_y).abs() + }; + let coarse = run(20_000); + let fine = run(10_000); + assert!(fine < coarse * 0.35, "coarse={coarse} fine={fine}"); + assert!(fine < 2e-6, "fine={fine}"); + } + + #[test] + fn new_filter_rejections_preserve_committed_state() { + let mut f = Filter { + time: 0, + p: Vector3::zero(), + v: Vector3::zero(), + q: UnitQuaternion::identity(), + bg: Vector3::zero(), + ba: Vector3::zero(), + datum: 0., + covariance: identity(), + timing: crate::timing::TimingEnvelope::ZERO, + }; + let before = f; + let mut h = [0.; N]; + h[0] = 1.; + assert_eq!(f.scalar(h, 100., 0.01), Ok(false)); + assert_eq!(f.covariance, before.covariance); + assert_eq!(f.p, before.p); + assert_eq!( + f.scalar(h, f32::MAX, f32::NAN), + Err(NavigationFailure::Numerical) + ); + assert_eq!(f.covariance, before.covariance); + assert_eq!(f.q, before.q); + assert!(f.scalar(h, 0.1, 0.01).unwrap()); + assert!(f.valid()); + assert!(f.p.x() > 0.); + assert!(f.covariance[0][0] < before.covariance[0][0]); + } +} diff --git a/firmware/libs/kaptan-navigation/src/flight.rs b/firmware/libs/kaptan-navigation/src/flight.rs new file mode 100644 index 0000000..2d46e7a --- /dev/null +++ b/firmware/libs/kaptan-navigation/src/flight.rs @@ -0,0 +1,868 @@ +//! Flight observation, uncertainty and bounded execution contracts. + +use crate::*; +use kaptan_math::{Frd, Ned, UnitQuaternion, Vector3}; +use kaptan_time::Timestamp; + +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct OriginId(pub u32); +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct SensorReferenceId(pub u32); + +/// Mean calibrated force/rate over the declared preceding acquisition window. +/// Interior propagation reconstructs that window with its constant average. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct FlightImuObservation { + pub context: ObservationContext, + pub acceleration: AccelerationFrd, + pub angular_rate: AngularRateFrd, + /// Positive duration in the endpoint clock, bounded by the configured IMU + /// interval. Both window endpoints must share the same timing model. + pub acquisition_duration_us: u32, +} +impl FlightImuObservation { + pub(crate) fn acquisition_start(self) -> Option { + if self.acquisition_duration_us == 0 { + return None; + } + self.context + .measurement_time + .as_microseconds() + .checked_sub(u64::from(self.acquisition_duration_us)) + } +} +/// Timing of the latest valid admitted IMU window, possibly not yet propagated. +/// This historical snapshot does not establish freshness, estimator health or +/// output authority. It may remain available after a numerical failure. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct ImuCoverage { + pub end: Timestamp, + pub acquisition_duration_us: u32, + pub time_uncertainty_microseconds: u32, +} +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct MagneticObservation { + pub context: ObservationContext, + pub microtesla_frd: Vector3, + pub variance_microtesla_squared: [ScalarVariance; 3], +} +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct PressureObservation { + pub context: ObservationContext, + pub pascals: f32, + pub variance_pascals_squared: ScalarVariance, +} +#[derive(Clone, Copy, Debug, PartialEq)] +pub enum SensorRecord { + Imu(FlightImuObservation), + LocalPositionVelocity(LocalPositionVelocityObservation), + Magnetic(MagneticObservation), + Pressure(PressureObservation), + Range(RangeObservation), +} +impl SensorRecord { + pub const fn context(self) -> ObservationContext { + match self { + Self::Imu(v) => v.context, + Self::LocalPositionVelocity(v) => v.context, + Self::Magnetic(v) => v.context, + Self::Pressure(v) => v.context, + Self::Range(v) => v.context, + } + } + pub const fn kind(self) -> u8 { + match self { + Self::Imu(_) => 1, + Self::LocalPositionVelocity(_) => 2, + Self::Magnetic(_) => 3, + Self::Pressure(_) => 4, + Self::Range(_) => 5, + } + } +} + +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +#[repr(u16)] +pub enum ObservationRejection { + WrongEpoch = 1, + InvalidTime = 2, + TooOld = 3, + Replay = 4, + SourceMismatch = 5, + FrameMismatch = 6, + CalibrationMismatch = 7, + InvalidValue = 8, + InvalidQuality = 9, + QueueFull = 10, + RateExceeded = 11, + BehindFusionHorizon = 12, + ReferenceMismatch = 13, + Terminated = 14, +} +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum ObservationAdmission { + Queued, + Rejected(ObservationRejection), +} +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum NavigationFailure { + ImuGap, + QueueOverrun, + WorkBudget, + Numerical, + RequiredCapabilityLost, +} +impl From for NavigationFailure { + fn from(_: kaptan_math::MathError) -> Self { + Self::Numerical + } +} +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum SourceStatus { + Unknown, + Healthy, + Stale, + Isolated, +} +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct SensorStatus { + pub kind: u8, + pub source: SourceId, + pub integrity: SourceStatus, + pub last_received_measurement: Option, + pub last_fused_measurement: Option, + pub queued: u16, + pub rejected: u32, + pub fused: u32, + pub dropped: u32, +} + +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct LocalOrigin { + pub id: OriginId, + pub established_at: Timestamp, + pub reference: SensorReferenceId, + pub launch_in_reference: PositionNed, + pub launch_variance_m2: [f32; 3], +} +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct CapabilityEvidence { + pub available: bool, + pub last_measurement_at: Option, + pub valid_until: Option, +} +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct FlightCapabilities { + pub tilt: CapabilityEvidence, + pub heading: CapabilityEvidence, + pub horizontal_position: CapabilityEvidence, + pub horizontal_velocity: CapabilityEvidence, + pub height: CapabilityEvidence, + pub vertical_velocity: CapabilityEvidence, + pub ground_clearance: CapabilityEvidence, +} +impl FlightCapabilities { + pub fn flight_ready(self) -> bool { + self.tilt.available + && self.heading.available + && self.horizontal_position.available + && self.horizontal_velocity.available + && self.height.available + && self.vertical_velocity.available + && self.ground_clearance.available + } + pub fn valid_until(self) -> Option { + if !self.flight_ready() { + return None; + } + [ + self.tilt, + self.heading, + self.horizontal_position, + self.horizontal_velocity, + self.height, + self.vertical_velocity, + self.ground_clearance, + ] + .into_iter() + .map(|v| v.valid_until) + .try_fold(Timestamp::from_microseconds(u64::MAX), |a, b| { + Some(a.min(b?)) + }) + } +} +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct GroundClearance { + pub measurement_at: Timestamp, + pub valid_until: Timestamp, + pub source: SourceId, + pub body_origin_height_metres: f32, + pub contact_clearance_metres: f32, + pub variance_m2: f32, + pub measurement_time_uncertainty_microseconds: u32, + pub upper_error_metres: f32, +} +impl GroundClearance { + /// Bound current clearance using current foot orientation and velocity + /// confidence. `upper_error_metres` bounds measured COM height, not a foot + /// projection frozen at the range timestamp. + pub fn upper_at( + self, + now: Timestamp, + estimate: &FlightEstimate, + contact: ContactGeometry, + ) -> Option { + let speed_bound = + estimate.velocity_error_bound().norm() + estimate.velocity.vector().norm(); + let angle_bound = estimate.attitude_error_bound().norm(); + if now >= self.valid_until + || estimate.at != now + || !speed_bound.is_finite() + || !angle_bound.is_finite() + || !contact.validate() + { + return None; + } + let earliest = self + .measurement_at + .as_microseconds() + .checked_sub(u64::from(self.measurement_time_uncertainty_microseconds))?; + let age = now.as_microseconds().checked_sub(earliest)? as f32 * 1e-6; + // Configuration checks the vehicle's free-flight acceleration bound. + let feet = contact.depth(estimate.attitude); + let upper = self.body_origin_height_metres + + self.upper_error_metres + + speed_bound * age + + 0.5 * MAXIMUM_FREE_ACCELERATION_MPS2 * age * age + - feet + + contact.rotation_radius_bound() * angle_bound; + upper.is_finite().then_some(upper) + } +} +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct FlightEstimate { + pub at: Timestamp, + pub fused_through: Timestamp, + /// Latest complete IMU acquisition-window end, not its start or receipt. + pub last_imu_at: Timestamp, + pub origin: LocalOrigin, + pub attitude: UnitQuaternion, + pub position: PositionNed, + pub velocity: VelocityNed, + /// Bias-corrected average of the latest complete declared window. This is not + /// an instantaneous angular-rate measurement at `at`. + pub angular_rate: AngularRateFrd, + pub position_variance_m2: [f32; 3], + pub velocity_variance_m2ps2: [f32; 3], + pub attitude_variance_rad2: [f32; 3], + pub timing_error_bound: TimingErrorBounds, + pub capabilities: FlightCapabilities, + pub ground_clearance: Option, +} +/// Deterministic absolute error radii from timing, separate from Gaussian P. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct TimingErrorBounds { + pub position_metres: [f32; 3], + pub velocity_mps: [f32; 3], + pub attitude_radians: [f32; 3], +} +impl TimingErrorBounds { + pub const ZERO: Self = Self { + position_metres: [0.0; 3], + velocity_mps: [0.0; 3], + attitude_radians: [0.0; 3], + }; +} +impl FlightEstimate { + pub fn velocity_error_bound(&self) -> Vector3 { + let e: [f32; 3] = core::array::from_fn(|i| { + 3.0 * kaptan_math::sqrt(self.velocity_variance_m2ps2[i]) + + self.timing_error_bound.velocity_mps[i] + }); + Vector3::new(e[0], e[1], e[2]) + } + pub fn attitude_error_bound(&self) -> Vector3 { + let e: [f32; 3] = core::array::from_fn(|i| { + 3.0 * kaptan_math::sqrt(self.attitude_variance_rad2[i]) + + self.timing_error_bound.attitude_radians[i] + }); + Vector3::new(e[0], e[1], e[2]) + } +} +/// Four equal contact spheres at the corners of a body-FRD rectangle. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct ContactGeometry { + pub half_span_x_metres: f32, + pub half_span_y_metres: f32, + pub centre_z_metres: f32, + pub radius_metres: f32, +} +impl ContactGeometry { + pub(crate) fn validate(self) -> bool { + self.half_span_x_metres.is_finite() + && self.half_span_x_metres > 0.0 + && self.half_span_y_metres.is_finite() + && self.half_span_y_metres > 0.0 + && self.centre_z_metres.is_finite() + && self.radius_metres.is_finite() + && self.radius_metres >= 0.0 + && (self.rotation_radius_bound() + self.radius_metres).is_finite() + } + fn rotation_radius_bound(self) -> f32 { + l1_bound(Vector3::new( + self.half_span_x_metres, + self.half_span_y_metres, + self.centre_z_metres, + )) + } + pub(crate) fn depth(self, q: UnitQuaternion) -> f32 { + let mut depth = f32::NEG_INFINITY; + for x in [-self.half_span_x_metres, self.half_span_x_metres] { + for y in [-self.half_span_y_metres, self.half_span_y_metres] { + let foot = q.body_to_ned(Vector3::new(x, y, self.centre_z_metres)).z() + + self.radius_metres; + if !foot.is_finite() { + return f32::NAN; + } + depth = depth.max(foot); + } + } + depth + } +} + +/// Selected source for each supported observation kind. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct NavigationSources { + pub imu: SourceId, + pub local_position_velocity: SourceId, + pub magnetic: SourceId, + pub pressure: SourceId, + pub range: SourceId, +} +impl NavigationSources { + pub(crate) const fn ordered(self) -> [SourceId; 5] { + [ + self.imu, + self.local_position_velocity, + self.magnetic, + self.pressure, + self.range, + ] + } +} + +/// Range-sensor extrinsics and deterministic quantization error. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct RangeInstallation { + pub frame: FrameId, + pub offset_frd_metres: Vector3, + /// Finite nonzero beam direction, normalized once by navigation construction. + pub direction_frd: Vector3, + pub quantization_error_metres: f32, +} +impl RangeInstallation { + fn validate(self) -> bool { + self.offset_frd_metres.is_finite() + && l1_bound(self.offset_frd_metres).is_finite() + && self.direction_frd.normalized().is_some() + && self.quantization_error_metres.is_finite() + && self.quantization_error_metres >= 0.0 + } + pub(crate) fn lever_bound(self) -> f32 { + l1_bound(self.offset_frd_metres) + } +} + +// The L1 norm bounds Euclidean radius. Round both additions outward. +fn l1_bound(v: Vector3) -> f32 { + ((v.x().abs() + v.y().abs()).next_up() + v.z().abs()).next_up() +} + +/// Local isothermal pressure reference. The conversion accepts 0.95..1.05 of it. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct BarometricReference { + pub pressure_pascals: f32, + pub temperature_kelvin: f32, +} +impl BarometricReference { + pub(crate) fn height_scale(self) -> f32 { + 287.05 * self.temperature_kelvin / GRAVITY_MPS2 + } + fn validate(self) -> bool { + let lower = 0.95 * self.pressure_pascals; + let upper = 1.05 * self.pressure_pascals; + let scale = self.height_scale(); + self.pressure_pascals.is_finite() + && self.pressure_pascals > 0.0 + && self.temperature_kelvin.is_finite() + && self.temperature_kelvin > 0.0 + && (self.pressure_pascals + upper).is_finite() + && scale.is_finite() + && (scale / lower).is_finite() + && scale / upper > 0.0 + } +} + +/// Required compiled installation. Calibration identifies one coherent set of +/// mounting, calibration and uncertainty data, changed only by reinitialization. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct NavigationInstallation { + pub sources: NavigationSources, + pub range: RangeInstallation, + pub contact: ContactGeometry, + pub atmosphere: BarometricReference, + pub magnetic_field_ned_microtesla: Vector3, + pub calibration: CalibrationRevision, + pub reference: SensorReferenceId, +} +impl NavigationInstallation { + fn validate(self) -> bool { + self.range.validate() + && self.contact.validate() + && self.atmosphere.validate() + && self.magnetic_field_ned_microtesla.is_finite() + && Vector3::::new( + self.magnetic_field_ned_microtesla.x(), + self.magnetic_field_ned_microtesla.y(), + 0.0, + ) + .norm() + > 10.0 + && self.magnetic_field_ned_microtesla.norm() < 100.0 + && self.reference.0 != 0 + } +} + +/// Continuous process-noise densities in SI units. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct NoiseDensities { + pub gyro: f32, // rad²/s + pub acceleration: f32, // m²/s³ + pub gyro_bias: f32, // rad²/s³ + pub acceleration_bias: f32, // m²/s⁵ + pub barometric_datum: f32, // m²/s +} +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct NavigationConfig { + pub noise: NoiseDensities, + /// Acquisition-window, numerical-step and current-extrapolation ceiling. + pub maximum_imu_interval_microseconds: u64, + pub imu_freshness_microseconds: u64, + pub aiding_freshness_microseconds: u64, + pub maximum_time_uncertainty_microseconds: u32, + pub lag_microseconds: u64, + pub installation: NavigationInstallation, +} +impl NavigationConfig { + #[cfg(test)] + pub const fn engineering_fixture() -> Self { + Self { + noise: NoiseDensities { + gyro: 2.5e-7, + acceleration: 2.5e-5, + gyro_bias: 1e-10, + acceleration_bias: 1e-8, + barometric_datum: 1e-5, + }, + maximum_imu_interval_microseconds: 20_000, + imu_freshness_microseconds: 20_000, + aiding_freshness_microseconds: 250_000, + maximum_time_uncertainty_microseconds: 2_000, + lag_microseconds: 100_000, + installation: NavigationInstallation { + sources: NavigationSources { + imu: SourceId::new(1), + local_position_velocity: SourceId::new(2), + magnetic: SourceId::new(3), + pressure: SourceId::new(4), + range: SourceId::new(5), + }, + range: RangeInstallation { + frame: FrameId::new(3), + offset_frd_metres: Vector3::new(0.0, 0.0, 0.04), + direction_frd: Vector3::new(0.0, 0.0, 1.0), + quantization_error_metres: 0.0005, + }, + contact: ContactGeometry { + half_span_x_metres: 0.12, + half_span_y_metres: 0.12, + centre_z_metres: 0.065, + radius_metres: 0.015, + }, + atmosphere: BarometricReference { + pressure_pascals: 101_325.0, + temperature_kelvin: 288.15, + }, + magnetic_field_ned_microtesla: Vector3::new(20.0, 0.0, 45.0), + calibration: CalibrationRevision::new(1), + reference: SensorReferenceId(1), + }, + } + } + pub fn validate(self) -> bool { + let n = self.noise; + [ + n.gyro, + n.acceleration, + n.gyro_bias, + n.acceleration_bias, + n.barometric_datum, + ] + .iter() + .all(|v| v.is_finite() && *v > 0.0 && *v <= 1.0) + && (10_000..=20_000).contains(&self.maximum_imu_interval_microseconds) + && self.imu_freshness_microseconds >= self.maximum_imu_interval_microseconds + && self.imu_freshness_microseconds <= 20_000 + && self.lag_microseconds == 100_000 + && (200_000..=500_000).contains(&self.aiding_freshness_microseconds) + && self.maximum_time_uncertainty_microseconds <= 2_000 + && self.installation.validate() + } +} + +pub(crate) fn earliest(c: ObservationContext) -> Option { + c.measurement_time + .as_microseconds() + .checked_sub(u64::from(c.time_uncertainty_microseconds)) +} +pub(crate) fn expiry(c: ObservationContext, age: u64) -> Option { + Some(Timestamp::from_microseconds(earliest(c)?.checked_add(age)?)) +} + +#[cfg(test)] +mod tests { + use super::*; + fn contact() -> ContactGeometry { + NavigationConfig::engineering_fixture().installation.contact + } + fn time(v: u64) -> Timestamp { + Timestamp::from_microseconds(v) + } + fn estimate(at: Timestamp) -> FlightEstimate { + let capability = CapabilityEvidence { + available: true, + last_measurement_at: Some(at), + valid_until: Some(time(u64::MAX)), + }; + FlightEstimate { + at, + fused_through: at, + last_imu_at: at, + origin: LocalOrigin { + id: OriginId(1), + established_at: at, + reference: SensorReferenceId(1), + launch_in_reference: PositionNed::metres(0.0, 0.0, 0.0), + launch_variance_m2: [0.0; 3], + }, + attitude: UnitQuaternion::identity(), + position: PositionNed::metres(0.0, 0.0, 0.0), + velocity: VelocityNed::metres_per_second(0.1, 0.0, 0.0), + angular_rate: AngularRateFrd::radians_per_second(0.0, 0.0, 0.0), + position_variance_m2: [0.0; 3], + velocity_variance_m2ps2: [0.0; 3], + attitude_variance_rad2: [0.0; 3], + timing_error_bound: TimingErrorBounds::ZERO, + capabilities: FlightCapabilities { + tilt: capability, + heading: capability, + horizontal_position: capability, + horizontal_velocity: capability, + height: capability, + vertical_velocity: capability, + ground_clearance: capability, + }, + ground_clearance: None, + } + } + #[test] + fn capability_expiry_requires_every_requested_capability() { + let evidence = CapabilityEvidence { + available: true, + last_measurement_at: Some(time(10)), + valid_until: Some(time(100)), + }; + let healthy = FlightCapabilities { + tilt: evidence, + heading: evidence, + horizontal_position: evidence, + horizontal_velocity: evidence, + height: evidence, + vertical_velocity: evidence, + ground_clearance: evidence, + }; + assert_eq!(healthy.valid_until(), Some(time(100))); + for index in 0..7 { + let mut c = healthy; + [ + &mut c.tilt, + &mut c.heading, + &mut c.horizontal_position, + &mut c.horizontal_velocity, + &mut c.height, + &mut c.vertical_velocity, + &mut c.ground_clearance, + ] + .into_iter() + .nth(index) + .unwrap() + .available = false; + assert!(!c.flight_ready()); + assert_eq!(c.valid_until(), None); + } + let mut c = healthy; + c.ground_clearance.valid_until = None; + assert_eq!(c.valid_until(), None); + c = healthy; + c.heading.valid_until = Some(time(50)); + assert_eq!(c.valid_until(), Some(time(50))); + } + #[test] + fn clearance_bound_uses_earliest_capture_and_rejects_invalid_arithmetic() { + let c = GroundClearance { + measurement_at: time(10_000), + valid_until: time(60_000), + source: SourceId::new(5), + body_origin_height_metres: 0.08, + contact_clearance_metres: 0.0, + variance_m2: 4e-6, + measurement_time_uncertainty_microseconds: 1_000, + upper_error_metres: 0.01, + }; + let e = estimate(time(20_000)); + assert!((c.upper_at(time(20_000), &e, contact()).unwrap() - 0.012_612_5).abs() < 1e-7); + assert_eq!( + c.upper_at(time(60_000), &estimate(time(60_000)), contact()), + None + ); + for speed in [f32::NAN, f32::INFINITY] { + let mut bad = e; + bad.velocity = VelocityNed::metres_per_second(speed, 0.0, 0.0); + assert_eq!(c.upper_at(time(20_000), &bad, contact()), None); + } + assert_eq!( + c.upper_at(time(8_000), &estimate(time(8_000)), contact()), + None + ); + assert_eq!(c.upper_at(time(20_001), &e, contact()), None); + let mut bad_angle = e; + bad_angle.attitude_variance_rad2[0] = f32::NAN; + assert_eq!(c.upper_at(time(20_000), &bad_angle, contact()), None); + let mut invalid_contact = contact(); + invalid_contact.radius_metres = -0.01; + assert_eq!(c.upper_at(time(20_000), &e, invalid_contact), None); + assert_eq!( + GroundClearance { + measurement_time_uncertainty_microseconds: 20_000, + ..c + } + .upper_at(time(20_000), &e, contact()), + None + ); + assert_eq!( + GroundClearance { + valid_until: time(3_000_000), + ..c + } + .upper_at( + time(2_000_000), + &FlightEstimate { + at: time(2_000_000), + velocity: VelocityNed::metres_per_second(f32::MAX, 0.0, 0.0), + ..e + }, + contact(), + ), + None + ); + } + #[test] + fn current_foot_geometry_and_velocity_confidence_bound_landing_clearance() { + // Independent rigid geometry: fixed COM 0.132 m; foot-sphere centres + // (+/-0.12,+/-0.12,0.065), radius 0.015. Roll changes 0.2 -> 0. + let old_depth = 0.12_f64 * 0.2_f64.sin() + 0.065 * 0.2_f64.cos() + 0.015; + let measured_contact = 0.132 - old_depth; + let old_bound = measured_contact + 0.0075 + 0.2 * 0.020 + 0.5 * 25.0 * 0.020_f64.powi(2); + assert!(old_bound < 0.05); + let actual_now = 0.132 - (0.065 + 0.015); + let c = GroundClearance { + measurement_at: time(10_000), + valid_until: time(70_000), + source: SourceId::new(5), + body_origin_height_metres: 0.132, + contact_clearance_metres: measured_contact as f32, + variance_m2: 4e-6, + measurement_time_uncertainty_microseconds: 0, + upper_error_metres: 0.0075, + }; + let mut e = estimate(time(30_000)); + e.velocity = VelocityNed::metres_per_second(0.0, 0.0, 0.0); + let upper = c.upper_at(e.at, &e, contact()).unwrap(); + assert!(f64::from(upper) >= actual_now); + assert!(upper > 0.05); // neither cap entry nor contact dwell may use the old bound + e.velocity_variance_m2ps2 = [0.04; 3]; + let uncertain = c.upper_at(e.at, &e, contact()).unwrap(); + assert!(uncertain > upper + 0.020); // 3*sqrt(trace(Pv))*20 ms, not fixed +0.2 m/s + } + #[test] + fn contact_geometry() { + for geometry in [ + contact(), + ContactGeometry { + half_span_x_metres: 0.3, + half_span_y_metres: 0.2, + centre_z_metres: 0.15, + radius_metres: 0.025, + }, + ] { + let x = f64::from(geometry.half_span_x_metres); + let y = f64::from(geometry.half_span_y_metres); + let z = f64::from(geometry.centre_z_metres); + assert!(f64::from(geometry.rotation_radius_bound()) >= (x * x + y * y + z * z).sqrt()); + for (roll, pitch) in [(-0.5_f64, 0.2_f64), (0.0, 0.0), (0.4, -0.3)] { + let (sr, cr) = ((roll / 2.0).sin(), (roll / 2.0).cos()); + let (sp, cp) = ((pitch / 2.0).sin(), (pitch / 2.0).cos()); + let q = UnitQuaternion::from_components( + (cp * cr) as f32, + (cp * sr) as f32, + (sp * cr) as f32, + (-sp * sr) as f32, + ) + .unwrap(); + let expected = x * pitch.sin().abs() + + y * (pitch.cos() * roll.sin()).abs() + + z * pitch.cos() * roll.cos() + + f64::from(geometry.radius_metres); + assert!((f64::from(geometry.depth(q)) - expected).abs() < 1e-7); + let e = FlightEstimate { + attitude: q, + ..estimate(time(10_000)) + }; + let c = GroundClearance { + measurement_at: e.at, + valid_until: time(70_000), + source: SourceId::new(90), + body_origin_height_metres: 1.0, + contact_clearance_metres: 0.0, + variance_m2: 1e-6, + measurement_time_uncertainty_microseconds: 0, + upper_error_metres: 0.001, + }; + assert!(f64::from(c.upper_at(e.at, &e, geometry).unwrap()) >= 1.0 - expected); + } + } + } + #[test] + fn installation_validation() { + let fixture = NavigationConfig::engineering_fixture(); + for field in 0..9 { + for bad in [f32::NAN, f32::INFINITY, -1.0] { + let mut config = fixture; + let i = &mut config.installation; + match field { + 0 => i.contact.half_span_x_metres = bad, + 1 => i.contact.half_span_y_metres = bad, + 2 => i.contact.radius_metres = bad, + 3 => i.range.quantization_error_metres = bad, + 4 => i.atmosphere.pressure_pascals = bad, + 5 => i.atmosphere.temperature_kelvin = bad, + 6 if !bad.is_finite() => i.contact.centre_z_metres = bad, + 7 if !bad.is_finite() => { + i.range.offset_frd_metres = Vector3::new(bad, 0.0, 0.0) + } + 8 if !bad.is_finite() => i.range.direction_frd = Vector3::new(0.0, 0.0, bad), + _ => continue, + } + assert!(Navigation::new(config).is_err()); + } + } + for field in 0..6 { + let mut config = fixture; + let i = &mut config.installation; + match field { + 0 => i.range.direction_frd = Vector3::zero(), + 1 => i.contact.half_span_x_metres = 0.0, + 2 => i.contact.half_span_y_metres = 0.0, + 3 => i.atmosphere.pressure_pascals = 0.0, + 4 => i.atmosphere.temperature_kelvin = 0.0, + _ => i.atmosphere.pressure_pascals = f32::MAX, + } + assert!(Navigation::new(config).is_err()); + } + let mut config = fixture; + config.installation.contact.half_span_x_metres = f32::MAX; + config.installation.contact.half_span_y_metres = f32::MAX; + assert!(Navigation::new(config).is_err()); + config = fixture; + config.installation.range.offset_frd_metres = Vector3::new(f32::MAX, f32::MAX, 0.0); + assert!(Navigation::new(config).is_err()); + for (pressure_pascals, temperature_kelvin) in [ + (101_325.0, f32::MAX), + (f32::MIN_POSITIVE, 288.15), + (1e20, f32::from_bits(1)), + ] { + config = fixture; + config.installation.atmosphere = BarometricReference { + pressure_pascals, + temperature_kelvin, + }; + assert!(Navigation::new(config).is_err()); + } + } + #[test] + fn navigation_configuration_rejects_each_invalid_domain() { + let factory = NavigationConfig::engineering_fixture(); + assert!(factory.validate()); + for index in 0..5 { + for bad in [f32::NAN, 0.0, -1.0, 1.01] { + let mut c = factory; + *[ + &mut c.noise.gyro, + &mut c.noise.acceleration, + &mut c.noise.gyro_bias, + &mut c.noise.acceleration_bias, + &mut c.noise.barometric_datum, + ] + .into_iter() + .nth(index) + .unwrap() = bad; + assert!(!c.validate()); + assert!(crate::Navigation::new(c).is_err()); + } + } + let mut c = factory; + c.maximum_imu_interval_microseconds = 9_999; + assert!(!c.validate()); + c = factory; + c.imu_freshness_microseconds = 19_999; + assert!(!c.validate()); + c = factory; + c.imu_freshness_microseconds = 20_001; + assert!(!c.validate()); + c = factory; + c.lag_microseconds = 0; + assert!(!c.validate()); + c = factory; + c.aiding_freshness_microseconds = 100_000; + assert!(!c.validate()); + c = factory; + c.maximum_time_uncertainty_microseconds = 2_001; + assert!(!c.validate()); + c = factory; + c.installation.magnetic_field_ned_microtesla = Vector3::new(f32::NAN, 0.0, 45.0); + assert!(!c.validate()); + c = factory; + c.installation.magnetic_field_ned_microtesla = Vector3::new(10.0, 0.0, 45.0); + assert!(!c.validate()); + c = factory; + c.installation.magnetic_field_ned_microtesla = Vector3::new(20.0, f32::MAX, 45.0); + assert!(!c.validate()); + c = factory; + c.installation.reference = SensorReferenceId(0); + assert!(!c.validate()); + c = factory; + c.installation.magnetic_field_ned_microtesla = Vector3::new(0.0, 20.0, 45.0); + assert!(c.validate()); + } +} diff --git a/firmware/libs/kaptan-navigation/src/lib.rs b/firmware/libs/kaptan-navigation/src/lib.rs index 98e8e82..4604b83 100644 --- a/firmware/libs/kaptan-navigation/src/lib.rs +++ b/firmware/libs/kaptan-navigation/src/lib.rs @@ -1,41 +1,27 @@ -//! Navigation observation contracts and a SITL-only raw-rate ESKF. -//! -//! Navigation owns the conversion from driver integer observations into SI -//! `f32` values. Board and platform crates must not manufacture navigation -//! observations or duplicate their frame/calibration metadata. +//! Allocation-free sensor-driven navigation shared by host and MCU endpoints. #![no_std] +#![forbid(unsafe_code)] -#[cfg(all(feature = "sitl", target_os = "none"))] -compile_error!( - "kaptan-navigation's raw-rate ESKF is restricted to the SITL executable; \ - it is not available to a board target" -); - -#[cfg(any(test, feature = "sitl"))] -mod eskf; +mod filter; +mod flight; +mod navigation; mod observation; +mod timing; + +pub use flight::*; +pub use navigation::Navigation; pub use observation::{ - AccelerationFrd, AngularRateFrd, CalibrationRevision, Covariance2, FrameId, - LocalPositionVelocityObservation, ObservationContext, ObservationSequence, - OpticalFlowObservation, PositionFrd, PositionNed, RangeObservation, ScalarVariance, - SensorExtrinsics, SourceId, Validity, VelocityNed, + AccelerationFrd, AngularRateFrd, CalibrationRevision, FrameId, + LocalPositionVelocityObservation, ObservationContext, ObservationSequence, PositionNed, + RangeObservation, ScalarVariance, SourceId, Validity, VelocityNed, }; -/// Internal-SITL API for the provisional raw-rate ESKF. -/// -/// It supports the host simulator only. It cannot compile for a bare-metal -/// target and must not be used as a flight-estimator API. -#[cfg(any(test, feature = "sitl"))] -pub mod sitl { - pub use crate::eskf::{ - Eskf, EstimatorCapabilities, EstimatorConfig, EstimatorHealth, FusionError, FusionResult, - InertialEskf, IntegrityConfig, LocalTerrainEskf, NominalState, PropagationError, - SourceStatus, StationaryInitializer, - }; - pub use crate::observation::{ImuNoise, ImuObservation}; -} - /// Standard gravity in metres per second squared. pub const GRAVITY_MPS2: f32 = 9.806_65; + +/// Supported free-flight acceleration bound in m/s², including gravity +/// and maximum collective thrust. Flight configuration checks T_max/m + g +/// against this bound. It does not bound contact impulses. +pub const MAXIMUM_FREE_ACCELERATION_MPS2: f32 = 25.0; diff --git a/firmware/libs/kaptan-navigation/src/navigation.rs b/firmware/libs/kaptan-navigation/src/navigation.rs new file mode 100644 index 0000000..34753aa --- /dev/null +++ b/firmware/libs/kaptan-navigation/src/navigation.rs @@ -0,0 +1,2771 @@ +//! Live admission, sensor-only initialization and bounded delayed fusion. + +use crate::{ + filter::{Filter, InputTiming, N, ZERO, components, pressure_height, rotation, skew, vector}, + *, +}; +use kaptan_math::{Frd, Ned, UnitQuaternion, Vector3, sqrt}; +use kaptan_time::Timestamp; + +#[derive(Clone, Copy)] +struct Moments { + n: u32, + mean: [f32; D], + m2: [f32; D], + noise: [f32; D], + weight: f32, + square_weight: f32, +} +impl Moments { + const EMPTY: Self = Self { + n: 0, + mean: [0.; D], + m2: [0.; D], + noise: [0.; D], + weight: 0.0, + square_weight: 0.0, + }; + fn push(&mut self, x: [f32; D], variance: [f32; D]) { + self.push_weighted(x, variance, 1.0); + } + fn push_weighted(&mut self, x: [f32; D], variance: [f32; D], weight: f32) { + self.n += 1; + self.weight += weight; + self.square_weight += weight * weight; + for i in 0..D { + let d = x[i] - self.mean[i]; + self.mean[i] += d * (weight / self.weight); + self.m2[i] += weight * d * (x[i] - self.mean[i]); + self.noise[i] += weight * weight * variance[i]; + } + } + fn variance(&self, i: usize) -> f32 { + self.m2[i] / self.weight.max(1.0) + } + fn mean_variance(&self, i: usize) -> f32 { + (self.variance(i) * self.square_weight / (self.weight.max(1.0) * self.weight.max(1.0))) + .max(self.noise[i] / (self.weight.max(1.0) * self.weight.max(1.0))) + .max(1e-10) + } +} +struct Initializer { + start: Option, + last_imu: Option, + maximum_time_error: [u32; 5], + maximum_gyro: f32, + maximum_velocity: f32, + accel: Moments<3>, + gyro: Moments<3>, + position: Moments<3>, + velocity: Moments<3>, + magnetic: Moments<3>, + pressure: Moments<1>, + range: Moments<1>, +} +impl Initializer { + const fn new() -> Self { + Self { + start: None, + last_imu: None, + maximum_time_error: [0; 5], + maximum_gyro: 0.0, + maximum_velocity: 0.0, + accel: Moments::EMPTY, + gyro: Moments::EMPTY, + position: Moments::EMPTY, + velocity: Moments::EMPTY, + magnetic: Moments::EMPTY, + pressure: Moments::EMPTY, + range: Moments::EMPTY, + } + } + fn push(&mut self, r: SensorRecord, config: NavigationConfig) { + let time = r.context().measurement_time.as_microseconds(); + // Only measurements in the IMU-established stationary window count. + // Bound accumulation even when the caller supplies records without ticks. + if self + .start + .is_some_and(|start| time.saturating_sub(start) > 2_500_000) + { + *self = Self::new(); + } + if self.start.is_none() && !matches!(r, SensorRecord::Imu(_)) { + return; + } + if self.start.is_some_and(|start| time < start) { + return; + } + let kind = usize::from(r.kind() - 1); + self.maximum_time_error[kind] = + self.maximum_time_error[kind].max(r.context().time_uncertainty_microseconds); + match r { + SensorRecord::Imu(v) => { + let Some(start) = v.acquisition_start() else { + return; + }; + self.start.get_or_insert(start); + self.last_imu = Some(time); + self.maximum_gyro = self.maximum_gyro.max(v.angular_rate.vector().norm()); + let duration = v.acquisition_duration_us as f32 * 1e-6; + self.accel.push_weighted( + components(v.acceleration.vector()), + [config.noise.acceleration / duration; 3], + v.acquisition_duration_us as f32, + ); + self.gyro.push_weighted( + components(v.angular_rate.vector()), + [config.noise.gyro / duration; 3], + v.acquisition_duration_us as f32, + ); + } + SensorRecord::LocalPositionVelocity(v) => { + self.maximum_velocity = self.maximum_velocity.max(v.velocity.vector().norm()); + self.position.push( + components(v.position.vector()), + v.position_variance.map(|v| v.get()), + ); + self.velocity.push( + components(v.velocity.vector()), + v.velocity_variance.map(|v| v.get()), + ); + } + SensorRecord::Magnetic(v) => self.magnetic.push( + components(v.microtesla_frd), + v.variance_microtesla_squared.map(|v| v.get()), + ), + SensorRecord::Pressure(v) => { + if let Some((z, d)) = pressure_height(v.pascals, config.installation.atmosphere) { + self.pressure + .push([z], [d * d * v.variance_pascals_squared.get()]); + } + } + SensorRecord::Range(v) => self.range.push([v.metres], [v.variance.get()]), + } + } + fn finish( + &self, + imu: FlightImuObservation, + config: NavigationConfig, + ) -> Option<(Filter, LocalOrigin)> { + let time = imu.context.measurement_time.as_microseconds(); + if time != self.last_imu? + || time.checked_sub(self.start?)? < 2_000_000 + || self.accel.n < 200 + || self.position.n < 40 + || self.magnetic.n < 40 + || self.pressure.n < 40 + || self.range.n < 80 + { + return None; + } + let accel = vector::(self.accel.mean); + let mag = vector::(self.magnetic.mean); + if (accel.norm() - GRAVITY_MPS2).abs() > 0.3 + || vector::(self.gyro.mean).norm() > 0.1 + || vector::(self.velocity.mean).norm() > 0.1 + || self.range.variance(0) > 0.0004 + || (0..3).any(|i| { + self.accel.variance(i) > 0.16 + || self.gyro.variance(i) > 0.0025 + || self.velocity.variance(i) > 0.01 + || self.magnetic.variance(i) > 0.25 + }) + { + return None; + } + let magnetic_field = config.installation.magnetic_field_ned_microtesla; + let q = triad(accel, mag, magnetic_field)?; + let duration = time.checked_sub(self.start?)? as f32 * 1e-6; + // Duration-weighted contiguous windows average to the continuous window + // mean. The bias at its END differs by Brownian variance q_bg*T/3. + let gyro_walk_time = duration / 3.0; + let mut covariance = ZERO; + for i in 0..3 { + covariance[i][i] = self.position.mean_variance(i).max(1e-6); + covariance[i + 3][i + 3] = self.velocity.mean_variance(i).max(1e-6); + // Mean noise and residual calibration/drift uncertainty are distinct. + covariance[i + 9][i + 9] = + self.gyro.mean_variance(i) + config.noise.gyro_bias * gyro_walk_time + 1e-8; + covariance[i + 12][i + 12] = 0.05 * 0.05; + } + // Joint TRIAD initialization Jacobian. Finite differences are bounded + // startup-only work; the independent tilted/bias tests exercise its signs. + for input in 0..6 { + let eps = 0.001; + let mut ap = self.accel.mean; + let mut am = ap; + let mut mp = self.magnetic.mean; + let mut mm = mp; + if input < 3 { + ap[input] += eps; + am[input] -= eps; + } else { + mp[input - 3] += eps; + mm[input - 3] -= eps; + } + let plus = triad(vector(ap), vector(mp), magnetic_field)?; + let minus = triad(vector(am), vector(mm), magnetic_field)?; + let delta = plus.compose(minus.inverse()).ok()?; + let j = [delta.x() / eps, delta.y() / eps, delta.z() / eps]; + let variance = if input < 3 { + self.accel.mean_variance(input) + 0.0025 + } else { + self.magnetic.mean_variance(input - 3) + 0.0004 + }; + for a in 0..3 { + for b in 0..3 { + covariance[a + 6][b + 6] += j[a] * variance * j[b]; + } + if input < 3 { + covariance[a + 6][input + 12] = -j[a] * 0.0025; + covariance[input + 12][a + 6] = covariance[a + 6][input + 12]; + } + } + } + for (i, row) in covariance.iter_mut().enumerate().skip(6).take(3) { + row[i] += 1e-7; + } + covariance[15][15] = covariance[2][2] + self.pressure.mean_variance(0) + 0.0001; + covariance[2][15] = covariance[2][2]; + covariance[15][2] = covariance[2][2]; + let p = vector(self.position.mean); + let mut filter = Filter { + time, + p, + v: vector(self.velocity.mean), + q, + bg: vector(self.gyro.mean), + ba: Vector3::zero(), + datum: self.pressure.mean[0] + p.z(), + covariance, + timing: crate::timing::TimingEnvelope::ZERO, + }; + // Startup timing is bounded calibration uncertainty. GPS position and + // datum share the same down error; it is not independent pressure noise. + let position_time = self.maximum_time_error[1] as f32 * 1e-6; + let velocity_noise = self.velocity.noise.iter().sum::() / self.velocity.n as f32; + let launch_speed_bound = self.maximum_velocity + 3.0 * sqrt(velocity_noise); + let launch_position_bound = launch_speed_bound * position_time + + 0.5 * (100.0 + GRAVITY_MPS2) * position_time * position_time; + for axis in 0..3 { + let mut g = [0.0; N]; + g[axis] = launch_position_bound; + if axis == 2 { + g[15] = g[2]; + } + filter.timing.add_generator(g).ok()?; + } + let angle = (self.maximum_gyro + 0.1) + * (self.maximum_time_error[0] + self.maximum_time_error[2]) as f32 + * 1e-6; + for axis in 0..3 { + let mut g = [0.0; N]; + g[axis + 6] = angle; + filter.timing.add_generator(g).ok()?; + } + let origin = LocalOrigin { + id: OriginId(1), + established_at: imu.context.measurement_time, + reference: config.installation.reference, + launch_in_reference: PositionNed::metres(p.x(), p.y(), p.z()), + launch_variance_m2: core::array::from_fn(|i| covariance[i][i]), + }; + filter.valid().then_some((filter, origin)) + } +} +fn triad( + accel: Vector3, + mag: Vector3, + field: Vector3, +) -> Option> { + if !(10.0..=100.0).contains(&mag.norm()) { + return None; + } + let down = accel.scale(-1.).normalized()?; + let north = mag.sub(down.scale(mag.dot(down))).normalized()?; + let east = down.cross(north).normalized()?; + let n = Vector3::new(field.x(), field.y(), 0.).normalized()?; + let e = Vector3::::new(0., 0., 1.).cross(n); + let axis = |i: usize| { + n.scale(components(north)[i]) + .add(e.scale(components(east)[i])) + .add(Vector3::new(0., 0., components(down)[i])) + }; + UnitQuaternion::from_body_axes_in_ned(axis(0), axis(1), axis(2)).ok() +} + +/// Fixed-capacity navigation estimator. +pub struct Navigation { + config: NavigationConfig, + initializer: Initializer, + filter: Option, + origin: Option, + origin_timing: [f32; 3], + imu: [Option; 32], + imu_len: usize, + aids: [Option; 32], + aid_len: usize, + range: Option, + stamps: [Option; 5], + used: [Option; 5], + status: [SensorStatus; 5], + rejects: [u8; 5], + last_rejection: [Option; 5], + probes: [u8; 5], + clearance: Option, + range_processed: Option, + estimate: Option, + failed: Option, +} +impl Navigation { + pub fn new(mut config: NavigationConfig) -> Result { + if !config.validate() { + return Err(NavigationFailure::Numerical); + } + config.installation.range.direction_frd = config + .installation + .range + .direction_frd + .normalized() + .ok_or(NavigationFailure::Numerical)?; + Ok(Self { + config, + initializer: Initializer::new(), + filter: None, + origin: None, + origin_timing: [0.0; 3], + imu: [None; 32], + imu_len: 0, + aids: [None; 32], + aid_len: 0, + range: None, + stamps: [None; 5], + used: [None; 5], + status: core::array::from_fn(|i| SensorStatus { + kind: i as u8 + 1, + source: config.installation.sources.ordered()[i], + integrity: SourceStatus::Unknown, + last_received_measurement: None, + last_fused_measurement: None, + queued: 0, + rejected: 0, + fused: 0, + dropped: 0, + }), + rejects: [0; 5], + last_rejection: [None; 5], + probes: [0; 5], + clearance: None, + range_processed: None, + estimate: None, + failed: None, + }) + } + pub fn estimate(&self) -> Option { + self.estimate + } + pub fn source_status(&self) -> [SensorStatus; 5] { + self.status + } + /// Admission timing only. Rejected records do not replace this window and + /// querying it does not advance prediction or renew capability evidence. + pub fn admitted_imu_coverage(&self) -> Option { + self.imu[..self.imu_len] + .last() + .copied() + .flatten() + .map(|imu| ImuCoverage { + end: imu.context.measurement_time, + acquisition_duration_us: imu.acquisition_duration_us, + time_uncertainty_microseconds: imu.context.time_uncertainty_microseconds, + }) + } + pub fn observe( + &mut self, + r: SensorRecord, + now: Timestamp, + ) -> Result { + if let Some(e) = self.failed { + return Err(e); + } + let result = self.admit(r, now); + if let Err(e) = result { + self.failed = Some(e); + self.estimate = None; + } + result + } + fn reject(&mut self, record: SensorRecord, why: ObservationRejection) -> ObservationAdmission { + let i = usize::from(record.kind() - 1); + let measured = record.context().measurement_time; + self.status[i].rejected = self.status[i].rejected.saturating_add(1); + self.last_rejection[i] = Some(self.last_rejection[i].map_or(measured, |t| t.max(measured))); + self.rejects[i] = self.rejects[i].saturating_add(1); + self.probes[i] = 0; + if self.rejects[i] >= 3 { + self.status[i].integrity = SourceStatus::Isolated; + } + if self.filter.is_none() { + self.initializer = Initializer::new(); + } + ObservationAdmission::Rejected(why) + } + fn admit( + &mut self, + r: SensorRecord, + now: Timestamp, + ) -> Result { + use ObservationRejection as R; + let i = usize::from(r.kind() - 1); + let c = r.context(); + let refuse = |why| Ok(ObservationAdmission::Rejected(why)); + if c.source != self.config.installation.sources.ordered()[i] { + return refuse(R::SourceMismatch); + } + let frame = match r { + SensorRecord::Imu(_) | SensorRecord::Magnetic(_) => FrameId::BODY_FRD, + SensorRecord::LocalPositionVelocity(_) => FrameId::LOCAL_NED, + SensorRecord::Pressure(_) => FrameId::SCALAR, + SensorRecord::Range(_) => self.config.installation.range.frame, + }; + if c.frame != frame { + return refuse(R::FrameMismatch); + } + if c.calibration != self.config.installation.calibration { + return refuse(R::CalibrationMismatch); + } + if c.time_uncertainty_microseconds > self.config.maximum_time_uncertainty_microseconds + || earliest(c).is_none() + || matches!(r, SensorRecord::Imu(v) + if v.acquisition_start().is_none() + || u64::from(v.acquisition_duration_us) + > self.config.maximum_imu_interval_microseconds) + || c.measurement_time + .as_microseconds() + .checked_add(u64::from(c.time_uncertainty_microseconds)) + .is_none_or(|v| v > c.receipt_time.as_microseconds()) + || c.receipt_time > now + { + return refuse(R::InvalidTime); + } + let age = if i == 0 { + self.config.imu_freshness_microseconds + } else { + self.config.aiding_freshness_microseconds + }; + if expiry(c, age).is_none_or(|v| now >= v) { + return refuse(R::TooOld); + } + if let Some(previous) = self.stamps[i] { + let delta = c.sequence.get().wrapping_sub(previous.sequence.get()); + if c.measurement_time <= previous.measurement_time || delta == 0 || delta >= 1 << 31 { + return refuse(R::Replay); + } + let interval = + c.measurement_time.as_microseconds() - previous.measurement_time.as_microseconds(); + let minimum = if i == 4 { 15_000 } else { 40_000 }; + if i != 0 && interval < minimum { + return refuse(R::RateExceeded); + } + } + if i != 0 + && i != 4 + && self + .filter + .is_some_and(|f| c.measurement_time.as_microseconds() <= f.time) + { + return refuse(R::BehindFusionHorizon); + } + self.stamps[i] = Some(c); + self.status[i].last_received_measurement = Some(c.measurement_time); + if c.validity != Validity::Valid || c.quality == 0 { + return Ok(self.reject(r, R::InvalidQuality)); + } + let valid = match r { + SensorRecord::Imu(v) => { + v.acceleration.vector().is_finite() + && v.angular_rate.vector().is_finite() + && v.acceleration.vector().norm() < 100. + && v.angular_rate.vector().norm() < 20. + } + SensorRecord::LocalPositionVelocity(v) => { + if v.reference != self.config.installation.reference { + return Ok(self.reject(r, R::ReferenceMismatch)); + } + v.position.vector().is_finite() + && v.velocity.vector().is_finite() + && v.position.vector().norm() < 10_000. + && v.velocity.vector().norm() < 25. + } + SensorRecord::Magnetic(v) => { + v.microtesla_frd.is_finite() + && (v.microtesla_frd.norm() + - self + .config + .installation + .magnetic_field_ned_microtesla + .norm()) + .abs() + < 5. + } + SensorRecord::Pressure(v) => { + pressure_height(v.pascals, self.config.installation.atmosphere).is_some() + } + SensorRecord::Range(v) => v.metres.is_finite() && (0.01..=40.).contains(&v.metres), + }; + if !valid { + return Ok(self.reject(r, R::InvalidValue)); + } + if let SensorRecord::Imu(v) = r + && let Some(previous) = self.used[0] + && v.acquisition_start() != Some(previous.measurement_time.as_microseconds()) + { + // Only accepted, valid acquisition coverage can be extended. A + // consumed bad-quality/value watermark is not inertial evidence. + if self.filter.is_some() { + return Err(NavigationFailure::ImuGap); + } + if v.acquisition_start().unwrap() < previous.measurement_time.as_microseconds() { + return refuse(R::RateExceeded); + } + self.initializer = Initializer::new(); + } + if self.filter.is_none() { + self.initializer.push(r, self.config); + } + match r { + SensorRecord::Imu(v) => { + if self.imu_len == 32 { + if self.filter.is_some() { + self.status[0].dropped = self.status[0].dropped.saturating_add(1); + return Err(NavigationFailure::QueueOverrun); + } + self.imu.copy_within(1..32, 0); + self.imu_len -= 1; + } + self.imu[self.imu_len] = Some(v); + self.imu_len += 1; + self.used[0] = Some(c); + self.status[0].integrity = SourceStatus::Healthy; + } + SensorRecord::Range(v) => self.range = Some(v), + _ => { + if self.filter.is_some() { + if self.aid_len == 32 { + self.status[i].dropped = self.status[i].dropped.saturating_add(1); + return Err(NavigationFailure::QueueOverrun); + } + let key = |s: SensorRecord| (s.context().measurement_time, s.kind()); + let at = (0..self.aid_len) + .find(|&j| key(self.aids[j].unwrap()) > key(r)) + .unwrap_or(self.aid_len); + self.aids.copy_within(at..self.aid_len, at + 1); + self.aids[at] = Some(r); + self.aid_len += 1; + } + } + } + Ok(ObservationAdmission::Queued) + } + pub fn tick(&mut self, now: Timestamp) -> Result<(), NavigationFailure> { + if let Some(e) = self.failed { + return Err(e); + } + let result = self.advance(now); + if let Err(e) = result { + self.failed = Some(e); + self.estimate = None; + } + result + } + fn advance(&mut self, now: Timestamp) -> Result<(), NavigationFailure> { + let Some(last) = self.imu_len.checked_sub(1).and_then(|i| self.imu[i]) else { + return Ok(()); + }; + if self.filter.is_none() { + if let Some((f, o)) = self.initializer.finish(last, self.config) { + self.filter = Some(f); + self.origin = Some(o); + self.origin_timing = core::array::from_fn(|i| f.timing.radius(i)); + self.initializer = Initializer::new(); + self.used = [None; 5]; + self.used[0] = Some(last.context); + self.rejects = [0; 5]; + self.last_rejection = [None; 5]; + self.probes = [0; 5]; + // Startup history precedes the committed state. Retaining it + // through the inhibited lag warmup would exhaust the IMU ring. + self.imu = [None; 32]; + self.imu[0] = Some(last); + self.imu_len = 1; + } else { + if self + .initializer + .start + .is_some_and(|t| now.as_microseconds().saturating_sub(t) > 2_500_000) + { + self.initializer = Initializer::new(); + } + return Ok(()); + } + } + let mut f = self.filter.unwrap(); + let horizon = now + .as_microseconds() + .saturating_sub(self.config.lag_microseconds); + if horizon < f.time { + self.estimate = None; + return Ok(()); + } + let target = self.imu[..self.imu_len] + .iter() + .flatten() + .map(|v| v.context.measurement_time.as_microseconds()) + .filter(|&t| t <= horizon && t >= f.time) + .max() + .unwrap_or(f.time); + if target - f.time > 80_000 { + return Err(NavigationFailure::WorkBudget); + } + let mut fused = 0; + while self.aid_len > 0 + && self.aids[0] + .unwrap() + .context() + .measurement_time + .as_microseconds() + <= target + { + if fused == 8 { + return Err(NavigationFailure::WorkBudget); + } + fused += 1; + let r = self.aids[0].unwrap(); + predict( + &mut f, + r.context().measurement_time.as_microseconds(), + &self.imu, + self.imu_len, + self.config, + )?; + let mut candidate = f; + let motion = motion_at(&candidate, &self.imu, self.imu_len, self.config)?; + let accepted = fuse_with_motion(&mut candidate, r, self.config, motion)?; + if self.accept_aid(r, accepted) { + f = candidate; + } + // Commit each completed historical update together with its source + // evidence. A later work-budget or numerical fault must not report + // discarded candidate updates as successfully fused. + self.filter = Some(f); + self.aids.copy_within(1..self.aid_len, 0); + self.aid_len -= 1; + self.aids[self.aid_len] = None; + } + predict(&mut f, target, &self.imu, self.imu_len, self.config)?; + self.filter = Some(f); + let mut prediction = f; + if let Some(range) = self.range { + let t = range.context.measurement_time.as_microseconds(); + if self.range_processed != Some(range.context.sequence.get()) + && t >= f.time + && t <= now.as_microseconds() + { + predict(&mut prediction, t, &self.imu, self.imu_len, self.config)?; + let motion = motion_at(&prediction, &self.imu, self.imu_len, self.config)?; + let clear = clearance_with_rate( + prediction, + range, + motion.rate_bound, + self.config.installation, + ); + self.range_processed = Some(range.context.sequence.get()); + if self.accept_aid(SensorRecord::Range(range), clear.is_some()) { + self.clearance = clear; + } + } + } + let imu_valid = + expiry(last.context, self.config.imu_freshness_microseconds).is_some_and(|t| now < t); + if imu_valid { + predict( + &mut prediction, + now.as_microseconds(), + &self.imu, + self.imu_len, + self.config, + )?; + } + // Keep the average ending at the fusion boundary and all newer windows. + // The preceding average is needed to bound shifted window boundaries. + let remove = (0..self.imu_len) + .rfind(|&i| { + self.imu[i] + .unwrap() + .context + .measurement_time + .as_microseconds() + <= f.time + }) + .unwrap_or(0); + if remove > 0 { + self.imu.copy_within(remove..self.imu_len, 0); + self.imu_len -= remove; + for x in &mut self.imu[self.imu_len..] { + *x = None; + } + } + let origin = self.origin.unwrap(); + let pv = core::array::from_fn(|i| { + let s = sqrt(prediction.covariance[i][i]) + sqrt(origin.launch_variance_m2[i]); + s * s + }); + let vv = core::array::from_fn(|i| prediction.covariance[i + 3][i + 3]); + let av = core::array::from_fn(|i| prediction.covariance[i + 6][i + 6]); + let timing_error_bound = TimingErrorBounds { + position_metres: core::array::from_fn(|i| { + prediction.timing.radius(i) + self.origin_timing[i] + }), + velocity_mps: core::array::from_fn(|i| prediction.timing.radius(i + 3)), + attitude_radians: core::array::from_fn(|i| prediction.timing.radius(i + 6)), + }; + let usable = |variance: f32, bound: f32, maximum_variance: f32| { + 3.0 * sqrt(variance) + bound < 3.0 * sqrt(maximum_variance) + }; + let evidence = |index: usize, age: u64, valid: bool| { + let c = self.used[index]; + let until = c.and_then(|c| expiry(c, age)); + CapabilityEvidence { + available: imu_valid + && valid + && self.status[index].integrity != SourceStatus::Isolated + && until.is_some_and(|t| now < t), + last_measurement_at: c.map(|c| c.measurement_time), + valid_until: until, + } + }; + let tilt = evidence( + 0, + self.config.imu_freshness_microseconds, + usable(av[0], timing_error_bound.attitude_radians[0], 0.03) + && usable(av[1], timing_error_bound.attitude_radians[1], 0.03), + ); + let heading = evidence( + 2, + self.config.aiding_freshness_microseconds, + usable(av[2], timing_error_bound.attitude_radians[2], 0.03), + ); + let hp = evidence( + 1, + self.config.aiding_freshness_microseconds, + usable(pv[0], timing_error_bound.position_metres[0], 0.25) + && usable(pv[1], timing_error_bound.position_metres[1], 0.25), + ); + let hv = evidence( + 1, + self.config.aiding_freshness_microseconds, + usable(vv[0], timing_error_bound.velocity_mps[0], 0.04) + && usable(vv[1], timing_error_bound.velocity_mps[1], 0.04), + ); + let mut height = evidence( + 3, + self.config.aiding_freshness_microseconds, + usable(pv[2], timing_error_bound.position_metres[2], 0.25), + ); + height.available &= hp.available; + let vz = evidence( + 1, + self.config.aiding_freshness_microseconds, + usable(vv[2], timing_error_bound.velocity_mps[2], 0.04), + ); + let ground = evidence(4, 60_000, self.clearance.is_some() && tilt.available); + for i in 0..5 { + let age = if i == 0 { + self.config.imu_freshness_microseconds + } else if i == 4 { + 60_000 + } else { + self.config.aiding_freshness_microseconds + }; + if self.status[i].integrity == SourceStatus::Healthy + && self.used[i] + .and_then(|c| expiry(c, age)) + .is_none_or(|t| now >= t) + { + self.status[i].integrity = SourceStatus::Stale; + } + self.status[i].queued = if i == 0 { + self.imu_len as u16 + } else { + self.aids[..self.aid_len] + .iter() + .flatten() + .filter(|r| usize::from(r.kind() - 1) == i) + .count() as u16 + }; + } + let pos = prediction.p.sub(origin.launch_in_reference.vector()); + let rate = last.angular_rate.vector().sub(prediction.bg); + self.estimate = Some(FlightEstimate { + at: Timestamp::from_microseconds(prediction.time), + fused_through: Timestamp::from_microseconds(f.time), + last_imu_at: last.context.measurement_time, + origin, + attitude: prediction.q, + position: PositionNed::metres(pos.x(), pos.y(), pos.z()), + velocity: VelocityNed::metres_per_second( + prediction.v.x(), + prediction.v.y(), + prediction.v.z(), + ), + angular_rate: AngularRateFrd::radians_per_second(rate.x(), rate.y(), rate.z()), + position_variance_m2: pv, + velocity_variance_m2ps2: vv, + attitude_variance_rad2: av, + timing_error_bound, + capabilities: FlightCapabilities { + tilt, + heading, + horizontal_position: hp, + horizontal_velocity: hv, + height, + vertical_velocity: vz, + ground_clearance: ground, + }, + ground_clearance: self.clearance, + }); + Ok(()) + } + fn accept_aid(&mut self, r: SensorRecord, accepted: bool) -> bool { + let i = usize::from(r.kind() - 1); + if !accepted { + self.reject(r, ObservationRejection::InvalidValue); + return false; + } + let after_rejection = + self.last_rejection[i].is_none_or(|t| r.context().measurement_time > t); + if self.status[i].integrity == SourceStatus::Isolated { + // A delayed good sample is not evidence that a newer source fault + // recovered. Probes must postdate the latest known rejection. + if !after_rejection { + return false; + } + self.probes[i] = self.probes[i].saturating_add(1); + if self.probes[i] < 2 { + return false; + } + } + if after_rejection { + self.rejects[i] = 0; + self.probes[i] = 0; + } + self.used[i] = Some(r.context()); + self.status[i].integrity = SourceStatus::Healthy; + self.status[i].last_fused_measurement = Some(r.context().measurement_time); + self.status[i].fused = self.status[i].fused.saturating_add(1); + true + } +} +fn predict( + f: &mut Filter, + to: u64, + imu: &[Option; 32], + len: usize, + c: NavigationConfig, +) -> Result<(), NavigationFailure> { + if to < f.time { + return Err(NavigationFailure::ImuGap); + } + let mut steps = 0; + while f.time < to { + if steps == 24 { + return Err(NavigationFailure::WorkBudget); + } + steps += 1; + let index = (0..len) + .find(|&i| imu[i].unwrap().context.measurement_time.as_microseconds() > f.time) + .or_else(|| len.checked_sub(1)) + .ok_or(NavigationFailure::ImuGap)?; + let sample = imu[index].unwrap(); + let end = sample.context.measurement_time.as_microseconds(); + let next = if f.time < end { + if sample + .acquisition_start() + .is_none_or(|start| start > f.time) + { + return Err(NavigationFailure::ImuGap); + } + to.min(end) + } else { + // Reconstruct history only from complete coverage. Beyond the last + // coverage end, hold its average for the bounded current prediction. + if to - end > c.maximum_imu_interval_microseconds { + return Err(NavigationFailure::ImuGap); + } + to + }; + let start = f.time; + let timing = imu[..len].windows(2).filter_map(|pair| { + let previous = pair[0].unwrap(); + let following = pair[1].unwrap(); + let boundary = previous.context.measurement_time.as_microseconds(); + let error = u64::from( + previous + .context + .time_uncertainty_microseconds + .max(following.context.time_uncertainty_microseconds), + ); + let low = start.max(boundary.saturating_sub(error)); + let high = next.min(boundary.saturating_add(error)); + (low < high).then_some(InputTiming { + start: low, + end: high, + force_jump: following + .acceleration + .vector() + .sub(previous.acceleration.vector()), + rate_jump: following + .angular_rate + .vector() + .sub(previous.angular_rate.vector()), + }) + }); + f.propagate_timed(sample, next, c, timing)?; + } + Ok(()) +} +#[derive(Clone, Copy)] +struct MotionBounds { + rate: Vector3, + acceleration: Vector3, + rate_error: f32, + acceleration_error: f32, + rate_bound: f32, + force_bound: f32, +} + +fn motion_at( + f: &Filter, + history: &[Option; 32], + len: usize, + config: NavigationConfig, +) -> Result { + let current = history[..len] + .iter() + .flatten() + .find(|sample| sample.context.measurement_time.as_microseconds() >= f.time) + .copied() + .or_else(|| history[..len].last().copied().flatten()) + .ok_or(NavigationFailure::ImuGap)?; + let width = u64::from(config.maximum_time_uncertainty_microseconds); + let low = f.time.saturating_sub(width); + let high = f.time.saturating_add(width); + let mut rate_jump: f32 = 0.0; + let mut force_jump: f32 = 0.0; + let mut minimum_duration = current.acquisition_duration_us; + for sample in &history[..len] { + let sample = sample.ok_or(NavigationFailure::ImuGap)?; + let end = sample + .context + .measurement_time + .as_microseconds() + .saturating_add(width); + let start = sample + .acquisition_start() + .ok_or(NavigationFailure::ImuGap)? + .saturating_sub(width); + if start <= high && end >= low { + minimum_duration = minimum_duration.min(sample.acquisition_duration_us); + rate_jump = rate_jump.max( + sample + .angular_rate + .vector() + .sub(current.angular_rate.vector()) + .norm(), + ); + force_jump = force_jump.max( + sample + .acceleration + .vector() + .sub(current.acceleration.vector()) + .norm(), + ); + } + } + let rate = current.angular_rate.vector().sub(f.bg); + let force = current.acceleration.vector().sub(f.ba); + let acceleration = + f.q.body_to_ned(force) + .add(Vector3::new(0.0, 0.0, GRAVITY_MPS2)); + let trace = |start: usize| (start..start + 3).map(|i| f.covariance[i][i]).sum::(); + // The clock error is hard-bounded. The sensor/state terms here are the + // declared three-sigma confidence envelope, not a Gaussian clock error. + let rate_error = rate_jump + + 3.0 * sqrt(trace(9)) + + f.timing.norm_bound(9, 3) + + 3.0 * sqrt(3.0 * config.noise.gyro / (minimum_duration as f32 * 1e-6)); + let angle_error = 3.0 * sqrt(trace(6)) + f.timing.norm_bound(6, 3); + let force_error = force_jump + + 3.0 * sqrt(trace(12)) + + f.timing.norm_bound(12, 3) + + 3.0 * sqrt(3.0 * config.noise.acceleration / (minimum_duration as f32 * 1e-6)); + let rate_bound = rate.norm() + rate_error; + let force_bound = force.norm() + force_error; + let acceleration_error = force_error + force_bound * angle_error; + if !rate_bound.is_finite() || !force_bound.is_finite() || !acceleration_error.is_finite() { + return Err(NavigationFailure::Numerical); + } + Ok(MotionBounds { + rate, + acceleration, + rate_error, + acceleration_error, + rate_bound, + force_bound, + }) +} + +#[cfg(test)] +fn fuse( + f: &mut Filter, + r: SensorRecord, + c: NavigationConfig, + sample: FlightImuObservation, +) -> Result { + let mut history = [None; 32]; + history[0] = Some(sample); + let motion = motion_at(f, &history, 1, c)?; + fuse_with_motion(f, r, c, motion) +} + +fn fuse_with_motion( + f: &mut Filter, + r: SensorRecord, + c: NavigationConfig, + motion: MotionBounds, +) -> Result { + let u = r.context().time_uncertainty_microseconds as f32 * 1e-6; + let mut generator = [0.0; N]; + let velocity_error = + 3.0 * sqrt((3..6).map(|i| f.covariance[i][i]).sum()) + f.timing.norm_bound(3, 3); + let acceleration_bound = motion.acceleration.norm() + motion.acceleration_error; + let position_remainder = velocity_error * u + 0.5 * acceleration_bound * u * u; + let velocity_remainder = + motion.acceleration_error * u + 0.5 * motion.force_bound * motion.rate_bound * u * u; + match r { + SensorRecord::LocalPositionVelocity(v) => { + for axis in 0..6 { + let mut h = [0.; N]; + h[axis] = 1.; + let (value, predicted, var) = if axis < 3 { + ( + components(v.position.vector())[axis], + components(f.p)[axis], + v.position_variance[axis].get(), + ) + } else { + ( + components(v.velocity.vector())[axis - 3], + components(f.v)[axis - 3], + v.velocity_variance[axis - 3].get(), + ) + }; + let (derivative, remainder) = if axis < 3 { + (components(f.v)[axis], position_remainder) + } else { + ( + components(motion.acceleration)[axis - 3], + velocity_remainder, + ) + }; + if !f.scalar_timed( + h, + value - predicted, + var, + derivative * u, + remainder, + &mut generator, + )? { + return Ok(false); + } + } + } + SensorRecord::Pressure(v) => { + let (z, derivative) = pressure_height(v.pascals, c.installation.atmosphere) + .ok_or(NavigationFailure::Numerical)?; + let mut h = [0.; N]; + h[2] = -1.; + h[15] = 1.; + let height_derivative = -f.v.z(); + let datum_walk = 3.0 * sqrt(c.noise.barometric_datum * u); + if !f.scalar_timed( + h, + z - (-f.p.z() + f.datum), + derivative * derivative * v.variance_pascals_squared.get(), + height_derivative * u, + position_remainder + datum_walk, + &mut generator, + )? { + return Ok(false); + } + } + SensorRecord::Magnetic(v) => { + for axis in 0..3 { + let r = rotation(f.q); + let s = skew(components(c.installation.magnetic_field_ned_microtesla)); + let mut h = [0.; N]; + for j in 0..3 { + for k in 0..3 { + h[j + 6] += r[k][axis] * s[k][j]; + } + } + let residual = components(v.microtesla_frd)[axis] + - components(f.q.ned_to_body(c.installation.magnetic_field_ned_microtesla)) + [axis]; + let field_body = + f.q.ned_to_body(c.installation.magnetic_field_ned_microtesla); + let derivative = motion.rate.cross(field_body).scale(-1.0); + let field_norm = c.installation.magnetic_field_ned_microtesla.norm(); + let angle_error = 3.0 * sqrt((6..9).map(|i| f.covariance[i][i]).sum()) + + f.timing.norm_bound(6, 3); + let remainder = + field_norm * (motion.rate_error + motion.rate_bound * angle_error) * u + + 0.5 * field_norm * motion.rate_bound * motion.rate_bound * u * u; + if !f.scalar_timed( + h, + residual, + v.variance_microtesla_squared[axis].get(), + components(derivative)[axis] * u, + remainder, + &mut generator, + )? { + return Ok(false); + } + } + } + _ => return Ok(false), + } + f.timing.add_generator(generator)?; + Ok(true) +} +#[cfg(test)] +fn clearance(f: Filter, r: RangeObservation) -> Option { + clearance_with_rate( + f, + r, + 20.0, + NavigationConfig::engineering_fixture().installation, + ) +} +fn clearance_with_rate( + f: Filter, + r: RangeObservation, + rate_bound: f32, + installation: NavigationInstallation, +) -> Option { + let sensor = installation.range; + let ray = f.q.body_to_ned(sensor.direction_frd); + if ray.z() < 0.8 { + return None; + } + let lever = f.q.body_to_ned(sensor.offset_frd_metres); + let feet = installation.contact.depth(f.q); + let height = lever.z() + r.metres * ray.z(); + let attitude_variance = f.covariance[6][6] + f.covariance[7][7] + f.covariance[8][8]; + let attitude_error = 3. * sqrt(attitude_variance) + + f.timing.norm_bound(6, 3) + + rate_bound * r.context.time_uncertainty_microseconds as f32 * 1e-6; + // Quantization is a deterministic bound, separate from observation variance. + let upper = 3. * sqrt(r.variance.get()) + + sensor.quantization_error_metres + + attitude_error * (sensor.lever_bound() + r.metres); + let contact = height - feet; + if !height.is_finite() || !contact.is_finite() || !upper.is_finite() || contact < -0.02 { + return None; + } + Some(GroundClearance { + measurement_at: r.context.measurement_time, + valid_until: expiry(r.context, 60_000)?, + source: r.context.source, + body_origin_height_metres: height, + contact_clearance_metres: contact, + variance_m2: r.variance.get(), + measurement_time_uncertainty_microseconds: r.context.time_uncertainty_microseconds, + upper_error_metres: upper, + }) +} + +#[cfg(test)] +mod tests { + use super::*; + fn context(t: u64, kind: u8, frame: FrameId) -> ObservationContext { + ObservationContext { + measurement_time: Timestamp::from_microseconds(t), + receipt_time: Timestamp::from_microseconds(t), + source: SourceId::new(kind), + frame, + calibration: CalibrationRevision::new(1), + quality: 255, + validity: Validity::Valid, + sequence: ObservationSequence::new((t / 10_000) as u32), + time_uncertainty_microseconds: 0, + } + } + + fn stationary_records(t: u64) -> [Option; 5] { + let stamp = |measured, kind, frame| { + let mut c = context(measured, kind, frame); + c.receipt_time = Timestamp::from_microseconds(t + 2_000); + c.time_uncertainty_microseconds = 2_000; + c + }; + [ + Some(SensorRecord::Imu(FlightImuObservation { + acquisition_duration_us: 10_000, + context: stamp(t, 1, FrameId::BODY_FRD), + acceleration: AccelerationFrd::metres_per_second_squared(0.0, 0.0, -GRAVITY_MPS2), + angular_rate: AngularRateFrd::radians_per_second(0.0, 0.0, 0.0), + })), + if t >= 80_000 && t % 50_000 == 30_000 { + Some(SensorRecord::LocalPositionVelocity( + LocalPositionVelocityObservation { + context: stamp(t - 30_000, 2, FrameId::LOCAL_NED), + position: PositionNed::metres(12.0, -7.0, 4.0), + velocity: VelocityNed::metres_per_second(0.0, 0.0, 0.0), + position_variance: [ScalarVariance::new(0.01).unwrap(); 3], + velocity_variance: [ScalarVariance::new(0.0025).unwrap(); 3], + reference: SensorReferenceId(1), + }, + )) + } else { + None + }, + t.is_multiple_of(50_000).then(|| { + SensorRecord::Magnetic(MagneticObservation { + context: stamp(t, 3, FrameId::BODY_FRD), + microtesla_frd: Vector3::new(20.0, 0.0, 45.0), + variance_microtesla_squared: [ScalarVariance::new(0.0004).unwrap(); 3], + }) + }), + t.is_multiple_of(50_000).then(|| { + SensorRecord::Pressure(PressureObservation { + context: stamp(t, 4, FrameId::SCALAR), + pascals: 101_325.0, + variance_pascals_squared: ScalarVariance::new(0.09).unwrap(), + }) + }), + t.is_multiple_of(20_000).then(|| { + SensorRecord::Range(RangeObservation { + context: stamp(t, 5, FrameId::new(3)), + metres: 0.04, + variance: ScalarVariance::new(4e-6).unwrap(), + }) + }), + ] + } + + #[test] + fn configured_installation_initializes() { + let mut config = NavigationConfig::engineering_fixture(); + config.installation.sources = NavigationSources { + imu: SourceId::new(11), + local_position_velocity: SourceId::new(22), + magnetic: SourceId::new(33), + pressure: SourceId::new(44), + range: SourceId::new(55), + }; + config.installation.calibration = CalibrationRevision::new(17); + config.installation.reference = SensorReferenceId(91); + config.installation.magnetic_field_ned_microtesla = Vector3::new(22.0, 7.0, 43.0); + config.installation.atmosphere.pressure_pascals = 80_000.0; + config.installation.range.frame = FrameId::new(9); + config.installation.range.offset_frd_metres = Vector3::new(0.02, -0.01, 0.06); + config.installation.range.direction_frd = Vector3::new(0.0, 0.0, 2.0); + config.installation.contact.centre_z_metres = 0.10; + let installation = config.installation; + let mut nav = Navigation::new(config).unwrap(); + for t in (10_000..=4_000_000).step_by(10_000) { + let now = Timestamp::from_microseconds(t + 2_000); + for mut record in stationary_records(t).into_iter().flatten() { + let kind = usize::from(record.kind() - 1); + let context = match &mut record { + SensorRecord::Imu(v) => &mut v.context, + SensorRecord::LocalPositionVelocity(v) => { + v.reference = installation.reference; + &mut v.context + } + SensorRecord::Magnetic(v) => { + v.microtesla_frd = Vector3::new(22.0, 7.0, 43.0); + &mut v.context + } + SensorRecord::Pressure(v) => { + v.pascals = 80_000.0; + &mut v.context + } + SensorRecord::Range(v) => { + v.context.frame = installation.range.frame; + // Contact at 0.115 m body height, sensor origin at 0.06 m. + v.metres = 0.055; + &mut v.context + } + }; + context.source = installation.sources.ordered()[kind]; + context.calibration = installation.calibration; + assert_eq!( + nav.observe(record, now).unwrap(), + ObservationAdmission::Queued + ); + } + nav.tick(now).unwrap(); + } + let e = nav.estimate().unwrap(); + assert!(e.capabilities.flight_ready()); + assert_eq!(e.origin.reference, installation.reference); + assert!(e.ground_clearance.unwrap().contact_clearance_metres.abs() < 1e-6); + assert_eq!( + nav.source_status().map(|s| s.source), + installation.sources.ordered() + ); + assert_eq!(e.last_imu_at, Timestamp::from_microseconds(4_000_000)); + assert_eq!(nav.source_status()[0].integrity, SourceStatus::Healthy); + assert!(nav.source_status()[1..].iter().all(|s| s.fused > 0)); + + let mut wrong = imu(4_010_000); + wrong.context.calibration = installation.calibration; + assert_eq!( + nav.observe(SensorRecord::Imu(wrong), wrong.context.receipt_time) + .unwrap(), + ObservationAdmission::Rejected(ObservationRejection::SourceMismatch) + ); + wrong.context.source = installation.sources.imu; + wrong.context.calibration = CalibrationRevision::new(1); + assert_eq!( + nav.observe(SensorRecord::Imu(wrong), wrong.context.receipt_time) + .unwrap(), + ObservationAdmission::Rejected(ObservationRejection::CalibrationMismatch) + ); + } + + #[test] + fn range_mount_geometry() { + let baseline = warm().filter.unwrap(); + let mut installation = NavigationConfig::engineering_fixture().installation; + installation.range.offset_frd_metres = Vector3::new(0.03, -0.02, 0.07); + installation.range.direction_frd = Vector3::new(0.1, 0.2, 0.98).normalized().unwrap(); + installation.range.quantization_error_metres = 0.002; + installation.contact = ContactGeometry { + half_span_x_metres: 0.3, + half_span_y_metres: 0.2, + centre_z_metres: 0.15, + radius_metres: 0.025, + }; + for (roll, pitch) in [(-0.3_f64, 0.2_f64), (0.0, 0.0), (0.2, -0.3)] { + let (sr, cr) = ((roll / 2.0).sin(), (roll / 2.0).cos()); + let (sp, cp) = ((pitch / 2.0).sin(), (pitch / 2.0).cos()); + let mut f = baseline; + f.q = UnitQuaternion::from_components( + (cp * cr) as f32, + (cp * sr) as f32, + (sp * cr) as f32, + (-sp * sr) as f32, + ) + .unwrap(); + let r = RangeObservation { + context: context(f.time, 5, installation.range.frame), + metres: 1.25, + variance: ScalarVariance::new(4e-6).unwrap(), + }; + let clearance = clearance_with_rate(f, r, 0.0, installation).unwrap(); + let offset = installation.range.offset_frd_metres; + let beam = installation.range.direction_frd; + let z_row = [ + -pitch.sin(), + pitch.cos() * roll.sin(), + pitch.cos() * roll.cos(), + ]; + let ray_z = f64::from(beam.x()) * z_row[0] + + f64::from(beam.y()) * z_row[1] + + f64::from(beam.z()) * z_row[2]; + let height = f64::from(offset.x()) * z_row[0] + + f64::from(offset.y()) * z_row[1] + + f64::from(offset.z()) * z_row[2] + + f64::from(r.metres) * ray_z; + let feet = f64::from(installation.contact.half_span_x_metres) * z_row[0].abs() + + f64::from(installation.contact.half_span_y_metres) * z_row[1].abs() + + f64::from(installation.contact.centre_z_metres) * z_row[2] + + f64::from(installation.contact.radius_metres); + assert!((f64::from(clearance.body_origin_height_metres) - height).abs() < 2e-7); + assert!((f64::from(clearance.contact_clearance_metres) - (height - feet)).abs() < 2e-7); + let mut coarser = installation; + coarser.range.quantization_error_metres += 0.01; + let coarse = clearance_with_rate(f, r, 0.0, coarser).unwrap(); + assert!((coarse.upper_error_metres - clearance.upper_error_metres - 0.01).abs() < 1e-7); + } + // These finite installations overflow rotation intermediates even + // though their mathematical projections would be representable. + let mut f = baseline; + f.q = UnitQuaternion::from_components(1.0, 0.0, 1.0, 0.0).unwrap(); + installation.range.direction_frd = Vector3::new(-1.0, 0.0, 0.0); + let r = RangeObservation { + context: context(f.time, 5, installation.range.frame), + metres: 1.25, + variance: ScalarVariance::new(4e-6).unwrap(), + }; + for huge_offset in [true, false] { + let mut candidate = installation; + if huge_offset { + candidate.range.offset_frd_metres = Vector3::new(3e38, 0.0, 0.0); + } else { + candidate.contact.half_span_x_metres = 3e38; + } + assert!( + NavigationConfig { + installation: candidate, + ..NavigationConfig::engineering_fixture() + } + .validate() + ); + assert!(clearance_with_rate(f, r, 0.0, candidate).is_none()); + } + } + + #[test] + fn public_sensor_pipeline_handles_delay_reordering_and_committed_horizon() { + let mut forward = Navigation::new(NavigationConfig::engineering_fixture()).unwrap(); + let mut reordered = Navigation::new(NavigationConfig::engineering_fixture()).unwrap(); + for t in (10_000..=4_200_000).step_by(10_000) { + let now = Timestamp::from_microseconds(t + 2_000); + let records = stationary_records(t); + for (index, nav) in [&mut forward, &mut reordered].into_iter().enumerate() { + // Establish the IMU window first; then vary cross-source arrival + // order without changing any sample, receipt or processing time. + let mut ordered = records; + if index == 1 { + ordered[1..].reverse(); + } + for record in ordered.into_iter().flatten() { + if t > 4_000_000 && record.kind() == 2 { + continue; + } + assert_eq!( + nav.observe(record, now).unwrap(), + ObservationAdmission::Queued + ); + } + nav.tick(now).unwrap(); + } + if t == 4_000_000 { + let e = forward.estimate().unwrap(); + assert!(e.capabilities.flight_ready()); + assert_eq!(e.at, now); + assert!(e.fused_through.as_microseconds() <= t + 2_000 - 100_000); + assert!(e.position.vector().norm() < 0.001); + assert!(e.ground_clearance.unwrap().contact_clearance_metres.abs() < 0.001); + assert_eq!(forward.estimate(), reordered.estimate()); + } + } + assert_eq!(forward.estimate(), reordered.estimate()); + let watermark = forward.stamps[1]; + let mut late = local(4_000_000); + late.context.receipt_time = Timestamp::from_microseconds(4_202_000); + assert_eq!( + forward + .observe( + SensorRecord::LocalPositionVelocity(late), + late.context.receipt_time + ) + .unwrap(), + ObservationAdmission::Rejected(ObservationRejection::BehindFusionHorizon) + ); + assert_eq!(forward.stamps[1], watermark); + assert!(forward.estimate().is_some()); + } + + fn imu(t: u64) -> FlightImuObservation { + FlightImuObservation { + acquisition_duration_us: 10_000, + context: context(t, 1, FrameId::BODY_FRD), + acceleration: AccelerationFrd::metres_per_second_squared(0.0, 0.0, -GRAVITY_MPS2), + angular_rate: AngularRateFrd::radians_per_second(0.0, 0.0, 0.0), + } + } + fn local(t: u64) -> LocalPositionVelocityObservation { + LocalPositionVelocityObservation { + context: context(t, 2, FrameId::LOCAL_NED), + position: PositionNed::metres(12.0, -7.0, 4.0), + velocity: VelocityNed::metres_per_second(0.0, 0.0, 0.0), + position_variance: [ScalarVariance::new(0.01).unwrap(); 3], + velocity_variance: [ScalarVariance::new(0.0025).unwrap(); 3], + reference: SensorReferenceId(1), + } + } + fn warm() -> Navigation { + let mut n = Navigation::new(NavigationConfig::engineering_fixture()).unwrap(); + for t in (10_000..=4_000_000).step_by(10_000) { + let now = Timestamp::from_microseconds(t + 2_000); + for r in stationary_records(t).into_iter().flatten() { + n.observe(r, now).unwrap(); + } + n.tick(now).unwrap(); + } + assert!(n.estimate().unwrap().capabilities.flight_ready()); + n + } + fn initialization_window() -> (Initializer, FlightImuObservation) { + let config = NavigationConfig::engineering_fixture(); + let mut init = Initializer::new(); + for t in (10_000..=2_030_000).step_by(10_000) { + for r in stationary_records(t).into_iter().flatten() { + init.push(r, config); + } + } + (init, imu(2_030_000)) + } + fn boundary_fixture() -> (Filter, [Option; 32]) { + let mut records = [None; 32]; + for (i, slot) in records[..3].iter_mut().enumerate() { + let mut r = imu((i as u64 + 1) * 10_000); + r.context.time_uncertainty_microseconds = 2_000; + if i == 2 { + r.angular_rate = AngularRateFrd::radians_per_second(0.0, 1.0, 0.0); + r.acceleration = + AccelerationFrd::metres_per_second_squared(0.0, 1.0, -GRAVITY_MPS2); + } + *slot = Some(r); + } + let mut covariance = ZERO; + for (i, row) in covariance.iter_mut().enumerate() { + row[i] = 1e-12; + } + ( + Filter { + time: 10_000, + p: Vector3::zero(), + v: Vector3::zero(), + q: UnitQuaternion::identity(), + bg: Vector3::zero(), + ba: Vector3::zero(), + datum: 0.0, + covariance, + timing: crate::timing::TimingEnvelope::ZERO, + }, + records, + ) + } + #[test] + fn end_stamped_windows_split_at_aiding_without_a_whole_interval_shift() { + let (mut f, mut records) = boundary_fixture(); + for record in records.iter_mut().flatten() { + record.context.time_uncertainty_microseconds = 0; + } + let config = NavigationConfig::engineering_fixture(); + for end in [20_000, 25_000, 30_000, 35_000, 50_000] { + predict(&mut f, end, &records, 3, config).unwrap(); + let elapsed = (end - 20_000) as f64 * 1e-6; + // Pitch leaves the body-Y force unchanged. Independent constant + // acceleration and rate answers include an interior aiding split. + assert!((f64::from(f.v.y()) - elapsed).abs() < 1e-8); + assert!((f64::from(f.p.y()) - 0.5 * elapsed * elapsed).abs() < 1e-9); + assert!((2.0 * f64::from(f.q.y()).atan2(f64::from(f.q.w())) - elapsed).abs() < 1e-8); + assert_eq!(f.timing.radius(7), 0.0); + } + assert_eq!( + predict(&mut f, 50_001, &records, 3, config), + Err(NavigationFailure::ImuGap) + ); + let (mut missing, records) = boundary_fixture(); + let mut gap = records; + gap[1] = records[2]; + gap[2] = None; + assert_eq!( + predict(&mut missing, 25_000, &gap, 2, config), + Err(NavigationFailure::ImuGap) + ); + assert_eq!(missing.time, 10_000); + } + #[test] + fn shifted_acquisition_boundaries_enclose_before_at_and_after_for_both_signs() { + let config = NavigationConfig::engineering_fixture(); + for duration in [9_700_u64, 10_000, 10_300, 20_000] { + let (mut initial, mut records) = boundary_fixture(); + for (i, r) in records[..3].iter_mut().flatten().enumerate() { + r.acquisition_duration_us = duration as u32; + r.context.measurement_time = + Timestamp::from_microseconds((i as u64 + 1) * duration); + } + initial.time = duration; + let boundary = 2 * duration; + for offset in [-3_000_i64, -2_000, -1_000, 0, 1_000, 2_000, 3_000] { + let end = (boundary as i64 + offset) as u64; + // Repeated splits exercise contributions on both sides of the + // nominal boundary, including exact-time aiding splits. + let mut f = initial; + for split in [ + boundary - 2_000, + boundary - 1_000, + boundary, + boundary + 1_000, + boundary + 2_000, + end, + ] { + if split > f.time && split <= end { + predict(&mut f, split, &records, 3, config).unwrap(); + } + } + for shift in [-2_000_i64, 2_000] { + // Independent integrated step responses of the frozen left-NED + // system. The gravity term checks attitude -> velocity -> position. + let actual = (end as i64 - (boundary as i64 + shift)).max(0) as f64 * 1e-6; + let nominal = (end as i64 - boundary as i64).max(0) as f64 * 1e-6; + let rate = actual - nominal; + let velocity = (actual.powi(2) - nominal.powi(2)) / 2.0; + let position = (actual.powi(3) - nominal.powi(3)) / 6.0; + for (axis, error) in [ + (7, rate), + (4, rate), + (1, velocity), + (3, -f64::from(GRAVITY_MPS2) * velocity), + (0, -f64::from(GRAVITY_MPS2) * position), + ] { + assert!(error.abs() <= f64::from(f.timing.radius(axis)) + 1e-12); + } + if offset == -1_000 && shift == -2_000 { + assert!((rate - 0.001).abs() < 1e-15); + assert!(f.timing.radius(7) >= 0.001); + } + } + } + } + } + #[test] + fn exact_time_aiding_sees_uncertain_imu_window_before_its_nominal_boundary() { + let (mut f, records) = boundary_fixture(); + // A low-white-noise known-answer case isolates the timing contribution + // to admission. The engineering fixture and flight thresholds are fixed. + let mut config = NavigationConfig::engineering_fixture(); + config.noise.acceleration = 1e-12; + config.noise.gyro = 1e-12; + predict(&mut f, 19_000, &records, 3, config).unwrap(); + let mut r = local(19_000); + r.position = PositionNed::metres(0.0, 0.5e-6, 0.0); + r.velocity = VelocityNed::metres_per_second(-0.5 * GRAVITY_MPS2 * 1e-6, 0.001, 0.0); + r.position_variance = [ScalarVariance::new(1e-12).unwrap(); 3]; + r.velocity_variance = [ScalarVariance::new(1e-12).unwrap(); 3]; + assert_eq!(r.context.time_uncertainty_microseconds, 0); + let mut without = f; + without.timing = crate::timing::TimingEnvelope::ZERO; + assert!( + !fuse( + &mut without, + SensorRecord::LocalPositionVelocity(r), + config, + records[1].unwrap() + ) + .unwrap() + ); + assert!( + fuse( + &mut f, + SensorRecord::LocalPositionVelocity(r), + config, + records[1].unwrap() + ) + .unwrap() + ); + assert!(f.timing.radius(7) >= 0.001); + } + #[test] + fn complete_window_coverage_is_required_at_startup_and_after_initialization() { + let (init, last) = initialization_window(); + assert_eq!(init.start, Some(0)); + assert_eq!(init.accel.n, 203); + assert!( + init.finish(imu(2_020_000), NavigationConfig::engineering_fixture()) + .is_none() + ); + assert!( + init.finish(last, NavigationConfig::engineering_fixture()) + .is_some() + ); + let mut n = Navigation::new(NavigationConfig::engineering_fixture()).unwrap(); + assert_eq!( + n.observe( + SensorRecord::Imu(imu(9_999)), + Timestamp::from_microseconds(9_999) + ), + Ok(ObservationAdmission::Rejected( + ObservationRejection::InvalidTime + )) + ); + let mut n = warm(); + assert_eq!( + n.observe( + SensorRecord::Imu(imu(4_020_000)), + Timestamp::from_microseconds(4_020_000) + ), + Err(NavigationFailure::ImuGap) + ); + assert!(n.estimate().is_none()); + } + #[test] + fn mapped_window_durations_cover_drift_and_integer_clock_phases() { + // Independent integer-floor endpoint mapping. Both source endpoints are + // mapped; received/processing instants remain independently later times. + for ppm in [-30_000_i64, -1000, -1, 1, 1000, 30_000] { + for phase in [0_u64, 137, 9_999] { + let map = |t: u64| { + 1_000_000 + + ((u128::from(t + phase) * (1_000_000 + ppm) as u128) / 1_000_000) as u64 + }; + let mut n = Navigation::new(NavigationConfig::engineering_fixture()).unwrap(); + let mut initialized_at = None; + for source_end in (10_000..=4_000_000).step_by(10_000) { + let receipt = Timestamp::from_microseconds(map(source_end + 5_000)); + for mut record in stationary_records(source_end).into_iter().flatten() { + let c = match &mut record { + SensorRecord::Imu(v) => { + let end = v.context.measurement_time.as_microseconds(); + v.acquisition_duration_us = (map(end) - map(end - 10_000)) as u32; + &mut v.context + } + SensorRecord::LocalPositionVelocity(v) => &mut v.context, + SensorRecord::Magnetic(v) => &mut v.context, + SensorRecord::Pressure(v) => &mut v.context, + SensorRecord::Range(v) => &mut v.context, + }; + c.measurement_time = + Timestamp::from_microseconds(map(c.measurement_time.as_microseconds())); + c.receipt_time = receipt; + // Drift can move initialization past the next delayed + // position measurement; that history remains inadmissible. + let expected = if record.kind() == 2 + && initialized_at.is_some_and(|at| { + record.context().measurement_time.as_microseconds() <= at + }) { + ObservationAdmission::Rejected( + ObservationRejection::BehindFusionHorizon, + ) + } else { + ObservationAdmission::Queued + }; + let watermark = n.stamps[1]; + assert_eq!(n.observe(record, receipt).unwrap(), expected); + if expected != ObservationAdmission::Queued { + assert_eq!(n.stamps[1], watermark); + } + } + n.tick(receipt).unwrap(); + if initialized_at.is_none() { + initialized_at = n.filter.map(|f| f.time); + } + } + let e = n.estimate().unwrap(); + assert_eq!(e.last_imu_at, Timestamp::from_microseconds(map(4_000_000))); + assert!(e.capabilities.flight_ready()); + assert!(e.position.vector().norm() < 0.001); + } + } + } + #[test] + fn duration_weighted_window_mean_and_white_noise_use_actual_exposure() { + let mut moments = Moments::<1>::EMPTY; + moments.push_weighted([1.0], [0.0], 9_700.0); + moments.push_weighted([-2.0], [0.0], 10_300.0); + let mean = (9700.0_f64 - 2.0 * 10300.0) / 20000.0; + // Division, multiplication and addition in weighted Welford are f32. + let roundoff = 4.0 * f64::from(f32::EPSILON) * 3.0; + assert!((f64::from(moments.mean[0]) - mean).abs() <= roundoff); + let mut white = Moments::<1>::EMPTY; + let mut total = 0_u32; + for duration in [9700_u32, 10300, 5000, 20000] { + white.push_weighted([0.0], [2.5e-5 / (duration as f32 * 1e-6)], duration as f32); + total += duration; + let expected = 2.5e-5 / (f64::from(total) * 1e-6); + assert!( + (f64::from(white.mean_variance(0)) - expected).abs() + <= 16.0 * f64::from(f32::EPSILON) * expected + ); + } + } + #[test] + fn native_intervals_preserve_startup_and_work_limits() { + let mut slow = Navigation::new(NavigationConfig::engineering_fixture()).unwrap(); + for t in (10_000..=5_000_000).step_by(10_000) { + let now = Timestamp::from_microseconds(t + 2_000); + for mut record in stationary_records(t).into_iter().flatten() { + if let SensorRecord::Imu(v) = &mut record { + if !t.is_multiple_of(20_000) { + continue; + } + v.acquisition_duration_us = 20_000; + } + assert_eq!(slow.observe(record, now), Ok(ObservationAdmission::Queued)); + } + slow.tick(now).unwrap(); + assert!(slow.estimate().is_none()); + assert!(slow.initializer.accel.n < 200); + } + let mut fast = warm(); + let mut failure = None; + for i in 1..=100 { + let mut r = imu(4_000_000 + i * 1_000); + r.acquisition_duration_us = 1_000; + r.context.sequence = ObservationSequence::new(400 + i as u32); + let now = + Timestamp::from_microseconds(r.context.measurement_time.as_microseconds() + 2_000); + let result = fast + .observe(SensorRecord::Imu(r), now) + .and_then(|admission| { + assert_eq!(admission, ObservationAdmission::Queued); + fast.tick(now) + }); + if let Err(error) = result { + failure = Some(error); + break; + } + } + assert!(matches!( + failure, + Some(NavigationFailure::QueueOverrun | NavigationFailure::WorkBudget) + )); + assert!(fast.estimate().is_none()); + } + #[test] + fn invalid_duration_or_consumed_bad_records_cannot_supply_imu_coverage() { + for (maximum, end, duration) in [ + (20_000, 40_000, 0), + (20_000, 40_000, 20_001), + (20_000, 40_000, u32::MAX), + (10_000, 40_000, 10_001), + (20_000, 9_999, 10_000), + ] { + let mut config = NavigationConfig::engineering_fixture(); + config.maximum_imu_interval_microseconds = maximum; + let mut n = Navigation::new(config).unwrap(); + let mut r = imu(end); + r.acquisition_duration_us = duration; + assert_eq!( + n.observe(SensorRecord::Imu(r), r.context.receipt_time), + Ok(ObservationAdmission::Rejected( + ObservationRejection::InvalidTime + )) + ); + assert_eq!(n.stamps[0], None); + assert_eq!(n.used[0], None); + assert_eq!(n.admitted_imu_coverage(), None); + } + for duration in [1, 9_989, 10_011, 20_000] { + let mut n = Navigation::new(NavigationConfig::engineering_fixture()).unwrap(); + let mut r = imu(40_000); + r.acquisition_duration_us = duration; + assert_eq!( + n.observe(SensorRecord::Imu(r), r.context.receipt_time), + Ok(ObservationAdmission::Queued) + ); + assert_eq!( + n.admitted_imu_coverage().unwrap().acquisition_duration_us, + duration + ); + } + for bad_quality in [false, true] { + let mut n = warm(); + let previous = n.used[0]; + let coverage = n.admitted_imu_coverage(); + let estimate = n.estimate(); + let mut bad = imu(4_009_000); + bad.context.sequence = ObservationSequence::new(401); + let reason = if bad_quality { + bad.context.quality = 0; + ObservationRejection::InvalidQuality + } else { + bad.acceleration = AccelerationFrd::metres_per_second_squared(f32::NAN, 0.0, 0.0); + ObservationRejection::InvalidValue + }; + assert_eq!( + n.observe(SensorRecord::Imu(bad), bad.context.receipt_time), + Ok(ObservationAdmission::Rejected(reason)) + ); + assert_eq!(n.used[0], previous); + assert_eq!(n.admitted_imu_coverage(), coverage); + assert_eq!(n.stamps[0], Some(bad.context)); + let mut valid = imu(4_010_000); + valid.context.sequence = ObservationSequence::new(402); + assert_eq!( + n.observe(SensorRecord::Imu(valid), valid.context.receipt_time), + Ok(ObservationAdmission::Queued) + ); + assert_eq!(n.used[0], Some(valid.context)); + assert_eq!( + n.admitted_imu_coverage(), + Some(ImuCoverage { + end: valid.context.measurement_time, + acquisition_duration_us: valid.acquisition_duration_us, + time_uncertainty_microseconds: valid.context.time_uncertainty_microseconds, + }) + ); + assert_eq!(n.estimate(), estimate); // admission timing is not prediction evidence + + let mut n = warm(); + bad.context.measurement_time = Timestamp::from_microseconds(4_010_000); + bad.context.receipt_time = bad.context.measurement_time; + n.observe(SensorRecord::Imu(bad), bad.context.receipt_time) + .unwrap(); + let valid = imu(4_020_000); + assert_eq!( + n.observe(SensorRecord::Imu(valid), valid.context.receipt_time), + Err(NavigationFailure::ImuGap) + ); + assert!(n.estimate().is_none()); + } + } + #[test] + fn endpoint_gyro_bias_prior_includes_window_random_walk() { + let (init, last) = initialization_window(); + let mut config = NavigationConfig::engineering_fixture(); + config.noise.gyro_bias = 1e-3; + let (f, _) = init.finish(last, config).unwrap(); + let duration = + (last.context.measurement_time.as_microseconds() - init.start.unwrap()) as f64 * 1e-6; + // Independent integration of the endpoint-minus-uniform-window-mean + // Brownian kernel (s/T)^2, using midpoint quadrature. + let steps = 10_000; + let integral = (0..steps) + .map(|i| { + let s = (i as f64 + 0.5) * duration / steps as f64; + (s / duration).powi(2) * duration / steps as f64 + }) + .sum::(); + for axis in 0..3 { + let expected = f64::from(init.gyro.mean_variance(axis)) + + 1e-8 + + f64::from(config.noise.gyro_bias) * integral; + assert!(f64::from(f.covariance[axis + 9][axis + 9]) >= expected * (1.0 - 2e-6)); + assert!(f.covariance[axis + 9][axis + 9] > 0.0006); + } + assert!(f.valid()); + } + #[test] + fn persistent_two_millisecond_offset_during_rotation_is_inside_confidence() { + let mut n = Navigation::new(NavigationConfig::engineering_fixture()).unwrap(); + let start = 3_000_000; + for t in (10_000..=3_480_000).step_by(10_000) { + let now = Timestamp::from_microseconds(t + 2_000); + for mut record in stationary_records(t).into_iter().flatten() { + let actual = record.context().measurement_time.as_microseconds() + 2_000; + let angle = actual.saturating_sub(start) as f64 * 1e-6; + let c = angle.cos(); + let s = angle.sin(); + match &mut record { + SensorRecord::Imu(imu) => { + let begin = actual - u64::from(imu.acquisition_duration_us); + let duration = f64::from(imu.acquisition_duration_us) * 1e-6; + let first_angle = begin.saturating_sub(start) as f64 * 1e-6; + let active = angle - first_angle; + let average_sine = (first_angle.cos() - c) / duration; + let average_cosine = (s - first_angle.sin() + duration - active) / duration; + imu.angular_rate = AngularRateFrd::radians_per_second( + 0.0, + (active / duration) as f32, + 0.0, + ); + imu.acceleration = AccelerationFrd::metres_per_second_squared( + (f64::from(GRAVITY_MPS2) * average_sine) as f32, + 0.0, + (-f64::from(GRAVITY_MPS2) * average_cosine) as f32, + ); + } + SensorRecord::Magnetic(m) => { + m.microtesla_frd = Vector3::new( + (20.0 * c - 45.0 * s) as f32, + 0.0, + (20.0 * s + 45.0 * c) as f32, + ) + } + SensorRecord::Range(r) => r.metres = (0.5 / c - 0.04) as f32, + _ => {} + } + assert_eq!( + n.observe(record, now).unwrap(), + ObservationAdmission::Queued + ); + } + n.tick(now).unwrap(); + } + let e = n.estimate().unwrap(); + let true_angle = (e.at.as_microseconds() - start) as f64 * 1e-6; + let forward = e.attitude.body_to_ned(Vector3::new(1.0, 0.0, 0.0)); + let estimated = (-f64::from(forward.z())).atan2(f64::from(forward.x())); + let bias = (estimated - true_angle).abs() as f32; + let gaussian = 3.0 * sqrt(e.attitude_variance_rad2[1]); + assert!(bias > gaussian, "bias={bias} gaussian={gaussian}"); + assert!(bias <= gaussian + e.timing_error_bound.attitude_radians[1]); + assert!(e.timing_error_bound.attitude_radians[1] > 0.001); + assert!(e.capabilities.flight_ready()); + } + #[test] + fn magnetic_timing_derivative_changes_innovation_admission() { + let mut f = warm().filter.unwrap(); + f.covariance = ZERO; + for (i, row) in f.covariance.iter_mut().enumerate() { + row[i] = 1e-12; + } + f.timing = crate::timing::TimingEnvelope::ZERO; + let mut rate = imu(f.time); + rate.angular_rate = AngularRateFrd::radians_per_second(0.0, 1.0, 0.0); + let angle = 0.002_f64; + let field = Vector3::new( + (20.0 * angle.cos() - 45.0 * angle.sin()) as f32, + 0.0, + (20.0 * angle.sin() + 45.0 * angle.cos()) as f32, + ); + let displacement = field.sub(Vector3::new(20.0, 0.0, 45.0)).norm(); + assert!((displacement - 0.098_488_6).abs() < 1e-5); + let m = MagneticObservation { + context: context(f.time, 3, FrameId::BODY_FRD), + microtesla_frd: field, + variance_microtesla_squared: [ScalarVariance::new(0.0004).unwrap(); 3], + }; + let config = NavigationConfig::engineering_fixture(); + assert!(!fuse(&mut f, SensorRecord::Magnetic(m), config, rate).unwrap()); + let mut timed = m; + timed.context.time_uncertainty_microseconds = 2_000; + assert!(fuse(&mut f, SensorRecord::Magnetic(timed), config, rate).unwrap()); + assert!(f.timing.norm_bound(6, 3) > 0.0); + } + #[test] + fn initialization_requires_complete_stationary_consistent_sensor_evidence() { + let config = NavigationConfig::engineering_fixture(); + for field in 0..16 { + let (mut init, last) = initialization_window(); + match field { + 0 => init.accel.n = 199, + 1 => init.position.n = 39, + 2 => init.magnetic.n = 39, + 3 => init.pressure.n = 39, + 4 => init.range.n = 79, + 5 => init.accel.mean = [0.0; 3], + 6 => init.gyro.mean = [0.2, 0.0, 0.0], + 7 => init.velocity.mean = [1.0, 0.0, 0.0], + 8 => init.range.m2[0] = init.range.n as f32 * 0.001, + 9 => init.accel.m2[0] = init.accel.weight * 0.2, + 10 => init.gyro.m2[0] = init.gyro.weight * 0.003, + 11 => init.velocity.m2[0] = init.velocity.n as f32 * 0.02, + 12 => init.magnetic.m2[0] = init.magnetic.n as f32 * 0.3, + 13 => init.magnetic.mean = [0.0; 3], + 14 => init.position.noise[0] = f32::INFINITY, + _ => init.magnetic.mean = [0.0, 0.0, 45.0], + } + assert!(init.finish(last, config).is_none(), "case {field}"); + } + let mut init = Initializer::new(); + let pressure = SensorRecord::Pressure(PressureObservation { + context: context(50_000, 4, FrameId::SCALAR), + pascals: 101_325.0, + variance_pascals_squared: ScalarVariance::new(0.09).unwrap(), + }); + init.push(pressure, config); + assert_eq!(init.pressure.n, 0); + init.push(SensorRecord::Imu(imu(100_000)), config); + init.push(pressure, config); + assert_eq!(init.pressure.n, 0); + init.push(SensorRecord::Imu(imu(2_610_000)), config); + assert_eq!(init.accel.n, 1); + assert_eq!(init.start, Some(2_600_000)); // first complete window starts 10 ms earlier + init.push( + SensorRecord::Pressure(PressureObservation { + context: context(2_620_000, 4, FrameId::SCALAR), + pascals: 0.0, + variance_pascals_squared: ScalarVariance::new(0.09).unwrap(), + }), + config, + ); + assert_eq!(init.pressure.n, 0); + let mut n = Navigation::new(config).unwrap(); + n.tick(Timestamp::from_microseconds(0)).unwrap(); + n.observe( + SensorRecord::Imu(imu(100_000)), + Timestamp::from_microseconds(100_000), + ) + .unwrap(); + n.tick(Timestamp::from_microseconds(3_000_000)).unwrap(); + assert_eq!(n.initializer.start, None); + } + + #[test] + fn finite_but_uncertain_estimates_do_not_advertise_capabilities() { + for axis in [0, 1, 2, 3, 4, 5, 6, 7, 8] { + let mut n = warm(); + n.filter.as_mut().unwrap().covariance[axis][axis] += 1.0; + n.range = None; // availability is independently visible from covariance + n.tick(Timestamp::from_microseconds(4_002_000)).unwrap(); + let c = n.estimate().unwrap().capabilities; + match axis { + 0 | 1 => assert!(!c.horizontal_position.available), + 2 => assert!(!c.height.available), + 3 | 4 => assert!(!c.horizontal_velocity.available), + 5 => assert!(!c.vertical_velocity.available), + 6 | 7 => assert!(!c.tilt.available), + _ => assert!(!c.heading.available), + } + } + let mut n = warm(); + n.range = None; + n.clearance = None; + n.aid_len = 0; + n.tick(Timestamp::from_microseconds(4_002_000)).unwrap(); + assert!( + !n.estimate() + .unwrap() + .capabilities + .ground_clearance + .available + ); + n.tick(Timestamp::from_microseconds(4_030_000)).unwrap(); + assert!(!n.estimate().unwrap().capabilities.tilt.available); + } + + #[test] + fn queue_and_work_bounds_cover_each_owner() { + let mut n = warm(); + let mut overflow = None; + 'input: for t in (4_050_000..=4_650_000).step_by(50_000) { + let now = Timestamp::from_microseconds(t + 2_000); + let r = SensorRecord::LocalPositionVelocity(local(t)); + for r in [Some(r), stationary_records(t)[2], stationary_records(t)[3]] + .into_iter() + .flatten() + { + if let Err(e) = n.observe(r, now) { + overflow = Some(e); + break 'input; + } + } + } + assert_eq!(overflow, Some(NavigationFailure::QueueOverrun)); + let mut n = warm(); + let base = n.filter.unwrap().time; + n.aids = [None; 32]; + n.aid_len = 9; + // Exercise the service-work guard with a structurally valid queue. + for i in 0..9 { + n.aids[i] = Some(SensorRecord::LocalPositionVelocity(local( + base + i as u64 + 1, + ))); + } + assert_eq!( + n.tick(Timestamp::from_microseconds(4_012_000)), + Err(NavigationFailure::WorkBudget) + ); + let mut f = warm().filter.unwrap(); + f.time = 0; + let history = core::array::from_fn(|i| Some(imu((i as u64 + 1) * 10_000))); + assert_eq!( + predict( + &mut f, + 250_000, + &history, + 32, + NavigationConfig::engineering_fixture() + ), + Err(NavigationFailure::WorkBudget) + ); + assert_eq!( + predict( + &mut f, + 0, + &history, + 32, + NavigationConfig::engineering_fixture() + ), + Err(NavigationFailure::ImuGap) + ); + f.time = 0; + let mut one = [None; 32]; + one[0] = Some(imu(0)); + assert_eq!( + predict( + &mut f, + 20_001, + &one, + 1, + NavigationConfig::engineering_fixture() + ), + Err(NavigationFailure::ImuGap) + ); + } + + #[test] + fn rejected_aiding_and_range_geometry_do_not_refresh_measurement_evidence() { + let mut n = warm(); + let mut impossible = local(4_000_000); + impossible.position = PositionNed::metres(12.1, -7.0, 4.0); + impossible.velocity = VelocityNed::metres_per_second(0.0, 0.0, 20.0); + impossible.context.receipt_time = Timestamp::from_microseconds(4_030_000); + let previous = n.stamps[1]; + for t in (4_010_000..=4_100_000).step_by(10_000) { + let now = Timestamp::from_microseconds(t + 2_000); + n.observe(SensorRecord::Imu(imu(t)), now).unwrap(); + if t == 4_030_000 { + n.observe(SensorRecord::LocalPositionVelocity(impossible), now) + .unwrap(); + } + n.tick(now).unwrap(); + } + assert_eq!(n.used[1], previous); + assert!(n.estimate().unwrap().position.vector().x().abs() < 1e-6); + let config = NavigationConfig::engineering_fixture(); + let baseline = warm().filter.unwrap(); + for r in [ + SensorRecord::LocalPositionVelocity(impossible), + SensorRecord::Pressure(PressureObservation { + context: context(4_000_000, 4, FrameId::SCALAR), + pascals: 100_000.0, + variance_pascals_squared: ScalarVariance::new(0.09).unwrap(), + }), + SensorRecord::Magnetic(MagneticObservation { + context: context(4_000_000, 3, FrameId::BODY_FRD), + microtesla_frd: Vector3::new(-20.0, 0.0, -45.0), + variance_microtesla_squared: [ScalarVariance::new(0.0004).unwrap(); 3], + }), + SensorRecord::Imu(imu(4_000_000)), + ] { + let mut candidate = baseline; + assert!(!fuse(&mut candidate, r, config, imu(baseline.time)).unwrap()); + } + let range = RangeObservation { + context: context(4_000_000, 5, FrameId::new(3)), + metres: 0.04, + variance: ScalarVariance::new(4e-6).unwrap(), + }; + let mut tilted = baseline; + tilted.q = UnitQuaternion::from_components(1.0, 1.0, 0.0, 0.0).unwrap(); + assert!(clearance(tilted, range).is_none()); + let mut uncertain = baseline; + for axis in 6..9 { + uncertain.covariance[axis][axis] = f32::MAX; + } + assert!(clearance(uncertain, range).is_none()); + assert!( + clearance( + baseline, + RangeObservation { + metres: 0.01, + ..range + } + ) + .is_none() + ); + n.range = Some(RangeObservation { + context: context(n.filter.unwrap().time - 1, 5, FrameId::new(3)), + ..range + }); + n.tick(Timestamp::from_microseconds(4_102_000)).unwrap(); + n.range = Some(RangeObservation { + context: context(4_200_000, 5, FrameId::new(3)), + ..range + }); + n.tick(Timestamp::from_microseconds(4_102_000)).unwrap(); + n.range = Some(RangeObservation { + metres: 0.01, + ..range + }); + n.range_processed = None; + // An old but newly processed range is refused before it can refresh. + n.range.as_mut().unwrap().context.measurement_time = + Timestamp::from_microseconds(4_100_000); + n.tick(Timestamp::from_microseconds(4_102_000)).unwrap(); + } + #[test] + fn ingress_metadata_refusal_and_consumed_numeric_rejections_are_distinct() { + use ObservationRejection as R; + for (field, expected) in [ + (0, R::SourceMismatch), + (1, R::FrameMismatch), + (2, R::CalibrationMismatch), + (3, R::InvalidTime), + (4, R::InvalidTime), + (5, R::InvalidTime), + (6, R::InvalidTime), + ] { + let mut n = Navigation::new(NavigationConfig::engineering_fixture()).unwrap(); + let mut r = imu(10_000); + let now = Timestamp::from_microseconds(10_000); + match field { + 0 => r.context.source = SourceId::new(9), + 1 => r.context.frame = FrameId::LOCAL_NED, + 2 => r.context.calibration = CalibrationRevision::new(2), + 3 => r.context.time_uncertainty_microseconds = 2_001, + 4 => { + r.context.measurement_time = Timestamp::from_microseconds(0); + r.context.time_uncertainty_microseconds = 1; + } + 5 => { + r.context.measurement_time = Timestamp::from_microseconds(u64::MAX); + r.context.time_uncertainty_microseconds = 1; + } + _ => r.context.receipt_time = Timestamp::from_microseconds(10_001), + } + assert_eq!( + n.observe(SensorRecord::Imu(r), now).unwrap(), + ObservationAdmission::Rejected(expected) + ); + assert_eq!(n.stamps[0], None); + } + for field in 0..6 { + let mut n = Navigation::new(NavigationConfig::engineering_fixture()).unwrap(); + let mut r = imu(10_000); + let expected = match field { + 0 => { + r.context.validity = Validity::Invalid; + R::InvalidQuality + } + 1 => { + r.context.quality = 0; + R::InvalidQuality + } + 2 => { + r.acceleration = AccelerationFrd::metres_per_second_squared(f32::NAN, 0.0, 0.0); + R::InvalidValue + } + 3 => { + r.angular_rate = AngularRateFrd::radians_per_second(f32::NAN, 0.0, 0.0); + R::InvalidValue + } + 4 => { + r.acceleration = AccelerationFrd::metres_per_second_squared(100.0, 0.0, 0.0); + R::InvalidValue + } + _ => { + r.angular_rate = AngularRateFrd::radians_per_second(20.0, 0.0, 0.0); + R::InvalidValue + } + }; + assert_eq!( + n.observe(SensorRecord::Imu(r), r.context.receipt_time) + .unwrap(), + ObservationAdmission::Rejected(expected) + ); + assert_eq!(n.stamps[0], Some(r.context)); + assert_eq!(n.used[0], None); + } + let mut n = Navigation::new(NavigationConfig::engineering_fixture()).unwrap(); + n.observe( + SensorRecord::Imu(imu(10_000)), + Timestamp::from_microseconds(10_000), + ) + .unwrap(); + for (t, sequence) in [(10_000, 2), (20_000, 1), (20_000, u32::MAX)] { + let mut r = imu(t); + r.context.sequence = ObservationSequence::new(sequence); + assert_eq!( + n.observe(SensorRecord::Imu(r), r.context.receipt_time) + .unwrap(), + ObservationAdmission::Rejected(R::Replay) + ); + } + let mut fast = imu(14_000); + fast.context.sequence = ObservationSequence::new(2); + assert_eq!( + n.observe(SensorRecord::Imu(fast), fast.context.receipt_time) + .unwrap(), + ObservationAdmission::Rejected(R::RateExceeded) + ); + n.observe( + SensorRecord::Imu(imu(40_000)), + Timestamp::from_microseconds(40_000), + ) + .unwrap(); + assert_eq!(n.initializer.accel.n, 1); // a cold inertial gap restarts evidence + } + + #[test] + fn each_aiding_value_domain_refuses_without_initializing() { + let mut rate_limited = Navigation::new(NavigationConfig::engineering_fixture()).unwrap(); + let first = local(50_000); + rate_limited + .observe( + SensorRecord::LocalPositionVelocity(first), + first.context.receipt_time, + ) + .unwrap(); + let fast = local(60_000); + assert_eq!( + rate_limited.observe( + SensorRecord::LocalPositionVelocity(fast), + fast.context.receipt_time + ), + Ok(ObservationAdmission::Rejected( + ObservationRejection::RateExceeded + )) + ); + assert_eq!(rate_limited.stamps[1], Some(first.context)); + assert_eq!(rate_limited.used[1], None); + for field in 0..5 { + let mut r = local(50_000); + match field { + 0 => r.reference = SensorReferenceId(2), + 1 => r.position = PositionNed::metres(f32::NAN, 0.0, 0.0), + 2 => r.velocity = VelocityNed::metres_per_second(f32::NAN, 0.0, 0.0), + 3 => r.position = PositionNed::metres(10_001.0, 0.0, 0.0), + _ => r.velocity = VelocityNed::metres_per_second(25.0, 0.0, 0.0), + } + let mut n = Navigation::new(NavigationConfig::engineering_fixture()).unwrap(); + let expected = if field == 0 { + ObservationRejection::ReferenceMismatch + } else { + ObservationRejection::InvalidValue + }; + assert_eq!( + n.observe( + SensorRecord::LocalPositionVelocity(r), + r.context.receipt_time + ) + .unwrap(), + ObservationAdmission::Rejected(expected) + ); + } + for vector in [ + Vector3::new(f32::NAN, 0.0, 0.0), + Vector3::new(0.0, 0.0, 100.0), + ] { + let mut n = Navigation::new(NavigationConfig::engineering_fixture()).unwrap(); + let r = SensorRecord::Magnetic(MagneticObservation { + context: context(50_000, 3, FrameId::BODY_FRD), + microtesla_frd: vector, + variance_microtesla_squared: [ScalarVariance::new(0.0004).unwrap(); 3], + }); + assert_eq!( + n.observe(r, r.context().receipt_time).unwrap(), + ObservationAdmission::Rejected(ObservationRejection::InvalidValue) + ); + } + let mut n = Navigation::new(NavigationConfig::engineering_fixture()).unwrap(); + let p = SensorRecord::Pressure(PressureObservation { + context: context(50_000, 4, FrameId::SCALAR), + pascals: 0.0, + variance_pascals_squared: ScalarVariance::new(0.09).unwrap(), + }); + assert_eq!( + n.observe(p, p.context().receipt_time).unwrap(), + ObservationAdmission::Rejected(ObservationRejection::InvalidValue) + ); + for metres in [f32::NAN, 0.0, 41.0] { + let mut n = Navigation::new(NavigationConfig::engineering_fixture()).unwrap(); + let r = SensorRecord::Range(RangeObservation { + context: context(50_000, 5, FrameId::new(3)), + metres, + variance: ScalarVariance::new(4e-6).unwrap(), + }); + assert_eq!( + n.observe(r, r.context().receipt_time).unwrap(), + ObservationAdmission::Rejected(ObservationRejection::InvalidValue) + ); + } + } + + #[test] + fn inertial_loss_and_bounded_backlog_fail_without_automatic_recovery() { + let mut n = warm(); + let gap = imu(4_030_000); + assert_eq!( + n.observe(SensorRecord::Imu(gap), gap.context.receipt_time), + Err(NavigationFailure::ImuGap) + ); + assert!(n.estimate().is_none()); + assert_eq!( + n.tick(gap.context.receipt_time), + Err(NavigationFailure::ImuGap) + ); + assert_eq!( + n.observe(SensorRecord::Imu(gap), gap.context.receipt_time), + Err(NavigationFailure::ImuGap) + ); + let mut n = warm(); + let mut outcome = Ok(ObservationAdmission::Queued); + for t in (4_010_000..=4_320_000).step_by(10_000) { + outcome = n.observe(SensorRecord::Imu(imu(t)), Timestamp::from_microseconds(t)); + if outcome.is_err() { + break; + } + } + assert_eq!(outcome, Err(NavigationFailure::QueueOverrun)); + assert!(n.estimate().is_none()); + let mut n = warm(); + for t in (4_010_000..=4_090_000).step_by(10_000) { + n.observe(SensorRecord::Imu(imu(t)), Timestamp::from_microseconds(t)) + .unwrap(); + } + assert_eq!( + n.tick(Timestamp::from_microseconds(4_092_000)), + Err(NavigationFailure::WorkBudget) + ); + } + + #[test] + fn isolated_source_requires_newer_gated_probes_not_old_good_history() { + let mut n = warm(); + for t in (4_010_000..=4_350_000).step_by(10_000) { + let now = Timestamp::from_microseconds(t + 2_000); + for mut r in stationary_records(t).into_iter().flatten() { + if let SensorRecord::Magnetic(ref mut v) = r { + v.context.quality = 0; + } + n.observe(r, now).unwrap(); + } + n.tick(now).unwrap(); + } + assert_eq!(n.source_status()[2].integrity, SourceStatus::Isolated); + let accepted = n.used[2]; + for t in (4_360_000..=4_560_000).step_by(10_000) { + let now = Timestamp::from_microseconds(t + 2_000); + for r in stationary_records(t).into_iter().flatten() { + n.observe(r, now).unwrap(); + } + n.tick(now).unwrap(); + if t == 4_500_000 { + assert_eq!(n.used[2], accepted); + assert_eq!(n.status[2].integrity, SourceStatus::Isolated); + } + } + assert_eq!(n.source_status()[2].integrity, SourceStatus::Healthy); + assert!(n.estimate().unwrap().capabilities.flight_ready()); + } + + #[test] + fn old_range_cannot_count_as_a_post_isolation_probe() { + let mut n = warm(); + let accepted = n.used[4]; + for t in (4_010_000..=4_080_000).step_by(10_000) { + let now = Timestamp::from_microseconds(t); + n.observe(SensorRecord::Imu(imu(t)), now).unwrap(); + if t.is_multiple_of(20_000) { + let mut c = context(t, 5, FrameId::new(3)); + if t > 4_020_000 { + c.quality = 0; + } + n.observe( + SensorRecord::Range(RangeObservation { + context: c, + metres: 0.04, + variance: ScalarVariance::new(4e-6).unwrap(), + }), + now, + ) + .unwrap(); + } + } + n.tick(Timestamp::from_microseconds(4_082_000)).unwrap(); + assert_eq!(n.status[4].integrity, SourceStatus::Isolated); + assert_eq!(n.probes[4], 0); + assert_eq!(n.used[4], accepted); + } + + #[test] + fn numerical_failures_at_historical_and_current_prediction_boundaries_latch() { + let mut historical = warm(); + for t in (4_010_000..=4_050_000).step_by(10_000) { + historical + .observe(SensorRecord::Imu(imu(t)), Timestamp::from_microseconds(t)) + .unwrap(); + } + historical.filter.as_mut().unwrap().covariance[0][0] = -1.0; + assert_eq!( + historical.tick(Timestamp::from_microseconds(4_052_000)), + Err(NavigationFailure::Numerical) + ); + assert!(historical.estimate().is_none()); + let mut current = warm(); + current.range = None; + current.filter.as_mut().unwrap().covariance[0][0] = -1.0; + assert_eq!( + current.tick(Timestamp::from_microseconds(4_002_000)), + Err(NavigationFailure::Numerical) + ); + assert!(current.estimate().is_none()); + let mut f = warm().filter.unwrap(); + f.covariance[15][15] = -1.0; + let prior = f.covariance; + let pressure = SensorRecord::Pressure(PressureObservation { + context: context(f.time, 4, FrameId::SCALAR), + pascals: 101_325.0, + variance_pascals_squared: ScalarVariance::new(0.09).unwrap(), + }); + let sample = imu(f.time); + assert_eq!( + fuse( + &mut f, + pressure, + NavigationConfig::engineering_fixture(), + sample + ), + Err(NavigationFailure::Numerical) + ); + assert_eq!(f.covariance, prior); + } + #[test] + fn motion_confidence_and_timed_aiding_fail_closed_on_unrepresentable_state() { + let original = warm().filter.unwrap(); + let config = NavigationConfig::engineering_fixture(); + assert_eq!( + motion_at(&original, &[None; 32], 0, config).err(), + Some(NavigationFailure::ImuGap) + ); + let sample = imu(original.time); + let mut history = [None; 32]; + history[0] = Some(sample); + let mut extrapolated = original; + extrapolated.time += 5_000; + assert!(motion_at(&extrapolated, &history, 1, config).is_ok()); + for field in 0..3 { + let mut f = original; + match field { + 0 => f.bg = Vector3::new(f32::MAX, f32::MAX, f32::MAX), + 1 => f.ba = Vector3::new(f32::MAX, f32::MAX, f32::MAX), + _ => { + for i in [6, 7, 8, 12, 13, 14] { + f.covariance[i][i] = f32::MAX / 4.0; + } + } + } + assert_eq!( + motion_at(&f, &history, 1, config).err(), + Some(NavigationFailure::Numerical) + ); + } + for r in [ + SensorRecord::LocalPositionVelocity(local(original.time)), + stationary_records(4_000_000)[2].unwrap(), + ] { + let mut f = original; + f.covariance[15][15] = -1.0; + let before = f.covariance; + assert_eq!( + fuse(&mut f, r, config, sample), + Err(NavigationFailure::Numerical) + ); + assert_eq!(f.covariance, before); + assert_eq!(f.q, original.q); + } + } + #[test] + fn tilted_sensor_initialization_retains_joint_datum_and_bias_uncertainty() { + let config = NavigationConfig::engineering_fixture(); + let q = UnitQuaternion::::from_components( + 0.968_912_4, + 0.061_851, + -0.086_591, + 0.222_664, + ) + .unwrap(); + let acceleration = q.ned_to_body(Vector3::new(0., 0., -GRAVITY_MPS2)); + let magnetic = q.ned_to_body(config.installation.magnetic_field_ned_microtesla); + let mut init = Initializer::new(); + let mut last = None; + for t in (0..=2_000_000).step_by(10_000) { + let imu = FlightImuObservation { + acquisition_duration_us: 10_000, + context: context(t, 1, FrameId::BODY_FRD), + acceleration: AccelerationFrd::metres_per_second_squared( + acceleration.x(), + acceleration.y(), + acceleration.z(), + ), + angular_rate: AngularRateFrd::radians_per_second(0.01, -0.02, 0.005), + }; + init.push(SensorRecord::Imu(imu), config); + last = Some(imu); + if t.is_multiple_of(50_000) { + init.push( + SensorRecord::LocalPositionVelocity(LocalPositionVelocityObservation { + context: context(t, 2, FrameId::LOCAL_NED), + position: PositionNed::metres(12., -7., 4.), + velocity: VelocityNed::metres_per_second(0., 0., 0.), + position_variance: [ScalarVariance::new(0.01).unwrap(); 3], + velocity_variance: [ScalarVariance::new(0.0025).unwrap(); 3], + reference: SensorReferenceId(1), + }), + config, + ); + init.push( + SensorRecord::Magnetic(MagneticObservation { + context: context(t, 3, FrameId::BODY_FRD), + microtesla_frd: magnetic, + variance_microtesla_squared: [ScalarVariance::new(0.0004).unwrap(); 3], + }), + config, + ); + init.push( + SensorRecord::Pressure(PressureObservation { + context: context(t, 4, FrameId::SCALAR), + pascals: 101325., + variance_pascals_squared: ScalarVariance::new(0.09).unwrap(), + }), + config, + ); + } + if t.is_multiple_of(20_000) { + init.push( + SensorRecord::Range(RangeObservation { + context: context(t, 5, FrameId::new(3)), + metres: 0.05, + variance: ScalarVariance::new(4e-6).unwrap(), + }), + config, + ); + } + } + let (f, origin) = init.finish(last.unwrap(), config).unwrap(); + let error = f.q.compose(q.inverse()).unwrap(); + assert!(error.x().abs() + error.y().abs() + error.z().abs() < 1e-5); + assert!((f.bg.x() - 0.01).abs() < 1e-6); + assert_eq!(f.datum, 4.); + assert_eq!(f.covariance[15][2], f.covariance[2][2]); + assert!(f.covariance[15][15] > f.covariance[2][2]); + assert!((6..9).any(|i| (12..15).any(|j| f.covariance[i][j].abs() > 1e-6))); + assert_eq!( + origin.launch_in_reference, + PositionNed::metres(12., -7., 4.) + ); + assert!(f.valid()); + } + #[test] + fn hard_time_bound_is_not_receipt_relative_freshness() { + let mut nav = Navigation::new(NavigationConfig::engineering_fixture()).unwrap(); + let mut c = context(10_000, 1, FrameId::BODY_FRD); + c.receipt_time = Timestamp::from_microseconds(12_000); + c.time_uncertainty_microseconds = 2_000; + let imu = FlightImuObservation { + acquisition_duration_us: 10_000, + context: c, + acceleration: AccelerationFrd::metres_per_second_squared(0., 0., -GRAVITY_MPS2), + angular_rate: AngularRateFrd::radians_per_second(0., 0., 0.), + }; + assert_eq!( + nav.observe(SensorRecord::Imu(imu), Timestamp::from_microseconds(28_000)) + .unwrap(), + ObservationAdmission::Rejected(ObservationRejection::TooOld) + ); + assert_eq!(nav.stamps[0], None); + assert_eq!( + nav.observe(SensorRecord::Imu(imu), Timestamp::from_microseconds(12_000)) + .unwrap(), + ObservationAdmission::Queued + ); + assert_eq!( + nav.used[0].unwrap().measurement_time, + Timestamp::from_microseconds(10_000) + ); + let mut future = imu; + future.context.measurement_time = Timestamp::from_microseconds(20_000); + future.context.receipt_time = Timestamp::from_microseconds(21_999); + future.context.sequence = ObservationSequence::new(2); + assert_eq!( + nav.observe( + SensorRecord::Imu(future), + Timestamp::from_microseconds(22_000) + ) + .unwrap(), + ObservationAdmission::Rejected(ObservationRejection::InvalidTime) + ); + } +} diff --git a/firmware/libs/kaptan-navigation/src/observation.rs b/firmware/libs/kaptan-navigation/src/observation.rs index 69a3c77..7a8fa4e 100644 --- a/firmware/libs/kaptan-navigation/src/observation.rs +++ b/firmware/libs/kaptan-navigation/src/observation.rs @@ -1,15 +1,10 @@ //! Typed SI observations and their calibration/frame provenance. -use kaptan_math::{Frd, Ned, Sensor, UnitQuaternion, Vector2, Vector3}; -#[cfg(any(test, feature = "sitl"))] -use kaptan_sensor::imu::ImuSnapshot; -#[cfg(any(test, feature = "sitl"))] -use kaptan_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot}; +use kaptan_math::{Frd, Ned, Vector3}; +#[cfg(any(test, feature = "sensor-snapshots"))] +use kaptan_sensor::range::RangeSnapshot; use kaptan_time::Timestamp; -#[cfg(any(test, feature = "sitl"))] -use crate::GRAVITY_MPS2; - /// Origin of one observation stream, allocated by a static board configuration. #[derive(Clone, Copy, Debug, Eq, PartialEq)] pub struct SourceId(u8); @@ -32,6 +27,7 @@ impl FrameId { pub const BODY_FRD: Self = Self(0); /// Active launch-local navigation frame with north/east/down axes. pub const LOCAL_NED: Self = Self(1); + pub const SCALAR: Self = Self(2); /// Creates a configured sensor frame identifier. pub const fn new(value: u8) -> Self { Self(value) @@ -44,11 +40,9 @@ impl FrameId { /// Monotonic revision of the calibration/extrinsics used for an observation. /// -/// TODO(configuration): this is a provenance token, not a calibration store. -/// The future configuration service must atomically install sensor mounting, -/// scale/bias calibration, uncertainty, and this revision while navigation is -/// stopped, then require reinitialization before admitting observations from -/// the new revision. +/// This token identifies a coherent mounting, calibration and uncertainty set. +/// Changing the set requires navigation to stop and reinitialize before it +/// accepts observations carrying the new revision. #[derive(Clone, Copy, Debug, Eq, PartialEq)] pub struct CalibrationRevision(u32); impl CalibrationRevision { @@ -86,6 +80,13 @@ pub enum Validity { } /// Common provenance for every navigation observation. +/// +/// The caller must map physical sample time into the controller clock, accounting +/// for sensor delay. A snapshot's request or reception timestamp is insufficient. +/// Without that mapping, withhold the observation or mark it [`Validity::Invalid`]. +/// Receipt time must be at or after the snapshot timestamp in the same clock domain. +/// Snapshot conversion preserves these times and changes only units. The estimator +/// checks ordering and freshness on admission. #[derive(Clone, Copy, Debug, Eq, PartialEq)] pub struct ObservationContext { /// Sensor measurement time synchronized into the controller monotonic domain. @@ -104,32 +105,8 @@ pub struct ObservationContext { pub validity: Validity, /// Source sequence number. pub sequence: ObservationSequence, -} - -impl ObservationContext { - /// Creates provenance shared by a concrete observation value. - #[allow(clippy::too_many_arguments)] - pub const fn new( - measurement_time: Timestamp, - receipt_time: Timestamp, - source: SourceId, - frame: FrameId, - calibration: CalibrationRevision, - quality: u8, - validity: Validity, - sequence: ObservationSequence, - ) -> Self { - Self { - measurement_time, - receipt_time, - source, - frame, - calibration, - quality, - validity, - sequence, - } - } + /// Hard absolute clock-mapping error, not a statistical standard deviation. + pub time_uncertainty_microseconds: u32, } /// Positive finite scalar measurement variance in squared SI units. @@ -150,29 +127,13 @@ impl ScalarVariance { } } -/// Symmetric two-dimensional covariance in squared SI units. -#[derive(Clone, Copy, Debug, PartialEq)] -pub struct Covariance2 { - /// xx variance. - pub xx: ScalarVariance, - /// xy covariance. - pub xy: f32, - /// yy variance. - pub yy: ScalarVariance, -} -impl Covariance2 { - /// Validates finite covariance and a positive determinant. - pub fn new(xx: ScalarVariance, xy: f32, yy: ScalarVariance) -> Option { - let determinant = xx.get() * yy.get() - xy * xy; - if xy.is_finite() && determinant.is_finite() && determinant > 0.0 { - Some(Self { xx, xy, yy }) - } else { - None - } - } -} - /// NED position in metres. +/// +/// Position and velocity remain distinct even in the same coordinate frame. +/// ```compile_fail,E0308 +/// use kaptan_navigation::{PositionNed, VelocityNed}; +/// let position: PositionNed = VelocityNed::metres_per_second(0.0, 0.0, 0.0); +/// ``` #[derive(Clone, Copy, Debug, PartialEq)] pub struct PositionNed(Vector3); impl PositionNed { @@ -203,8 +164,7 @@ impl VelocityNed { /// Complete position and velocity aiding in the active local-NED frame. /// /// Position and velocity share one source timestamp and sequence because the -/// producer measures the same vehicle state. Their diagonal variances remain -/// explicit rather than being hidden in a simulator or transport adapter. +/// producer measures the same vehicle state. Variances use squared SI units. #[derive(Clone, Copy, Debug, PartialEq)] pub struct LocalPositionVelocityObservation { /// Required source/time/frame provenance. @@ -214,23 +174,11 @@ pub struct LocalPositionVelocityObservation { /// Absolute local-NED velocity in metres per second. pub velocity: VelocityNed, /// Per-axis position measurement variance in m². - pub position_variance: ScalarVariance, + pub position_variance: [ScalarVariance; 3], /// Per-axis velocity measurement variance in (m/s)². - pub velocity_variance: ScalarVariance, -} - -/// Body FRD position offset in metres. -#[derive(Clone, Copy, Debug, PartialEq)] -pub struct PositionFrd(Vector3); -impl PositionFrd { - /// Creates a body-frame offset in metres. - pub const fn metres(forward: f32, right: f32, down: f32) -> Self { - Self(Vector3::new(forward, right, down)) - } - /// Returns its body vector. - pub const fn vector(self) -> Vector3 { - self.0 - } + pub velocity_variance: [ScalarVariance; 3], + /// Fixed sensor-reference coordinates. Navigation alone subtracts launch. + pub reference: crate::SensorReferenceId, } /// Body FRD specific force in metres per second squared. @@ -261,57 +209,6 @@ impl AngularRateFrd { } } -/// Configured IMU white-noise variances in SI squared units. -#[derive(Clone, Copy, Debug, PartialEq)] -#[cfg(any(test, feature = "sitl"))] -pub struct ImuNoise { - /// Gyro rate variance, (rad/s)^2. - pub gyro_rate: ScalarVariance, - /// Accelerometer specific-force variance, (m/s²)^2. - pub acceleration: ScalarVariance, -} - -/// IMU observation expressed in the calibrated aircraft FRD frame. -#[derive(Clone, Copy, Debug, PartialEq)] -#[cfg(any(test, feature = "sitl"))] -pub struct ImuObservation { - /// Required observation provenance. - pub context: ObservationContext, - /// Specific force in m/s². - pub acceleration: AccelerationFrd, - /// Angular rate in rad/s. - pub angular_rate: AngularRateFrd, - /// Explicit configured uncertainty. - pub noise: ImuNoise, -} - -#[cfg(any(test, feature = "sitl"))] -impl ImuObservation { - /// Converts the existing integer IMU contract to SI exactly at the - /// navigation boundary. The board has already aligned this snapshot to FRD. - pub fn from_snapshot( - snapshot: ImuSnapshot, - mut context: ObservationContext, - noise: ImuNoise, - ) -> Self { - context.measurement_time = snapshot.timestamp; - Self { - context, - acceleration: AccelerationFrd::metres_per_second_squared( - f32::from(snapshot.acceleration_milligravity[0]) * GRAVITY_MPS2 / 1_000.0, - f32::from(snapshot.acceleration_milligravity[1]) * GRAVITY_MPS2 / 1_000.0, - f32::from(snapshot.acceleration_milligravity[2]) * GRAVITY_MPS2 / 1_000.0, - ), - angular_rate: AngularRateFrd::radians_per_second( - f32::from(snapshot.angular_rate_milliradians_per_second[0]) / 1_000.0, - f32::from(snapshot.angular_rate_milliradians_per_second[1]) / 1_000.0, - f32::from(snapshot.angular_rate_milliradians_per_second[2]) / 1_000.0, - ), - noise, - } - } -} - /// Range observation in metres along the sensor's positive z axis. #[derive(Clone, Copy, Debug, PartialEq)] pub struct RangeObservation { @@ -325,13 +222,14 @@ pub struct RangeObservation { impl RangeObservation { /// Converts a driver range value to metres at the navigation boundary. - #[cfg(any(test, feature = "sitl"))] + /// + /// Uses the caller's timestamps under the [`ObservationContext`] timing contract. + #[cfg(any(test, feature = "sensor-snapshots"))] pub fn from_snapshot( snapshot: RangeSnapshot, - mut context: ObservationContext, + context: ObservationContext, variance: ScalarVariance, ) -> Self { - context.measurement_time = snapshot.timestamp; Self { context, metres: snapshot.distance_millimetres as f32 / 1_000.0, @@ -340,115 +238,23 @@ impl RangeObservation { } } -/// Ground-referenced XY velocity reported in a named sensor frame. -#[derive(Clone, Copy, Debug, PartialEq)] -pub struct OpticalFlowObservation { - /// Required observation provenance. - pub context: ObservationContext, - /// Sensor-frame x/y velocity in m/s. - pub velocity: Vector2, - /// Ground distance in metres; retained for plausibility/diagnostics. - pub ground_distance_metres: f32, - /// Explicit XY measurement covariance in (m/s)². - pub covariance: Covariance2, -} - -impl OpticalFlowObservation { - /// Converts the existing MTF-01-independent driver value into SI. - #[cfg(any(test, feature = "sitl"))] - pub fn from_snapshot( - snapshot: OpticalFlowSnapshot, - mut context: ObservationContext, - covariance: Covariance2, - ) -> Self { - context.measurement_time = snapshot.timestamp; - context.quality = snapshot.quality; - Self { - context, - velocity: Vector2::new( - snapshot.velocity_millimetres_per_second[0] as f32 / 1_000.0, - snapshot.velocity_millimetres_per_second[1] as f32 / 1_000.0, - ), - ground_distance_metres: snapshot.ground_distance_millimetres as f32 / 1_000.0, - covariance, - } - } -} - -/// Static calibrated transform from a sensor frame into aircraft FRD. -/// -/// TODO(configuration): this remains a build-time value until the -/// configuration service can stage the full sensor calibration bundle. Do not -/// expose setters for individual fields: source identity, frame, calibration -/// revision, rotation, and lever arm must change together and only while the -/// estimator and actuators are inhibited. -#[derive(Clone, Copy, Debug, PartialEq)] -pub struct SensorExtrinsics { - /// Statically configured source expected for this physical device. - pub source: SourceId, - /// Frame ID expected in every observation from this device. - pub sensor_frame: FrameId, - /// Calibration/extrinsics revision represented by this transform. - pub calibration: CalibrationRevision, - /// Rotation mapping sensor vectors into FRD. - pub sensor_to_body: UnitQuaternion, - /// Sensor origin relative to aircraft origin, in FRD metres. - pub position_body: PositionFrd, -} - -impl SensorExtrinsics { - /// Maps a sensor vector into FRD at the sole sensor-calibration boundary. - pub fn sensor_to_body_vector(self, value: Vector3) -> Vector3 { - self.sensor_to_body.rotate(value) - } -} - #[cfg(test)] mod tests { use super::*; - use kaptan_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot}; + use kaptan_sensor::range::RangeSnapshot; fn context(frame: FrameId) -> ObservationContext { - ObservationContext::new( - Timestamp::from_microseconds(10), - Timestamp::from_microseconds(12), - SourceId::new(2), + ObservationContext { + measurement_time: Timestamp::from_microseconds(95), + receipt_time: Timestamp::from_microseconds(120), + source: SourceId::new(2), frame, - CalibrationRevision::new(3), - 250, - Validity::Valid, - ObservationSequence::new(9), - ) - } - - #[test] - fn imu_snapshot_is_si() { - let noise = ImuNoise { - gyro_rate: ScalarVariance::new(0.01).unwrap(), - acceleration: ScalarVariance::new(0.02).unwrap(), - }; - let sample = ImuObservation::from_snapshot( - ImuSnapshot::new( - Timestamp::from_microseconds(10), - [1_000, 0, -1_000], - [1_000, 0, 0], - 0, - ), - context(FrameId::BODY_FRD), - noise, - ); - assert!((sample.acceleration.vector().x() - GRAVITY_MPS2).abs() < 1.0e-5); - assert!((sample.angular_rate.vector().x() - 1.0).abs() < 1.0e-6); - assert_eq!(sample.context.calibration, CalibrationRevision::new(3)); - } - - #[test] - fn covariance_validation() { - let variance = ScalarVariance::new(1.0).unwrap(); - assert!(Covariance2::new(variance, 1.0, variance).is_none()); - let large = ScalarVariance::new(f32::MAX).unwrap(); - assert!(Covariance2::new(large, 0.0, large).is_none()); - assert!(Covariance2::new(variance, f32::NAN, variance).is_none()); + calibration: CalibrationRevision::new(3), + quality: 250, + validity: Validity::Valid, + sequence: ObservationSequence::new(9), + time_uncertainty_microseconds: 0, + } } #[test] @@ -463,55 +269,24 @@ mod tests { } #[test] - fn snapshots_convert_at_boundary() { + fn range_conversion_preserves_established_times() { let variance = ScalarVariance::new(0.25).unwrap(); let range = RangeObservation::from_snapshot( - RangeSnapshot::new(Timestamp::from_microseconds(20), 1_234), + RangeSnapshot::new(Timestamp::from_microseconds(100), 1_234), context(FrameId::new(4)), variance, ); - assert_eq!( - range.context.measurement_time, - Timestamp::from_microseconds(20) - ); + assert_eq!(range.context, context(FrameId::new(4))); assert!((range.metres - 1.234).abs() < 1.0e-6); - - let covariance = Covariance2::new(variance, 0.01, variance).unwrap(); - let flow = OpticalFlowObservation::from_snapshot( - OpticalFlowSnapshot::new(Timestamp::from_microseconds(21), [-500, 250], 2_500, 42), - context(FrameId::new(5)), - covariance, - ); - assert_eq!( - flow.context.measurement_time, - Timestamp::from_microseconds(21) - ); - assert_eq!(flow.context.quality, 42); - assert_eq!((flow.velocity.x(), flow.velocity.y()), (-0.5, 0.25)); - assert_eq!(flow.ground_distance_metres, 2.5); } #[test] fn observation_frames() { - let noise = ImuNoise { - gyro_rate: ScalarVariance::new(0.01).unwrap(), - acceleration: ScalarVariance::new(0.02).unwrap(), - }; - let sample = ImuObservation::from_snapshot( - ImuSnapshot::new(Timestamp::from_microseconds(10), [0; 3], [0; 3], 0), - context(FrameId::BODY_FRD), - noise, - ); - assert_eq!( - sample.context.measurement_time, - Timestamp::from_microseconds(10) - ); assert_eq!(PositionNed::metres(1.0, 2.0, 3.0).vector().z(), 3.0); assert_eq!( VelocityNed::metres_per_second(1.0, 2.0, 3.0).vector().y(), 2.0 ); - assert_eq!(PositionFrd::metres(1.0, 2.0, 3.0).vector().x(), 1.0); assert_eq!( AccelerationFrd::metres_per_second_squared(1.0, 2.0, 3.0) .vector() @@ -524,17 +299,5 @@ mod tests { .z(), 3.0 ); - - let extrinsics = SensorExtrinsics { - source: SourceId::new(2), - sensor_frame: FrameId::new(5), - calibration: CalibrationRevision::new(3), - sensor_to_body: UnitQuaternion::identity(), - position_body: PositionFrd::metres(0.0, 0.0, 0.0), - }; - assert_eq!( - extrinsics.sensor_to_body_vector(Vector3::new(1.0, 2.0, 3.0)), - Vector3::new(1.0, 2.0, 3.0) - ); } } diff --git a/firmware/libs/kaptan-navigation/src/timing.rs b/firmware/libs/kaptan-navigation/src/timing.rs new file mode 100644 index 0000000..4616c1c --- /dev/null +++ b/firmware/libs/kaptan-navigation/src/timing.rs @@ -0,0 +1,335 @@ +//! Correlated, bounded timing-model error, separate from Gaussian covariance. +//! +//! `shape` encloses deterministic error in an ellipsoid. Linear transformations +//! preserve state correlations. Minkowski addition covers arbitrary correlation +//! between old and new bounded errors, including one persistent clock offset. +//! It is NOT independent measurement noise and is never added to Gaussian P. + +use crate::{ + NavigationFailure, + filter::{Matrix, N, ZERO, sandwich}, +}; +use kaptan_math::sqrt; + +#[derive(Clone, Copy)] +pub(crate) struct TimingEnvelope { + shape: Matrix, +} + +impl TimingEnvelope { + pub const ZERO: Self = Self { shape: ZERO }; + + pub fn valid(&self) -> bool { + self.shape.iter().flatten().all(|v| v.is_finite()) + && (0..N).all(|i| self.shape[i][i] >= 0.0) + } + + pub fn radius(&self, axis: usize) -> f32 { + sqrt(self.shape[axis][axis]) + } + + pub fn norm_bound(&self, start: usize, count: usize) -> f32 { + // trace bounds the largest eigenvalue, so this bounds the Euclidean + // norm of the requested projection of the ellipsoid, not a sigma. + sqrt((start..start + count).map(|i| self.shape[i][i]).sum()) + } + + pub fn projected(&self, h: &[f32; N]) -> Result { + let mut squared = 0.0; + for (i, row) in self.shape.iter().enumerate() { + for (j, value) in row.iter().enumerate() { + squared += h[i] * value * h[j]; + } + } + if !squared.is_finite() || squared < 0.0 { + return Err(NavigationFailure::Numerical); + } + Ok(sqrt(squared)) + } + + pub fn transform(&mut self, a: &Matrix) -> Result<(), NavigationFailure> { + if self.shape == ZERO { + return Ok(()); + } + // Reject nonfinite shape before skipping zero coefficients, which would + // otherwise hide 0*Inf/NaN failures. + if !self.shape.iter().flatten().all(|v| v.is_finite()) { + return Err(NavigationFailure::Numerical); + } + let mut next = sandwich(a, &self.shape); + let columns: [f32; N] = core::array::from_fn(|k| a.iter().map(|r| r[k].abs()).sum()); + for (i, row) in next.iter_mut().enumerate() { + let mut absolute_products = [0.0; N]; + for (j, aj) in a[i].iter().enumerate() { + if *aj != 0.0 { + for (k, ap) in absolute_products.iter_mut().enumerate() { + *ap += aj.abs() * self.shape[j][k].abs(); + } + } + } + let mut absolute_row = 0.0; + for (ap, column) in absolute_products.iter().zip(&columns) { + absolute_row += ap * column; + } + // Outward arithmetic enclosure, not a repair of Gaussian P. The + // 16-term products need < gamma_64; 256 eps covers the computed + // majorant and diagonal addition too. The absolute term also + // dominates underflow/flush-to-zero at these fixed matrix sizes. + row[i] = (row[i] + absolute_row * (256.0 * f32::EPSILON) + 1e-30).next_up(); + } + self.commit(next) + } + + pub fn add_generator(&mut self, g: [f32; N]) -> Result<(), NavigationFailure> { + if !g.iter().all(|v| v.is_finite()) { + return Err(NavigationFailure::Numerical); + } + if g.iter().all(|v| *v == 0.0) { + return Ok(()); + } + let old_trace: f32 = (0..N).map(|i| self.shape[i][i]).sum(); + let new_trace: f32 = g.iter().map(|v| v * v).sum(); + if !old_trace.is_finite() || !new_trace.is_finite() { + return Err(NavigationFailure::Numerical); + } + let (old_weight, new_weight) = if old_trace == 0.0 { + (0.0, 1.0) + } else { + // Every positive beta is a valid Minkowski enclosure. This trace + // choice in the declared SI coordinates reduces its size. Limiting + // beta is numerical conditioning, not a limit on admitted timing. + let old_norm = sqrt(old_trace); + let new_norm = sqrt(new_trace.max(1e-30)); + let beta = if new_norm > old_norm * 1e6 { + 1e6 + } else if new_norm < old_norm * 1e-6 { + 1e-6 + } else { + new_norm / old_norm + }; + (1.0 + beta, 1.0 + 1.0 / beta) + }; + let mut next = ZERO; + for (i, row) in next.iter_mut().enumerate() { + let mut magnitude = 0.0; + for (j, value) in row.iter_mut().enumerate() { + let left = old_weight * self.shape[i][j]; + let right = new_weight * g[i] * g[j]; + *value = left + right; + magnitude += left.abs() + right.abs(); + } + row[i] = (row[i] + magnitude * (32.0 * f32::EPSILON) + 1e-30).next_up(); + } + self.commit(next) + } + + fn commit(&mut self, mut next: Matrix) -> Result<(), NavigationFailure> { + for i in 0..N { + for j in 0..i { + let value = next[i][j] * 0.5 + next[j][i] * 0.5; + next[i][j] = value; + next[j][i] = value; + } + } + let candidate = Self { shape: next }; + if !candidate.valid() { + return Err(NavigationFailure::Numerical); + } + *self = candidate; + Ok(()) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::filter::identity; + + impl TimingEnvelope { + fn former_transform(&mut self, a: &Matrix) -> Result<(), NavigationFailure> { + if self.shape == ZERO { + return Ok(()); + } + let mut next = sandwich(a, &self.shape); + let columns: [f32; N] = core::array::from_fn(|k| a.iter().map(|r| r[k].abs()).sum()); + for (i, row) in next.iter_mut().enumerate() { + let mut absolute_row = 0.0; + for (k, column) in columns.iter().enumerate() { + let mut ap = 0.0; + for (j, aj) in a[i].iter().enumerate() { + ap += aj.abs() * self.shape[j][k].abs(); + } + absolute_row += ap * column; + } + // Outward arithmetic enclosure, not a repair of Gaussian P. The + // 16-term products need < gamma_64; 256 eps covers the computed + // majorant and diagonal addition too. The absolute term also + // dominates underflow/flush-to-zero at these fixed matrix sizes. + row[i] = (row[i] + absolute_row * (256.0 * f32::EPSILON) + 1e-30).next_up(); + } + self.commit(next) + } + } + + #[test] + fn sparse_absolute_products_preserve_the_dense_enclosure_and_refusal() { + fn compare(shape: Matrix, a: Matrix) { + let mut previous = TimingEnvelope { shape }; + let mut current = previous; + assert_eq!(current.transform(&a), previous.former_transform(&a)); + // Includes the retained input on refusal. Check signs of zero and + // original nonfinite payloads as well as every finite result bit. + for (actual, expected) in current + .shape + .iter() + .flatten() + .zip(previous.shape.iter().flatten()) + { + assert_eq!(actual.to_bits(), expected.to_bits()); + } + } + let mut sparse = identity(); + sparse[0][3] = -0.02; + sparse[3][7] = 0.196133; + sparse[6][9] = -0.02; + let dense = core::array::from_fn(|i| { + core::array::from_fn(|j| ((i * 7 + j * 3) as f32 - 47.0) * 0.03125) + }); + for scale in [ + -0.0, + f32::from_bits(1), + f32::MIN_POSITIVE, + 1e-20, + 0.25, + 1e20, + f32::MAX, + ] { + let shape = core::array::from_fn(|i| { + core::array::from_fn(|j| if i == j { scale } else { scale * 0.01 }) + }); + for a in [ZERO, identity(), sparse, dense] { + compare(shape, a); + } + } + for bad in [f32::NAN, f32::INFINITY, f32::NEG_INFINITY] { + for (row, column) in [(0, 0), (12, 4)] { + let mut shape = identity(); + shape[row][column] = bad; + for a in [ZERO, identity(), sparse, dense] { + compare(shape, a); + } + } + let mut a = identity(); + a[0][1] = bad; + compare(ZERO, a); // The existing zero-shape early return stays first. + compare(identity(), a); + } + let mut huge = identity(); + huge[0][0] = f32::MAX; + compare(identity(), huge); + } + + #[test] + fn correlated_generators_and_linear_motion_enclose_independent_supports() { + // The exact set is a sum of line segments. Its support in any direction + // is sum(|h dot g|), regardless of whether clock offsets are correlated. + let generators: [[f32; N]; 3] = core::array::from_fn(|j| { + core::array::from_fn(|i| ((i * 7 + j * 11) as f32 - 50.0) * 0.0001) + }); + let mut envelope = TimingEnvelope::ZERO; + for g in generators { + envelope.add_generator(g).unwrap(); + } + let mut a = identity(); + a[0][3] = 0.02; + a[3][7] = -0.196133; + a[6][9] = -0.02; + a[15][2] = 1.0; + envelope.transform(&a).unwrap(); + for trial in 0..128 { + let h: [f32; N] = + core::array::from_fn(|i| (((trial + 1) * (i + 3) * 17 % 103) as f32 - 51.0) / 51.0); + let exact: f64 = generators + .iter() + .map(|g| { + (0..N) + .map(|i| { + f64::from(h[i]) + * (0..N) + .map(|j| f64::from(a[i][j]) * f64::from(g[j])) + .sum::() + }) + .sum::() + .abs() + }) + .sum(); + assert!(f64::from(envelope.projected(&h).unwrap()) >= exact); + } + assert!(envelope.norm_bound(0, 3) >= envelope.radius(0)); + + // A common position/datum offset cancels in the pressure observation. + // Diagonal independent-error treatment would lose this information. + let mut correlated = TimingEnvelope::ZERO; + let mut g = [0.0; N]; + g[2] = 0.01; + g[15] = 0.01; + correlated.add_generator(g).unwrap(); + let mut h = [0.0; N]; + h[2] = -1.0; + h[15] = 1.0; + assert!(correlated.projected(&h).unwrap() < 0.0001); + } + + #[test] + fn disparate_and_subnormal_generators_remain_outward() { + for (first, second) in [(1e-8, 1.0), (1.0, 1e-8), (1e-40, 1e-40)] { + let mut envelope = TimingEnvelope::ZERO; + let mut g = [0.0; N]; + g[0] = first; + envelope.add_generator(g).unwrap(); + g[0] = second; + envelope.add_generator(g).unwrap(); + assert!(f64::from(envelope.radius(0)) >= f64::from(first) + f64::from(second)); + } + let mut zero = TimingEnvelope::ZERO; + zero.add_generator([0.0; N]).unwrap(); + zero.transform(&identity()).unwrap(); + assert_eq!(zero.shape, ZERO); + } + + #[test] + fn numerical_failures_preserve_the_timing_enclosure() { + let mut envelope = TimingEnvelope::ZERO; + let mut g = [0.0; N]; + g[0] = 0.002; + envelope.add_generator(g).unwrap(); + let before = envelope.shape; + for bad in [f32::NAN, f32::INFINITY, f32::MAX] { + g[0] = bad; + assert_eq!(envelope.add_generator(g), Err(NavigationFailure::Numerical)); + assert_eq!(envelope.shape, before); + } + let mut a = identity(); + a[0][0] = f32::MAX; + assert_eq!(envelope.transform(&a), Err(NavigationFailure::Numerical)); + assert_eq!(envelope.shape, before); + let mut h = [0.0; N]; + h[0] = f32::MAX; + assert_eq!(envelope.projected(&h), Err(NavigationFailure::Numerical)); + + // Exercise the finite screen independently of callers' Gaussian checks. + let mut invalid = TimingEnvelope { shape: identity() }; + invalid.shape[0][0] = -1.0; + assert!(!invalid.valid()); + h[0] = 1.0; + assert_eq!(invalid.projected(&h), Err(NavigationFailure::Numerical)); + invalid.shape[0][0] = f32::INFINITY; + assert!(!invalid.valid()); + invalid.shape = identity(); + for (i, row) in invalid.shape.iter_mut().enumerate() { + row[i] = f32::MAX; + } + g[0] = 0.002; + assert_eq!(invalid.add_generator(g), Err(NavigationFailure::Numerical)); + } +} diff --git a/firmware/libs/kaptan-dma/Cargo.toml b/firmware/libs/kaptan-ports/Cargo.toml similarity index 62% rename from firmware/libs/kaptan-dma/Cargo.toml rename to firmware/libs/kaptan-ports/Cargo.toml index 225fd82..1f4186b 100644 --- a/firmware/libs/kaptan-dma/Cargo.toml +++ b/firmware/libs/kaptan-ports/Cargo.toml @@ -1,5 +1,5 @@ [package] -name = "kaptan-dma" +name = "kaptan-ports" edition.workspace = true license.workspace = true rust-version.workspace = true @@ -7,3 +7,6 @@ version.workspace = true [lints] workspace = true + +[dependencies] +kaptan-config = { path = "../kaptan-config" } diff --git a/firmware/libs/kaptan-ports/src/catalog.rs b/firmware/libs/kaptan-ports/src/catalog.rs new file mode 100644 index 0000000..0bd3e50 --- /dev/null +++ b/firmware/libs/kaptan-ports/src/catalog.rs @@ -0,0 +1,227 @@ +use kaptan_config::{ConfigViolation, FieldId, ValidationRule}; + +use crate::{PortId, Role}; + +/// One platform-owned UART's static capabilities and schema identities. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct PortDescriptor { + pub id: PortId, + pub role_field: FieldId, + pub baud_field: FieldId, + pub role_key: &'static str, + pub baud_key: &'static str, + /// Subset of `Role::CONFIG_ENUM_OPTIONS`; must include Disabled. + pub supported_roles: &'static [(u8, &'static str)], + /// Nonempty, unique nonzero baud whitelist, enforced even while disabled. + pub allowed_bauds: &'static [u32], + /// Exclusive resource tokens claimed by any enabled role on this UART. + pub resources: &'static [u16], +} + +impl PortDescriptor { + /// The same role and baud metadata exposed by [`crate::Ports`]. Platforms + /// can check that a composed root advertises the inventory they execute. + pub const fn fields(&self) -> [kaptan_config::FieldDescriptor; 2] { + crate::schema::fields(&[*self])[0] + } +} + +/// Immutable platform inventory. Resource tokens share one platform namespace. +/// +/// Use explicit stable IDs/keys, never array-derived wire identities. Include +/// every resource needed by the advertised roles (a conservative union is +/// acceptable). Cross-port overlaps are allowed in the inventory and rejected +/// only when both ports are enabled. Fixed claims must be disjoint from every +/// UART, even disabled ones. Do not put USB in either UART or reserved claims. +pub trait PortCatalog { + const DESCRIPTORS: [PortDescriptor; N]; + const RESERVED_RESOURCES: &'static [u16] = &[]; +} + +/// Checks an inventory at runtime or in a platform constant. +/// +/// Rejects duplicate port/field IDs, keys, choices, bauds, per-port resource +/// claims and reserved claims, and overlaps with reserved resources. Labels +/// must be the canonical `Role` labels. All failures use `Inconsistent`; +/// field-local or related identities are included when available. +/// [`crate::Ports::new`] and generated metadata also enforce this in a const. +/// +/// ```compile_fail +/// use kaptan_config::FieldId; +/// use kaptan_ports::*; +/// struct Bad; +/// impl PortCatalog<1> for Bad { +/// const DESCRIPTORS: [PortDescriptor; 1] = [PortDescriptor { +/// id: PortId::new(1), +/// role_field: FieldId::new(10), baud_field: FieldId::new(10), +/// role_key: "uart.role", baud_key: "uart.baud", +/// supported_roles: Role::CONFIG_ENUM_OPTIONS, +/// allowed_bauds: &[115_200], resources: &[], +/// }]; +/// } +/// const INVALID: Ports = Ports::new([ +/// PortSettings { role: Role::Disabled, baud: 115_200 }, +/// ]); +/// ``` +pub const fn validate_catalog( + descriptors: &[PortDescriptor; N], + reserved_resources: &[u16], +) -> Result<(), ConfigViolation> { + let rule = ValidationRule::Inconsistent; + if N > 256 || duplicates(reserved_resources) { + return Err(ConfigViolation::global(rule)); + } + let mut i = 0; + while i < N { + let port = &descriptors[i]; + if port.role_field.get() == port.baud_field.get() || same_text(port.role_key, port.baud_key) + { + return Err(ConfigViolation::related( + port.role_field, + port.baud_field, + rule, + )); + } + if !valid_text(port.role_key) || !valid_text(port.baud_key) { + return Err(ConfigViolation::field(port.role_field, rule)); + } + let fields = [port.role_field, port.baud_field]; + let keys = [port.role_key, port.baud_key]; + let mut j = 0; + while j < i { + let other = &descriptors[j]; + if port.id.get() == other.id.get() { + return Err(ConfigViolation::related( + port.role_field, + other.role_field, + rule, + )); + } + let other_fields = [other.role_field, other.baud_field]; + let other_keys = [other.role_key, other.baud_key]; + let mut a = 0; + while a < 2 { + let mut b = 0; + while b < 2 { + if fields[a].get() == other_fields[b].get() || same_text(keys[a], other_keys[b]) + { + return Err(ConfigViolation::related(fields[a], other_fields[b], rule)); + } + b += 1; + } + a += 1; + } + j += 1; + } + let mut disabled = false; + j = 0; + while j < port.supported_roles.len() { + let (value, label) = port.supported_roles[j]; + let mut canonical = false; + let mut k = 0; + while k < Role::CONFIG_ENUM_OPTIONS.len() { + let option = Role::CONFIG_ENUM_OPTIONS[k]; + if value == option.0 && same_text(label, option.1) { + canonical = true; + } + k += 1; + } + if !canonical { + return Err(ConfigViolation::field(port.role_field, rule)); + } + k = 0; + while k < j { + if port.supported_roles[k].0 == value { + return Err(ConfigViolation::field(port.role_field, rule)); + } + k += 1; + } + if value == Role::Disabled as u8 { + disabled = true; + } + j += 1; + } + if !disabled { + return Err(ConfigViolation::field(port.role_field, rule)); + } + if port.allowed_bauds.is_empty() { + return Err(ConfigViolation::field(port.baud_field, rule)); + } + j = 0; + while j < port.allowed_bauds.len() { + if port.allowed_bauds[j] == 0 { + return Err(ConfigViolation::field(port.baud_field, rule)); + } + let mut k = 0; + while k < j { + if port.allowed_bauds[j] == port.allowed_bauds[k] { + return Err(ConfigViolation::field(port.baud_field, rule)); + } + k += 1; + } + j += 1; + } + if duplicates(port.resources) { + return Err(ConfigViolation::field(port.role_field, rule)); + } + j = 0; + while j < port.resources.len() { + let mut k = 0; + while k < reserved_resources.len() { + if port.resources[j] == reserved_resources[k] { + return Err(ConfigViolation::field(port.role_field, rule)); + } + k += 1; + } + j += 1; + } + i += 1; + } + Ok(()) +} + +const fn duplicates(values: &[u16]) -> bool { + let mut i = 0; + while i < values.len() { + let mut j = 0; + while j < i { + if values[i] == values[j] { + return true; + } + j += 1; + } + i += 1; + } + false +} + +const fn same_text(left: &str, right: &str) -> bool { + let left = left.as_bytes(); + let right = right.as_bytes(); + if left.len() != right.len() { + return false; + } + let mut i = 0; + while i < left.len() { + if left[i] != right[i] { + return false; + } + i += 1; + } + true +} + +const fn valid_text(text: &str) -> bool { + if text.is_empty() || text.len() > u8::MAX as usize { + return false; + } + let bytes = text.as_bytes(); + let mut i = 0; + while i < bytes.len() { + if bytes[i] == 0 { + return false; + } + i += 1; + } + true +} diff --git a/firmware/libs/kaptan-ports/src/lib.rs b/firmware/libs/kaptan-ports/src/lib.rs new file mode 100644 index 0000000..b40be83 --- /dev/null +++ b/firmware/libs/kaptan-ports/src/lib.rs @@ -0,0 +1,334 @@ +//! Portable, restart-only UART configuration and boot planning. +//! +//! A platform supplies a [`PortCatalog`] with stable UART/field identities and +//! opaque resource tokens for pins, peripherals and DMA streams. USB is +//! initialized separately and must not appear in this inventory. +//! +//! Embed [`Ports`] as an ordinary child of a `#[derive(kaptan_config::Config)]` +//! root. At full-candidate validation, the root's `ConfigModel::validate` should +//! call [`Ports::validate_plan`] with the candidate's publication budget. Keep +//! the returned plan in the boot-active view managed by `ConfigManager`; execute +//! it only at boot. Committing another candidate must not reconfigure hardware. +//! +//! ``` +//! use kaptan_config::FieldId; +//! use kaptan_ports::{PortCatalog, PortDescriptor, PortId, PortSettings, Ports, Role}; +//! +//! struct BoardPorts; +//! impl PortCatalog<1> for BoardPorts { +//! const DESCRIPTORS: [PortDescriptor; 1] = [PortDescriptor { +//! id: PortId::new(4), +//! role_field: FieldId::new(208), baud_field: FieldId::new(209), +//! role_key: "ports.uart4.role", baud_key: "ports.uart4.baud", +//! supported_roles: Role::CONFIG_ENUM_OPTIONS, +//! allowed_bauds: &[115_200], resources: &[40, 41, 42], +//! }]; +//! const RESERVED_RESOURCES: &'static [u16] = &[90]; +//! } +//! const FACTORY: Ports = Ports::new([ +//! PortSettings { role: Role::Mtf01, baud: 115_200 }, +//! ]); +//! let plan = FACTORY.validate_plan(1_000, 200).unwrap(); +//! assert_eq!(plan.get(PortId::new(4)).unwrap().role, Role::Mtf01); +//! ``` + +#![no_std] +#![forbid(unsafe_code)] + +mod catalog; +mod schema; + +use core::marker::PhantomData; +use kaptan_config::{ + Config, ConfigValue, ConfigViolation, FieldDescriptor, FieldId, FieldWriteError, + ValidationRule, ValueKind, +}; + +pub use catalog::{PortCatalog, PortDescriptor, validate_catalog}; + +/// Stable platform-assigned identity, independent of catalog array position. +#[derive(Clone, Copy, Debug, Eq, Ord, PartialEq, PartialOrd, Hash)] +#[repr(transparent)] +pub struct PortId(u8); + +impl PortId { + /// All `u8` values are available to the platform's stable UART namespace. + pub const fn new(value: u8) -> Self { + Self(value) + } + + /// Returns the stable numeric identity. + pub const fn get(self) -> u8 { + self.0 + } +} + +/// UART service selected for the next boot. Wire values must never be reused. +#[derive(Clone, Copy, Debug, Eq, PartialEq, kaptan_config::ConfigEnum)] +#[repr(u8)] +pub enum Role { + /// No UART initialization or resource claims. + Disabled = 0, + /// Kaptan MAVLink. + #[config_enum(label = "KaptanMavlink")] + Mavlink = 1, + /// MTF-01 range/optical-flow service. + Mtf01 = 2, +} + +/// Candidate scalar values for one UART, in native KCFG widths. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct PortSettings { + pub role: Role, + pub baud: u32, +} + +/// Editable schema child backed by exactly `N` UART settings. +/// +/// Construction checks the catalog at compile time, but leaves candidate values +/// for local writes and complete validation. Settings passed to `new` follow +/// catalog declaration order; field and port lookups always use explicit IDs. +pub struct Ports, const N: usize> { + settings: [PortSettings; N], + catalog: PhantomData C>, +} + +impl, const N: usize> Copy for Ports {} + +impl, const N: usize> Clone for Ports { + fn clone(&self) -> Self { + *self + } +} + +impl, const N: usize> Ports { + const CHECK_CATALOG: () = assert!( + validate_catalog(&C::DESCRIPTORS, C::RESERVED_RESOURCES).is_ok(), + "invalid UART port catalog" + ); + + // Nested arrays avoid unstable generic const expressions such as [T; 2 * N]. + const FIELDS: [[FieldDescriptor; 2]; N] = { + let () = Self::CHECK_CATALOG; + schema::fields(&C::DESCRIPTORS) + }; + + /// Creates an editable candidate, including in a board's factory constant. + pub const fn new(settings: [PortSettings; N]) -> Self { + let () = Self::CHECK_CATALOG; + Self { + settings, + catalog: PhantomData, + } + } + + /// Reads candidate settings by stable UART identity. + pub fn get(&self, id: PortId) -> Option<&PortSettings> { + C::DESCRIPTORS + .iter() + .position(|descriptor| descriptor.id == id) + .map(|index| &self.settings[index]) + } + + /// Validates inventory, local values and resource exclusivity. + /// + /// This has no publication budget. Platforms with scheduled MAVLink traffic + /// must use [`Self::validate_plan`] when validating the complete candidate. + pub fn plan(&self) -> Result, ConfigViolation> { + self.validate_plan(0, 0) + } + + /// Fully validates and snapshots a boot plan with an 8N1 transmit budget. + /// + /// Both arguments are **wire bytes per second**. `reserve` is an additive + /// allowance for unscheduled traffic, not a percentage. Each enabled MAVLink + /// UART must fit `(scheduled_bytes_per_second + reserve) * 10 <= baud`. + /// Supply framing-inclusive publication costs computed from the complete + /// candidate. Arithmetic is checked in `u32`; overflow is `AboveMaximum`, + /// insufficient baud is `UnsupportedCapability`, both on the baud field. + /// Disabled/MTF-01 UARTs and fixed USB do not consume this budget. + pub fn validate_plan( + &self, + scheduled_bytes_per_second: u32, + reserve: u32, + ) -> Result, ConfigViolation> { + PortPlan::validate( + &C::DESCRIPTORS, + self.settings, + C::RESERVED_RESOURCES, + scheduled_bytes_per_second, + reserve, + ) + } +} + +impl, const N: usize> Config for Ports { + const FIELD_COUNT: usize = { + let () = Self::CHECK_CATALOG; + N * 2 + }; + const SNAPSHOT_VALUE_BYTES: usize = N * 5; + const SCHEMA_ID: [u8; 16] = + schema::fingerprint(&Self::FIELDS, &C::DESCRIPTORS, C::RESERVED_RESOURCES); + + fn descriptor(index: usize) -> Option<&'static FieldDescriptor> { + Self::FIELDS.get(index / 2).map(|fields| &fields[index % 2]) + } + + fn field_value(&self, id: FieldId) -> Option { + for (descriptor, settings) in C::DESCRIPTORS.iter().zip(&self.settings) { + if id == descriptor.role_field { + return Some(ConfigValue::U8(settings.role.wire_value())); + } + if id == descriptor.baud_field { + return Some(ConfigValue::U32(settings.baud)); + } + } + None + } + + fn set_field(&mut self, id: FieldId, value: ConfigValue) -> Result<(), FieldWriteError> { + for (descriptor, settings) in C::DESCRIPTORS.iter().zip(&mut self.settings) { + if id == descriptor.role_field { + let ConfigValue::U8(raw) = value else { + return Err(wrong_type(id, ValueKind::U8, value)); + }; + let role = Role::try_from_wire(raw).ok_or(FieldWriteError::Violation( + ConfigViolation::field(id, ValidationRule::InvalidEnumValue), + ))?; + validate_role(descriptor, role).map_err(FieldWriteError::Violation)?; + // Commit only the accepted scalar: failed writes leave no changes. + settings.role = role; + return Ok(()); + } + if id == descriptor.baud_field { + let ConfigValue::U32(baud) = value else { + return Err(wrong_type(id, ValueKind::U32, value)); + }; + validate_baud(descriptor, baud).map_err(FieldWriteError::Violation)?; + settings.baud = baud; + return Ok(()); + } + } + Err(FieldWriteError::UnknownField(id)) + } + + fn validate(&self) -> Result<(), ConfigViolation> { + self.plan().map(|_| ()) + } +} + +fn wrong_type(field: FieldId, expected: ValueKind, value: ConfigValue) -> FieldWriteError { + FieldWriteError::WrongType { + field, + expected, + actual: value.kind(), + } +} + +fn validate_role(descriptor: &PortDescriptor, role: Role) -> Result<(), ConfigViolation> { + if descriptor + .supported_roles + .iter() + .any(|option| option.0 == role.wire_value()) + { + Ok(()) + } else { + Err(ConfigViolation::field( + descriptor.role_field, + ValidationRule::UnsupportedCapability, + )) + } +} + +fn validate_baud(descriptor: &PortDescriptor, baud: u32) -> Result<(), ConfigViolation> { + if descriptor.allowed_bauds.contains(&baud) { + Ok(()) + } else { + Err(ConfigViolation::field( + descriptor.baud_field, + ValidationRule::UnsupportedCapability, + )) + } +} + +/// Immutable, validated boot decisions. Contains no physical resource handles. +/// +/// Construction always validates the catalog and candidate decisions. +/// Iteration includes disabled ports, in catalog declaration order. Executors +/// match the stable ID and role, and skip initialization for `Disabled`. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct PortPlan { + ids: [PortId; N], + settings: [PortSettings; N], +} + +impl PortPlan { + /// Validates projected settings against a borrowed catalog, including its + /// resources and the same 8N1 budget as [`Ports::validate_plan`]. This lets + /// platforms consume a board catalog without introducing another catalog type. + pub fn validate( + descriptors: &[PortDescriptor; N], + settings: [PortSettings; N], + reserved_resources: &[u16], + scheduled_bytes_per_second: u32, + reserve: u32, + ) -> Result { + validate_catalog(descriptors, reserved_resources)?; + for (index, (descriptor, setting)) in descriptors.iter().zip(&settings).enumerate() { + validate_role(descriptor, setting.role)?; + validate_baud(descriptor, setting.baud)?; + if setting.role == Role::Disabled { + continue; + } + for (previous, other) in descriptors[..index].iter().zip(&settings) { + if other.role != Role::Disabled + && descriptor + .resources + .iter() + .any(|resource| previous.resources.contains(resource)) + { + return Err(ConfigViolation::related( + descriptor.role_field, + previous.role_field, + ValidationRule::Inconsistent, + )); + } + } + } + for (descriptor, setting) in descriptors.iter().zip(&settings) { + if setting.role == Role::Mavlink { + let required = scheduled_bytes_per_second + .checked_add(reserve) + .and_then(|bytes| bytes.checked_mul(10)) + .ok_or(ConfigViolation::field( + descriptor.baud_field, + ValidationRule::AboveMaximum, + ))?; + if required > setting.baud { + return Err(ConfigViolation::field( + descriptor.baud_field, + ValidationRule::UnsupportedCapability, + )); + } + } + } + Ok(Self { + ids: descriptors.map(|descriptor| descriptor.id), + settings, + }) + } + + /// Returns the boot decision for a stable UART identity. + pub fn get(&self, id: PortId) -> Option<&PortSettings> { + self.ids + .iter() + .position(|&port| port == id) + .map(|index| &self.settings[index]) + } + + /// Reads all boot decisions without exposing mutable settings. + pub fn iter(&self) -> impl ExactSizeIterator + '_ { + self.ids.iter().copied().zip(self.settings.iter()) + } +} diff --git a/firmware/libs/kaptan-ports/src/schema.rs b/firmware/libs/kaptan-ports/src/schema.rs new file mode 100644 index 0000000..a7203a2 --- /dev/null +++ b/firmware/libs/kaptan-ports/src/schema.rs @@ -0,0 +1,105 @@ +use kaptan_config::{ConfigValue, FieldDescriptor, FieldId, ValueKind, validation}; + +use crate::PortDescriptor; + +pub(crate) const fn fields( + ports: &[PortDescriptor; N], +) -> [[FieldDescriptor; 2]; N] { + let empty = FieldDescriptor { + id: FieldId::new(1), + display_key: "", + kind: ValueKind::U8, + minimum: None, + maximum: None, + enum_options: &[], + unit: "", + finite: false, + nonzero: false, + }; + let mut fields = [[empty; 2]; N]; + let mut i = 0; + while i < N { + let port = &ports[i]; + let mut minimum = u32::MAX; + let mut maximum = 0; + let mut j = 0; + while j < port.allowed_bauds.len() { + let baud = port.allowed_bauds[j]; + if baud < minimum { + minimum = baud; + } + if baud > maximum { + maximum = baud; + } + j += 1; + } + fields[i] = [ + FieldDescriptor { + id: port.role_field, + display_key: port.role_key, + enum_options: port.supported_roles, + ..empty + }, + FieldDescriptor { + id: port.baud_field, + display_key: port.baud_key, + kind: ValueKind::U32, + minimum: Some(ConfigValue::U32(minimum)), + maximum: Some(ConfigValue::U32(maximum)), + unit: "baud", + ..empty + }, + ]; + i += 1; + } + fields +} + +// Reuse Config's schema combiner, including constraints not expressible by a +// scalar descriptor (whitelist holes, port IDs and resources). +// Ordering is schema-significant, like derived Config children; IDs themselves +// remain stable when descriptors are reordered. +pub(crate) const fn fingerprint( + fields: &[[FieldDescriptor; 2]; N], + ports: &[PortDescriptor; N], + reserved: &[u16], +) -> [u8; 16] { + let mut state = *b"KAPTAN-PORTS-v01"; + state = number(state, N as u64); + let mut i = 0; + while i < N { + state = validation::__combine_schema_ids(&[state, validation::__schema_id(&fields[i])]); + state = number(state, ports[i].id.get() as u64); + state = number(state, ports[i].allowed_bauds.len() as u64); + let mut j = 0; + while j < ports[i].allowed_bauds.len() { + state = number(state, ports[i].allowed_bauds[j] as u64); + j += 1; + } + state = number(state, ports[i].resources.len() as u64); + j = 0; + while j < ports[i].resources.len() { + state = number(state, ports[i].resources[j] as u64); + j += 1; + } + i += 1; + } + state = number(state, reserved.len() as u64); + i = 0; + while i < reserved.len() { + state = number(state, reserved[i] as u64); + i += 1; + } + state +} + +const fn number(state: [u8; 16], value: u64) -> [u8; 16] { + let bytes = value.to_le_bytes(); + let mut encoded = [0; 16]; + let mut i = 0; + while i < bytes.len() { + encoded[i] = bytes[i]; + i += 1; + } + validation::__combine_schema_ids(&[state, encoded]) +} diff --git a/firmware/libs/kaptan-ports/tests/ports.rs b/firmware/libs/kaptan-ports/tests/ports.rs new file mode 100644 index 0000000..7d280b6 --- /dev/null +++ b/firmware/libs/kaptan-ports/tests/ports.rs @@ -0,0 +1,642 @@ +use kaptan_config::{ + Config, ConfigManager, ConfigModel, ConfigStore, ConfigValue, ConfigViolation, FieldDescriptor, + FieldId, FieldWriteError, KcfgExporter, KcfgImporter, ReplaceError, ValidationRule, ValueKind, + decode_kcfg, encode_edited_kcfg, +}; +use kaptan_ports::{ + PortCatalog, PortDescriptor, PortId, PortPlan, PortSettings, Ports, Role, validate_catalog, +}; + +const UART1: PortDescriptor = PortDescriptor { + id: PortId::new(1), + role_field: FieldId::new(201), + baud_field: FieldId::new(202), + role_key: "ports.uart1.role", + baud_key: "ports.uart1.baud", + supported_roles: Role::CONFIG_ENUM_OPTIONS, + allowed_bauds: &[9_600, 57_600, 115_200], + resources: &[10, 11], +}; +const UART4: PortDescriptor = PortDescriptor { + id: PortId::new(4), + role_field: FieldId::new(207), + baud_field: FieldId::new(208), + role_key: "ports.uart4.role", + baud_key: "ports.uart4.baud", + resources: &[40, 41], + ..UART1 +}; +const OFF: PortSettings = PortSettings { + role: Role::Disabled, + baud: 115_200, +}; +const MAVLINK: PortSettings = PortSettings { + role: Role::Mavlink, + ..OFF +}; +const MTF01: PortSettings = PortSettings { + role: Role::Mtf01, + ..OFF +}; + +// Intentionally not numeric order; neither type needs Clone/Copy bounds. +struct Two; +impl PortCatalog<2> for Two { + const DESCRIPTORS: [PortDescriptor; 2] = [UART4, UART1]; + const RESERVED_RESOURCES: &'static [u16] = &[90, 91]; +} +const FACTORY: Ports = Ports::new([MTF01, MAVLINK]); + +struct Conflict; +impl PortCatalog<2> for Conflict { + const DESCRIPTORS: [PortDescriptor; 2] = [ + PortDescriptor { + resources: &[40, 10], + ..UART4 + }, + UART1, + ]; +} + +struct Limited; +impl PortCatalog<2> for Limited { + const DESCRIPTORS: [PortDescriptor; 2] = [ + PortDescriptor { + supported_roles: &[Role::CONFIG_ENUM_OPTIONS[0], Role::CONFIG_ENUM_OPTIONS[2]], + ..UART4 + }, + UART1, + ]; +} + +struct Eight; +impl PortCatalog<8> for Eight { + const DESCRIPTORS: [PortDescriptor; 8] = [ + UART4, + UART1, + PortDescriptor { + id: PortId::new(2), + role_field: FieldId::new(203), + baud_field: FieldId::new(204), + role_key: "ports.uart2.role", + baud_key: "ports.uart2.baud", + resources: &[20], + ..UART1 + }, + PortDescriptor { + id: PortId::new(3), + role_field: FieldId::new(205), + baud_field: FieldId::new(206), + role_key: "ports.uart3.role", + baud_key: "ports.uart3.baud", + resources: &[30], + ..UART1 + }, + PortDescriptor { + id: PortId::new(5), + role_field: FieldId::new(209), + baud_field: FieldId::new(210), + role_key: "ports.uart5.role", + baud_key: "ports.uart5.baud", + resources: &[50], + ..UART1 + }, + PortDescriptor { + id: PortId::new(6), + role_field: FieldId::new(211), + baud_field: FieldId::new(212), + role_key: "ports.uart6.role", + baud_key: "ports.uart6.baud", + resources: &[60], + ..UART1 + }, + PortDescriptor { + id: PortId::new(7), + role_field: FieldId::new(213), + baud_field: FieldId::new(214), + role_key: "ports.uart7.role", + baud_key: "ports.uart7.baud", + resources: &[70], + ..UART1 + }, + PortDescriptor { + id: PortId::new(8), + role_field: FieldId::new(215), + baud_field: FieldId::new(216), + role_key: "ports.uart8.role", + baud_key: "ports.uart8.baud", + resources: &[80], + ..UART1 + }, + ]; +} + +#[derive(Debug, PartialEq)] +enum Started { + UsbMavlink, + Uart(PortId, Role, u32), +} + +// A host executor consumes the actual immutable plan, not a parallel policy model. +fn execute(plan: &PortPlan) -> Vec { + let mut started = vec![Started::UsbMavlink]; + for (id, settings) in plan.iter() { + match settings.role { + Role::Disabled => {} + Role::Mavlink | Role::Mtf01 => { + started.push(Started::Uart(id, settings.role, settings.baud)) + } + } + } + started +} + +#[test] +fn crossed_roles_follow_stable_ids_and_published_plan_is_a_snapshot() { + let mut candidate = FACTORY; + let boot = candidate.plan().unwrap(); + candidate + .set_field(UART1.role_field, ConfigValue::U8(2)) + .unwrap(); + candidate + .set_field(UART4.role_field, ConfigValue::U8(1)) + .unwrap(); + candidate + .set_field(UART4.baud_field, ConfigValue::U32(57_600)) + .unwrap(); + let crossed = candidate.validate_plan(1_000, 200).unwrap(); + assert_eq!( + execute(&crossed), + vec![ + Started::UsbMavlink, + Started::Uart(PortId::new(4), Role::Mavlink, 57_600), + Started::Uart(PortId::new(1), Role::Mtf01, 115_200), + ] + ); + assert_eq!(boot.get(PortId::new(4)), Some(&MTF01)); + assert_eq!(boot.get(PortId::new(1)), Some(&MAVLINK)); + assert!(boot.get(PortId::new(0)).is_none()); + assert_eq!(crossed.iter().len(), 2); + + struct Reordered; + impl PortCatalog<2> for Reordered { + const DESCRIPTORS: [PortDescriptor; 2] = [UART1, UART4]; + } + let reordered = Ports::::new([MAVLINK, MTF01]).plan().unwrap(); + for (id, settings) in boot.iter() { + assert_eq!(reordered.get(id), Some(settings)); + } +} + +#[test] +fn resource_conflicts_block_execution_but_disabled_ports_claim_nothing() { + let mut candidate = Ports::::new([MTF01, MAVLINK]); + assert_eq!( + candidate.plan().unwrap_err(), + ConfigViolation::related( + UART1.role_field, + UART4.role_field, + ValidationRule::Inconsistent, + ) + ); + candidate + .set_field(UART1.role_field, ConfigValue::U8(0)) + .unwrap(); + assert_eq!( + execute(&candidate.plan().unwrap()), + vec![ + Started::UsbMavlink, + Started::Uart(PortId::new(4), Role::Mtf01, 115_200), + ] + ); + candidate + .set_field(UART4.role_field, ConfigValue::U8(0)) + .unwrap(); + assert_eq!( + execute(&candidate.plan().unwrap()), + vec![Started::UsbMavlink] + ); +} + +#[test] +fn eight_ports_allow_independent_sensor_services() { + let mut settings = [OFF; 8]; + settings[2] = MAVLINK; // UART2 + settings[7] = MTF01; // UART8 + let mut candidate = Ports::::new(settings); + let plan = candidate.validate_plan(5_000, 760).unwrap(); + assert_eq!(plan.iter().len(), 8); + assert_eq!( + execute(&plan), + vec![ + Started::UsbMavlink, + Started::Uart(PortId::new(2), Role::Mavlink, 115_200), + Started::Uart(PortId::new(8), Role::Mtf01, 115_200), + ] + ); + candidate + .set_field(UART1.role_field, ConfigValue::U8(2)) + .unwrap(); + let plan = candidate.plan().unwrap(); + assert_eq!(plan.get(UART1.id), Some(&MTF01)); + assert_eq!(plan.get(PortId::new(8)), Some(&MTF01)); +} + +#[test] +fn local_setters_keep_native_widths_enforce_choices_and_roll_back() { + let mut ports = Ports::::new([MTF01, OFF]); + for (field, value, expected) in [ + ( + UART4.role_field, + ConfigValue::U8(1), + ValidationRule::UnsupportedCapability, + ), + ( + UART4.role_field, + ConfigValue::U8(255), + ValidationRule::InvalidEnumValue, + ), + ( + UART4.baud_field, + ConfigValue::U32(0), + ValidationRule::UnsupportedCapability, + ), + ( + UART4.baud_field, + ConfigValue::U32(38_400), + ValidationRule::UnsupportedCapability, + ), + ] { + assert_eq!( + ports.set_field(field, value), + Err(FieldWriteError::Violation(ConfigViolation::field( + field, expected + ))) + ); + assert_eq!(ports.get(PortId::new(4)), Some(&MTF01)); + } + for (field, value, expected) in [ + (UART4.role_field, ConfigValue::U32(2), ValueKind::U8), + (UART4.baud_field, ConfigValue::U16(9_600), ValueKind::U32), + ] { + assert_eq!( + ports.set_field(field, value), + Err(FieldWriteError::WrongType { + field, + expected, + actual: value.kind(), + }) + ); + } + let missing = FieldId::new(999); + assert_eq!(ports.field_value(missing), None); + assert_eq!( + ports.set_field(missing, ConfigValue::U8(0)), + Err(FieldWriteError::UnknownField(missing)) + ); + // A disabled port still retains a locally valid baud for the next role. + assert!( + Ports::::new([PortSettings { baud: 0, ..OFF }, OFF]) + .plan() + .is_err() + ); + assert!(Ports::::new([MAVLINK, OFF]).plan().is_err()); +} + +#[test] +fn budgets_include_reserve_and_check_both_overflows_on_each_mavlink_uart() { + let low_baud = PortSettings { + baud: 9_600, + ..MAVLINK + }; + let ports = Ports::::new([low_baud, MAVLINK]); + assert!(ports.validate_plan(900, 60).is_ok()); + assert_eq!( + ports.validate_plan(900, 61).unwrap_err(), + ConfigViolation::field(UART4.baud_field, ValidationRule::UnsupportedCapability) + ); + for (scheduled, reserve) in [(u32::MAX, 1), (u32::MAX / 10 + 1, 0), (0, u32::MAX)] { + assert_eq!( + ports.validate_plan(scheduled, reserve).unwrap_err(), + ConfigViolation::field(UART4.baud_field, ValidationRule::AboveMaximum) + ); + } + let second = Ports::::new([MAVLINK, low_baud]); + assert_eq!( + second.validate_plan(961, 0).unwrap_err().primary(), + Some(UART1.baud_field) + ); + // No UART MAVLink transmitter: the USB budget is platform-owned. + assert!( + Ports::::new([MTF01, OFF]) + .validate_plan(u32::MAX, u32::MAX) + .is_ok() + ); +} + +#[test] +fn inventory_rejects_ambiguous_metadata_and_fixed_resource_claims() { + assert!(validate_catalog(&Two::DESCRIPTORS, &[90, 91]).is_ok()); + assert!(validate_catalog(&Conflict::DESCRIPTORS, &[]).is_ok()); + let invalid_ports = [ + PortDescriptor { + id: UART1.id, + ..UART4 + }, + PortDescriptor { + role_field: UART1.baud_field, + ..UART4 + }, + PortDescriptor { + baud_field: UART4.role_field, + ..UART4 + }, + PortDescriptor { + baud_key: UART1.role_key, + ..UART4 + }, + PortDescriptor { + role_key: UART4.baud_key, + ..UART4 + }, + PortDescriptor { + role_key: "", + ..UART4 + }, + PortDescriptor { + baud_key: "bad\0key", + ..UART4 + }, + PortDescriptor { + supported_roles: &[(0, "Disabled"), (0, "Disabled")], + ..UART4 + }, + PortDescriptor { + supported_roles: &[(0, "Disabled"), (3, "Unknown")], + ..UART4 + }, + PortDescriptor { + supported_roles: &[(1, "KaptanMavlink")], + ..UART4 + }, + PortDescriptor { + supported_roles: &[(0, "wrong label")], + ..UART4 + }, + PortDescriptor { + allowed_bauds: &[], + ..UART4 + }, + PortDescriptor { + allowed_bauds: &[0], + ..UART4 + }, + PortDescriptor { + allowed_bauds: &[115_200, 115_200], + ..UART4 + }, + PortDescriptor { + resources: &[40, 40], + ..UART4 + }, + ]; + for port in invalid_ports { + assert_eq!( + validate_catalog(&[port, UART1], &[]).unwrap_err().rule(), + ValidationRule::Inconsistent + ); + } + assert!(validate_catalog(&Two::DESCRIPTORS, &[40]).is_err()); + assert!(validate_catalog(&Two::DESCRIPTORS, &[90, 90]).is_err()); + const NO_MTF: PortDescriptor = PortDescriptor { + supported_roles: &[Role::CONFIG_ENUM_OPTIONS[0], Role::CONFIG_ENUM_OPTIONS[1]], + ..UART1 + }; + assert!(validate_catalog(&[NO_MTF], &[]).is_ok()); +} + +#[test] +fn descriptor_tables_and_fingerprint_include_catalog_constraints() { + assert_eq!( as Config>::FIELD_COUNT, 4); + assert_eq!( as Config>::FIELD_COUNT, 16); + assert_eq!( as Config>::SNAPSHOT_VALUE_BYTES, 40); + assert_eq!( + Ports::::descriptor(0).unwrap().enum_options, + &[(0, "Disabled"), (2, "Mtf01")] + ); + let baud = Ports::::descriptor(1).unwrap(); + assert_eq!( + (baud.kind, baud.minimum, baud.maximum), + ( + ValueKind::U32, + Some(ConfigValue::U32(9_600)), + Some(ConfigValue::U32(115_200)), + ) + ); + assert!(Ports::::descriptor(4).is_none()); + assert!(Ports::::descriptor(usize::MAX).is_none()); + assert_ne!( + as Config>::SCHEMA_ID, + as Config>::SCHEMA_ID + ); + + struct WhitelistHole; + impl PortCatalog<2> for WhitelistHole { + const DESCRIPTORS: [PortDescriptor; 2] = [ + PortDescriptor { + allowed_bauds: &[9_600, 115_200], + ..UART4 + }, + UART1, + ]; + const RESERVED_RESOURCES: &'static [u16] = Two::RESERVED_RESOURCES; + } + // Same min/max, different exact whitelist must still change schema identity. + assert_eq!( + Ports::::descriptor(1), + Ports::::descriptor(1) + ); + assert_ne!( + as Config>::SCHEMA_ID, + as Config>::SCHEMA_ID + ); +} + +// Real derived root plus ConfigModel, using the same Ports child and actual +// KCFG import/export and restart-only lifecycle as a platform integration. +#[derive(Clone, kaptan_config::Config)] +struct Publications { + #[config(id = 50, key = "telemetry.wire_bytes_per_second", unit = "B/s")] + bytes_per_second: u32, +} + +#[derive(Clone, kaptan_config::Config)] +struct Root { + ports: Ports, + publications: Publications, +} + +struct Active { + ports: PortPlan<2>, + bytes_per_second: u32, +} + +impl ConfigModel for Root { + type Active = Active; + type Context = u32; // reserve wire bytes/sec + const SCHEMA_ID: [u8; 16] = ::SCHEMA_ID; + fn descriptor(index: u16) -> Option<&'static FieldDescriptor> { + ::descriptor(usize::from(index)) + } + fn field_count() -> u16 { + ::FIELD_COUNT as u16 + } + fn field_value(&self, id: FieldId) -> Option { + Config::field_value(self, id) + } + fn active_field_value(active: &Active, id: FieldId) -> Option { + if id == FieldId::new(50) { + return Some(ConfigValue::U32(active.bytes_per_second)); + } + for descriptor in Two::DESCRIPTORS { + let settings = active.ports.get(descriptor.id)?; + if id == descriptor.role_field { + return Some(ConfigValue::U8(settings.role.wire_value())); + } + if id == descriptor.baud_field { + return Some(ConfigValue::U32(settings.baud)); + } + } + None + } + fn set_field(&mut self, id: FieldId, value: ConfigValue) -> Result<(), FieldWriteError> { + Config::set_field(self, id, value) + } + fn validate(&self, reserve: &u32) -> Result { + Config::validate(self)?; + Ok(Active { + ports: self + .ports + .validate_plan(self.publications.bytes_per_second, *reserve)?, + bytes_per_second: self.publications.bytes_per_second, + }) + } +} + +#[derive(Default)] +struct MemoryStore(Option); +impl ConfigStore for MemoryStore { + type Error = core::convert::Infallible; + fn load(&mut self, _: &Root) -> Result, Self::Error> { + Ok(self.0.clone()) + } + fn persist(&mut self, candidate: &Root) -> Result<(), Self::Error> { + self.0 = Some(candidate.clone()); + Ok(()) + } +} + +fn factory() -> Root { + Root { + ports: FACTORY, + publications: Publications { + bytes_per_second: 900, + }, + } +} + +fn import_edit(factory: &Root, mut edit: impl FnMut(FieldId, ConfigValue) -> ConfigValue) -> Root { + let exporter = KcfgExporter::new(factory, factory, false).unwrap(); + let mut bytes = vec![0; exporter.length()]; + assert_eq!(exporter.read(0, &mut bytes).unwrap(), bytes.len()); + let artifact = + decode_kcfg(&bytes, &mut [0; kaptan_config::KCFG_FIELD_ID_SCRATCH_BYTES]).unwrap(); + let mut edited = vec![0; bytes.len()]; + let edited = encode_edited_kcfg(&artifact, true, &mut edited, |field| { + edit(field.id(), field.value()) + }) + .unwrap(); + let mut importer = KcfgImporter::::new(factory).unwrap(); + for chunk in edited.chunks(7) { + importer.push(factory, chunk).unwrap(); + } + importer.finish().unwrap().candidate +} + +#[test] +fn real_kcfg_composition_exports_native_fields_and_commits_only_for_restart() { + let factory = factory(); + let exporter = KcfgExporter::new(&factory, &factory, false).unwrap(); + let mut bytes = vec![0; exporter.length()]; + exporter.read(0, &mut bytes).unwrap(); + let artifact = + decode_kcfg(&bytes, &mut [0; kaptan_config::KCFG_FIELD_ID_SCRATCH_BYTES]).unwrap(); + assert_eq!(artifact.schema_id(), ::SCHEMA_ID); + assert_eq!(artifact.field_count(), 5); + let fields: Vec<_> = artifact.fields().collect(); + assert_eq!( + (fields[0].id(), fields[0].kind(), fields[0].value()), + (UART4.role_field, ValueKind::U8, ConfigValue::U8(2)) + ); + assert_eq!( + (fields[1].id(), fields[1].kind(), fields[1].value()), + (UART4.baud_field, ValueKind::U32, ConfigValue::U32(115_200)) + ); + assert_eq!( + fields[0] + .enum_options() + .map(|option| (option.value(), option.label())) + .collect::>(), + Role::CONFIG_ENUM_OPTIONS + ); + let crossed = import_edit(&factory, |id, value| { + if id == UART4.role_field { + ConfigValue::U8(1) + } else if id == UART1.role_field { + ConfigValue::U8(2) + } else { + value + } + }); + let mut manager = ConfigManager::recover(factory.clone(), 60, MemoryStore::default()).unwrap(); + let before = manager.active().ports; + assert!(manager.replace(crossed).unwrap().restart_required); + assert_eq!(manager.active().ports, before); + let committed = manager.committed().unwrap(); + assert_eq!( + committed.ports.get(PortId::new(4)).unwrap().role, + Role::Mavlink + ); + let restarted = + ConfigManager::recover(factory, 60, MemoryStore(Some(committed.clone()))).unwrap(); + assert_eq!( + restarted.active().ports.get(PortId::new(4)).unwrap().role, + Role::Mavlink + ); + assert!(!restarted.restart_required()); +} + +#[test] +fn imported_publication_budget_must_fit_uart_capacity() { + let factory = factory(); + let mut manager = ConfigManager::recover(factory.clone(), 60, MemoryStore::default()).unwrap(); + // Both edits are locally valid, but together exceed the UART's capacity. + let overloaded = import_edit(&factory, |id, value| { + if id == UART1.baud_field { + ConfigValue::U32(9_600) + } else if id == FieldId::new(50) { + ConfigValue::U32(901) + } else { + value + } + }); + assert!( + matches!(manager.replace(overloaded), Err(ReplaceError::InvalidCandidate(violation)) + if violation.primary() == Some(UART1.baud_field)) + ); + assert!(!manager.restart_required()); + assert_eq!( + manager.committed().unwrap().ports.get(PortId::new(1)), + Some(&MAVLINK) + ); +} diff --git a/firmware/libs/kaptan-safety/src/kani_proofs.rs b/firmware/libs/kaptan-safety/src/kani_proofs.rs new file mode 100644 index 0000000..fef346e --- /dev/null +++ b/firmware/libs/kaptan-safety/src/kani_proofs.rs @@ -0,0 +1,155 @@ +//! Bounded public-operation histories at the synchronous commit boundary. + +use super::*; + +fn healthy() -> SafetyInputs { + SafetyInputs { + estimator_healthy: true, + imu_fresh: true, + phase_authorized: true, + output_healthy: true, + } +} + +fn any_inputs() -> SafetyInputs { + SafetyInputs { + estimator_healthy: kani::any(), + imu_fresh: kani::any(), + phase_authorized: kani::any(), + output_healthy: kani::any(), + } +} + +fn supervisor() -> (SafetySupervisor, u64) { + let dwell: u64 = kani::any(); + kani::assume(dwell > 0); + ( + SafetySupervisor::new(SafetyConfig::new(dwell).unwrap()), + dwell, + ) +} + +#[kani::proof] +fn commit_requires_current_health_and_completed_dwell() { + let (mut safety, dwell) = supervisor(); + let start: u64 = kani::any(); + let now: u64 = kani::any(); + safety.request_arm(Timestamp::from_microseconds(start), healthy()); + + let inputs = any_inputs(); + let admitted = inputs == healthy() + && now + .checked_sub(start) + .is_some_and(|elapsed| elapsed >= dwell); + let mut calls = 0_u8; + let value: u32 = kani::any(); + let outcome = safety.evaluate_and_commit(Timestamp::from_microseconds(now), inputs, || { + calls += 1; + Ok::<_, ()>(value) + }); + assert_eq!(calls, u8::from(admitted)); + if admitted { + assert_eq!(outcome, CommitOutcome::Committed(value)); + assert_eq!(safety.state(), SafetyState::Armed); + } else { + assert!(matches!(outcome, CommitOutcome::Inhibited(_))); + } + kani::cover!(admitted); + kani::cover!(safety.state() == SafetyState::PreArm); + kani::cover!(now < start); + kani::cover!(!inputs.imu_fresh && now >= start); +} + +#[kani::proof] +fn failed_commit_preserves_error_and_latches_inhibition() { + let (mut safety, dwell) = supervisor(); + let start: u64 = kani::any(); + let Some(armed_at) = start.checked_add(dwell) else { + return; + }; + safety.request_arm(Timestamp::from_microseconds(start), healthy()); + let error: u32 = kani::any(); + let mut calls = 0_u8; + let failed = + safety.evaluate_and_commit(Timestamp::from_microseconds(armed_at), healthy(), || { + calls += 1; + Err::<(), _>(error) + }); + assert_eq!(calls, 1); + assert_eq!(failed, CommitOutcome::Failed(error)); + assert_eq!(safety.state(), SafetyState::Failsafe); + assert_eq!( + safety.decision().failure(), + Some(SafetyFailure::CommitFailed) + ); + + // No disarm or epoch reset occurs between failure and this next evaluation. + let next = Timestamp::from_microseconds(kani::any()); + let inputs = any_inputs(); + let outcome = safety.evaluate_and_commit(next, inputs, || { + calls += 1; + Ok::<_, ()>(()) + }); + assert!(matches!(outcome, CommitOutcome::Inhibited(_))); + assert_eq!(calls, 1); + assert_eq!(safety.state(), SafetyState::Failsafe); + kani::cover!(inputs == healthy() && next.as_microseconds() >= armed_at); +} + +#[kani::proof] +fn clock_regression_requires_successful_inhibited_reset() { + let (mut safety, dwell) = supervisor(); + let first: u64 = kani::any(); + let earlier: u64 = kani::any(); + kani::assume(earlier < first); + safety.request_arm(Timestamp::from_microseconds(first), healthy()); + let mut commits = 0_u8; + let regressed = + safety.evaluate_and_commit(Timestamp::from_microseconds(earlier), healthy(), || { + commits += 1; + Ok::<_, ()>(()) + }); + assert!(matches!(regressed, CommitOutcome::Inhibited(_))); + assert_eq!( + safety.decision().failure(), + Some(SafetyFailure::ClockRegression) + ); + safety.request_disarm(); + assert_eq!( + safety + .request_arm(Timestamp::from_microseconds(first), healthy()) + .state(), + SafetyState::Failsafe + ); + + safety.request_disarm(); + let inhibit_succeeds: bool = kani::any(); + let error: u32 = kani::any(); + let mut inhibitions = 0_u8; + let reset = safety.reset_time_epoch(|| { + inhibitions += 1; + if inhibit_succeeds { Ok(()) } else { Err(error) } + }); + assert_eq!(inhibitions, 1); + if inhibit_succeeds { + assert_eq!(reset, Ok(())); + } else { + assert_eq!(reset, Err(ResetError::Inhibit(error))); + assert_eq!(safety.state(), SafetyState::Failsafe); + } + + safety.request_disarm(); + safety.request_arm(Timestamp::from_microseconds(0), healthy()); + let outcome = + safety.evaluate_and_commit(Timestamp::from_microseconds(dwell), healthy(), || { + commits += 1; + Ok::<_, ()>(()) + }); + assert_eq!(commits, u8::from(inhibit_succeeds)); + assert_eq!( + matches!(outcome, CommitOutcome::Committed(())), + inhibit_succeeds + ); + kani::cover!(inhibit_succeeds); + kani::cover!(!inhibit_succeeds); +} diff --git a/firmware/libs/kaptan-safety/src/lib.rs b/firmware/libs/kaptan-safety/src/lib.rs index f8cc2b6..7df0c7d 100644 --- a/firmware/libs/kaptan-safety/src/lib.rs +++ b/firmware/libs/kaptan-safety/src/lib.rs @@ -1,79 +1,80 @@ -//! Bounded safety supervisor for actuator authorization. +//! Bounded safety supervision with a synchronous actuator commit boundary. //! -//! The platform turns estimator, command, and transport health into the -//! explicit inputs below. This crate does not authenticate a command or drive -//! hardware; a reviewed platform must evaluate it before every output commit -//! and make the output port validate the returned opaque permit. +//! Decisions are diagnostics. Only `evaluate_and_commit` invokes an enabled +//! output operation, while holding the issuing supervisor's exclusive borrow. +//! The callback must commit synchronously, not queue future authority. +//! +//! Callbacks must not panic. They must report operational failure with `Err`. +//! Panics do not run the returned-error transition or guarantee inhibition. +//! If a host catches an unwind, it must discard the affected supervisor and +//! callback-owned state, never resume them or use an epoch reset as recovery. +//! The platform owns abort/halt panic containment. Neither abort nor halt alone +//! establishes that physical outputs are zero. +//! +//! There is no transferable permission or cross-supervisor validation API. +//! ```compile_fail +//! use kaptan_safety::ActuatorPermit; +//! ``` +//! ```compile_fail +//! use kaptan_safety::{SafetyConfig, SafetySupervisor}; +//! let a = SafetySupervisor::new(SafetyConfig::new(1).unwrap()); +//! let b = SafetySupervisor::new(SafetyConfig::new(1).unwrap()); +//! b.permit_is_current(a.decision().permit().unwrap()); +//! ``` #![no_std] +#![forbid(unsafe_code)] use kaptan_err_reportable::KaptanError; use kaptan_time::Timestamp; -/// Supervisor arming state. +#[cfg(kani)] +mod kani_proofs; + #[derive(Clone, Copy, Debug, Eq, PartialEq)] pub enum SafetyState { - /// Physical outputs inhibited. + /// No current arm transaction. Disarmed, - /// Checks are continuously passing during the configured dwell period. + /// Continuously healthy evidence must satisfy the configured dwell. PreArm, - /// Outputs may be enabled by a platform transport that validates a permit. + /// The next synchronous commit still requires evaluation of current inputs. Armed, - /// Outputs are inhibited pending an explicit disarm and new arm request. + /// Inhibited until explicit disarm. Clock faults also require an epoch reset. Failsafe, } -/// Typed reason for refusing or ending actuator authorization. #[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)] pub enum SafetyFailure { - /// Estimator is absent, stale, or non-finite. #[kaptan_error(id = 9001)] EstimatorUnhealthy, - /// Required inertial sensor stream is stale. #[kaptan_error(id = 9002)] ImuStale, - /// Required command/link stream is stale. #[kaptan_error(id = 9003)] - LinkStale, - /// Actuator transport reports a fault. + PhaseUnauthorized, #[kaptan_error(id = 9004)] ActuatorFault, - /// No current explicit arm transaction exists. #[kaptan_error(id = 9005)] ArmNotRequested, - /// The platform supplied time that moved backwards. #[kaptan_error(id = 9006)] ClockRegression, - /// A non-wrapping actuator-permit epoch could not be allocated. - #[kaptan_error(id = 9007)] - PermitEpochExhausted, + /// The synchronous commit failed. Its error is returned to the caller. + #[kaptan_error(id = 9008)] + CommitFailed, } -/// Invalid safety configuration. #[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)] pub enum SafetyConfigError { - /// A zero dwell would permit two evaluations at one timestamp to arm. #[kaptan_error(id = 9101)] ZeroPrearmDwell, } -/// Explicit platform policy for the safety state machine. -/// -/// This is intentionally not `Default`: a vehicle or simulator must choose -/// and review its own continuously-healthy prearm dwell. -/// -/// TODO(configuration): this value is currently selected by the build-time -/// vehicle profile. A ground-station configuration service must validate it -/// as part of one persisted safety/output profile and only activate the new -/// revision while disarmed; a live duration change must not restart or extend -/// an arming transaction. +/// Validated build-time policy. Activate profile changes only while disarmed. #[derive(Clone, Copy, Debug, Eq, PartialEq)] pub struct SafetyConfig { prearm_dwell_microseconds: u64, } impl SafetyConfig { - /// Builds a policy with a strictly positive prearm dwell. pub const fn new(prearm_dwell_microseconds: u64) -> Result { if prearm_dwell_microseconds == 0 { Err(SafetyConfigError::ZeroPrearmDwell) @@ -83,83 +84,65 @@ impl SafetyConfig { }) } } - - /// Returns the required continuously-healthy dwell duration. - pub const fn prearm_dwell_microseconds(self) -> u64 { - self.prearm_dwell_microseconds - } } -/// Platform-normalized bounded health input. Every `true` is a current -/// measurement, not a promise about sensor calibration or physical safety. +/// Current platform-normalized evidence, not authentication or physical safety. #[derive(Clone, Copy, Debug, Eq, PartialEq)] pub struct SafetyInputs { - /// Estimator finite/initialized health is acceptable for the requested mode. + /// The estimator is initialized, finite, and fresh for the requested mode. pub estimator_healthy: bool, - /// IMU receipt is within the static platform freshness limit. + /// The required inertial stream is within the configured freshness limit. pub imu_fresh: bool, - /// Control command/link receipt is within its freshness limit. - /// - /// TODO(flight-config): an external-control flight mode must map lease - /// expiry into a reviewed bounded hold or landing mode while estimator and - /// actuator health remain valid; it must not pass that expired lease here - /// as an immediate physical output inhibit. Fatal sensor/control/transport - /// faults remain inhibit conditions. - pub link_fresh: bool, - /// Output transport is initialized and fault-free. - pub actuator_healthy: bool, + /// Runtime has applied mode deadlines and admitted this phase's authority. + pub phase_authorized: bool, + /// The synchronous output transport is initialized and fault-free. + pub output_healthy: bool, } -/// Authorization token that external code cannot construct or clone. -/// -/// A retained token cannot authorize output after a supervisor transition. -#[derive(Debug, Eq, PartialEq)] -pub struct ActuatorPermit { - epoch: u64, -} - -/// Observable supervisor transition decision. -/// -/// The token is deliberately opaque and non-`Copy`. A transport must call -/// [`SafetySupervisor::permit_is_current`] immediately before a nonzero -/// output commit; the decision itself is not an output command. -#[derive(Debug, Eq, PartialEq)] +/// Diagnostic snapshot. It cannot authorize an output operation. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] pub struct SafetyDecision { state: SafetyState, failure: Option, - permit: Option, } impl SafetyDecision { - /// Resulting supervisor state. - pub const fn state(&self) -> SafetyState { + pub const fn state(self) -> SafetyState { self.state } - - /// Reason whenever physical output is inhibited. - pub const fn failure(&self) -> Option { + pub const fn failure(self) -> Option { self.failure } - - /// Returns the authorization token only while the decision is armed. - pub const fn permit(&self) -> Option<&ActuatorPermit> { - self.permit.as_ref() - } } -/// Small safety supervisor with no queues, heap, peripheral access, or hidden -/// clock. A platform supplies one monotonic timestamp to every transition. +#[derive(Debug, Eq, PartialEq)] +pub enum CommitOutcome { + /// The callback was not invoked. + Inhibited(SafetyDecision), + Committed(T), + /// The callback ran once and failed. The supervisor is now in failsafe. + Failed(E), +} + +#[derive(Debug, Eq, PartialEq)] +pub enum ResetError { + NotDisarmed, + Inhibit(E), +} + +/// No queues, heap, peripherals, hidden clock, or transferable authority. +/// Callbacks must not panic. A supervisor crossed by unwinding must be discarded. +/// Platform panic containment is required independently of this state machine. pub struct SafetySupervisor { config: SafetyConfig, state: SafetyState, last_failure: Option, prearm_since: Option, last_evaluation: Option, - permit_epoch: u64, + clock_fault: bool, } impl SafetySupervisor { - /// Starts disarmed and inhibited irrespective of command availability. pub const fn new(config: SafetyConfig) -> Self { Self { config, @@ -167,449 +150,178 @@ impl SafetySupervisor { last_failure: Some(SafetyFailure::ArmNotRequested), prearm_since: None, last_evaluation: None, - permit_epoch: 0, + clock_fault: false, } } - /// Consumes one explicit arm transaction. - /// - /// Only a healthy request while disarmed enters prearm. A failed request is - /// never retained for a later healthy evaluation. + /// A failed request is never retained for subsequent health recovery. pub fn request_arm(&mut self, now: Timestamp, inputs: SafetyInputs) -> SafetyDecision { - if !self.observe_time(now) { + if !self.observe_time(now) || self.state != SafetyState::Disarmed { return self.decision(); } - if self.state != SafetyState::Disarmed { - return self.decision(); + match preflight_failure(inputs) { + Some(SafetyFailure::ActuatorFault) => self.report_actuator_fault(), + Some(failure) => self.last_failure = Some(failure), + None => { + self.state = SafetyState::PreArm; + self.prearm_since = Some(now); + self.last_failure = None; + } } - if let Some(failure) = preflight_failure(inputs) { - self.last_failure = Some(failure); - return self.decision(); - } - self.state = SafetyState::PreArm; - self.prearm_since = Some(now); - self.last_failure = None; self.decision() } - /// Explicitly inhibits output and consumes any outstanding arm state. + /// Consumes arming state. A clock fault still requires an explicit epoch reset. pub fn request_disarm(&mut self) { - if self.state == SafetyState::Armed && !self.invalidate_active_permit() { - return; - } self.state = SafetyState::Disarmed; self.prearm_since = None; - self.last_failure = Some(SafetyFailure::ArmNotRequested); + self.last_failure = Some(if self.clock_fault { + SafetyFailure::ClockRegression + } else { + SafetyFailure::ArmNotRequested + }); } - /// Begins a new platform clock epoch after output has been inhibited. - /// - /// This is for a simulator restart or an explicitly reviewed platform - /// clock reset. It preserves the monotonically increasing permit epoch so - /// a token from before the reset can never validate after rearming. - pub fn reset_time_epoch(&mut self) { - self.request_disarm(); - self.prearm_since = None; + /// Resets time only while disarmed and after synchronous inhibition succeeds. + /// A failed output leaves all clock history intact and latches actuator fault. + /// `inhibit` must return `Err` on failure, not panic. After unwinding, discard + /// the supervisor and callback-owned state instead of retrying this method. + pub fn reset_time_epoch( + &mut self, + inhibit: impl FnOnce() -> Result<(), E>, + ) -> Result<(), ResetError> { + if self.state != SafetyState::Disarmed { + return Err(ResetError::NotDisarmed); + } + if let Err(error) = inhibit() { + self.report_actuator_fault(); + return Err(ResetError::Inhibit(error)); + } self.last_evaluation = None; + self.clock_fault = false; + self.request_disarm(); + Ok(()) } - /// Evaluates preflight/failsafe inputs supplied by a platform that owns - /// their timestamp and freshness policy. Platforms must call this before - /// every physical output commit. - pub fn evaluate(&mut self, now: Timestamp, inputs: SafetyInputs) -> SafetyDecision { + /// Evaluates current evidence and invokes `commit` at most once, only armed. + /// No permission escapes this exclusive borrow. Returned callback errors latch + /// failsafe before returning their original error for required cleanup. + /// `commit` must not panic. Unwinding skips this error transition and requires + /// discarding the supervisor and callback-owned state, not resuming execution. + /// + /// A callback cannot re-enter or replace its issuing supervisor. + /// ```compile_fail + /// use kaptan_safety::{SafetyConfig, SafetyInputs, SafetySupervisor}; + /// use kaptan_time::Timestamp; + /// let mut safety = SafetySupervisor::new(SafetyConfig::new(1).unwrap()); + /// let inputs = SafetyInputs { + /// estimator_healthy: true, imu_fresh: true, + /// phase_authorized: true, output_healthy: true, + /// }; + /// safety.evaluate_and_commit(Timestamp::from_microseconds(1), inputs, || { + /// safety.request_disarm(); + /// Ok::<_, ()>(()) + /// }); + /// ``` + pub fn evaluate_and_commit( + &mut self, + now: Timestamp, + inputs: SafetyInputs, + commit: impl FnOnce() -> Result, + ) -> CommitOutcome { + self.evaluate(now, inputs); + if self.state != SafetyState::Armed { + return CommitOutcome::Inhibited(self.decision()); + } + match commit() { + Ok(value) => CommitOutcome::Committed(value), + Err(error) => { + self.fail(SafetyFailure::CommitFailed); + CommitOutcome::Failed(error) + } + } + } + + /// Output failures, including failed inhibition, latch until explicit disarm. + pub fn report_actuator_fault(&mut self) { + self.fail(SafetyFailure::ActuatorFault); + } + + pub const fn state(&self) -> SafetyState { + self.state + } + + pub const fn decision(&self) -> SafetyDecision { + SafetyDecision { + state: self.state, + failure: self.last_failure, + } + } + + fn evaluate(&mut self, now: Timestamp, inputs: SafetyInputs) { if !self.observe_time(now) { - return self.decision(); + return; } let failure = preflight_failure(inputs); + if failure == Some(SafetyFailure::ActuatorFault) { + self.report_actuator_fault(); + return; + } match self.state { SafetyState::Disarmed => { self.last_failure = failure.or(Some(SafetyFailure::ArmNotRequested)); } SafetyState::PreArm => { if let Some(reason) = failure { - self.state = SafetyState::Disarmed; - self.prearm_since = None; + self.request_disarm(); self.last_failure = Some(reason); } else if self.prearm_since.is_some_and(|since| { - elapsed_at_least(now, since, self.config.prearm_dwell_microseconds) + now.as_microseconds() - since.as_microseconds() + >= self.config.prearm_dwell_microseconds }) { - self.enter_armed(); + self.state = SafetyState::Armed; + self.prearm_since = None; + self.last_failure = None; } } SafetyState::Armed => { if let Some(reason) = failure { - self.leave_armed(SafetyState::Failsafe, reason); + self.fail(reason); } } SafetyState::Failsafe => {} } - self.decision() - } - - /// Checks an opaque token at the output boundary. - pub fn permit_is_current(&self, permit: &ActuatorPermit) -> bool { - self.state == SafetyState::Armed && self.permit_epoch == permit.epoch - } - - /// Returns the current state without granting a bypass permission. - pub const fn state(&self) -> SafetyState { - self.state } fn observe_time(&mut self, now: Timestamp) -> bool { if self.last_evaluation.is_some_and(|previous| now < previous) { - self.prearm_since = None; - if self.state == SafetyState::Armed { - self.leave_armed(SafetyState::Failsafe, SafetyFailure::ClockRegression); - } else if self.state == SafetyState::PreArm { - self.state = SafetyState::Failsafe; - self.last_failure = Some(SafetyFailure::ClockRegression); - } else { - self.last_failure = Some(SafetyFailure::ClockRegression); - } + self.clock_fault = true; + } + if self.clock_fault { + self.fail(SafetyFailure::ClockRegression); return false; } self.last_evaluation = Some(now); true } - fn enter_armed(&mut self) { - let Some(next_epoch) = self.permit_epoch.checked_add(1) else { - self.state = SafetyState::Failsafe; - self.prearm_since = None; - self.last_failure = Some(SafetyFailure::PermitEpochExhausted); - return; - }; - self.permit_epoch = next_epoch; - self.state = SafetyState::Armed; + fn fail(&mut self, failure: SafetyFailure) { + self.state = SafetyState::Failsafe; self.prearm_since = None; - self.last_failure = None; + self.last_failure = Some(failure); } - - fn leave_armed(&mut self, next_state: SafetyState, failure: SafetyFailure) { - if self.invalidate_active_permit() { - self.state = next_state; - self.prearm_since = None; - self.last_failure = Some(failure); - } - } - - fn invalidate_active_permit(&mut self) -> bool { - let Some(next_epoch) = self.permit_epoch.checked_add(1) else { - self.state = SafetyState::Failsafe; - self.prearm_since = None; - self.last_failure = Some(SafetyFailure::PermitEpochExhausted); - return false; - }; - self.permit_epoch = next_epoch; - true - } - - fn decision(&self) -> SafetyDecision { - SafetyDecision { - state: self.state, - failure: self.last_failure, - permit: if self.state == SafetyState::Armed { - Some(ActuatorPermit { - epoch: self.permit_epoch, - }) - } else { - None - }, - } - } -} - -fn elapsed_at_least(now: Timestamp, since: Timestamp, duration_microseconds: u64) -> bool { - now.as_microseconds() - since.as_microseconds() >= duration_microseconds } fn preflight_failure(inputs: SafetyInputs) -> Option { - if !inputs.estimator_healthy { + if !inputs.output_healthy { + Some(SafetyFailure::ActuatorFault) + } else if !inputs.estimator_healthy { Some(SafetyFailure::EstimatorUnhealthy) } else if !inputs.imu_fresh { Some(SafetyFailure::ImuStale) - } else if !inputs.link_fresh { - Some(SafetyFailure::LinkStale) - } else if !inputs.actuator_healthy { - Some(SafetyFailure::ActuatorFault) + } else if !inputs.phase_authorized { + Some(SafetyFailure::PhaseUnauthorized) } else { None } } - -#[cfg(test)] -mod tests { - use super::*; - - const DWELL_MICROSECONDS: u64 = 100; - - fn supervisor() -> SafetySupervisor { - SafetySupervisor::new(SafetyConfig::new(DWELL_MICROSECONDS).unwrap()) - } - - fn time(microseconds: u64) -> Timestamp { - Timestamp::from_microseconds(microseconds) - } - - fn healthy() -> SafetyInputs { - SafetyInputs { - estimator_healthy: true, - imu_fresh: true, - link_fresh: true, - actuator_healthy: true, - } - } - - fn arm(supervisor: &mut SafetySupervisor) -> SafetyDecision { - assert_eq!( - supervisor.request_arm(time(10), healthy()).state(), - SafetyState::PreArm - ); - supervisor.evaluate(time(10 + DWELL_MICROSECONDS), healthy()) - } - - #[test] - fn configuration_requires_nonzero_dwell() { - assert_eq!( - SafetyConfig::new(0), - Err(SafetyConfigError::ZeroPrearmDwell) - ); - assert_eq!( - SafetyConfig::new(DWELL_MICROSECONDS) - .unwrap() - .prearm_dwell_microseconds(), - DWELL_MICROSECONDS - ); - } - - #[test] - fn failed_arm_is_not_latched_for_later_health() { - let mut safety = supervisor(); - let mut unhealthy = healthy(); - unhealthy.imu_fresh = false; - assert_eq!( - safety.request_arm(time(1), unhealthy).failure(), - Some(SafetyFailure::ImuStale) - ); - let decision = safety.evaluate(time(1 + DWELL_MICROSECONDS), healthy()); - assert_eq!(decision.state(), SafetyState::Disarmed); - assert!(decision.permit().is_none()); - assert_eq!(decision.failure(), Some(SafetyFailure::ArmNotRequested)); - } - - #[test] - fn dwell_requires_elapsed_monotonic_time() { - let mut safety = supervisor(); - assert_eq!( - safety.request_arm(time(10), healthy()).state(), - SafetyState::PreArm - ); - assert_eq!( - safety.evaluate(time(10), healthy()).state(), - SafetyState::PreArm - ); - assert_eq!( - safety - .evaluate(time(10 + DWELL_MICROSECONDS - 1), healthy()) - .state(), - SafetyState::PreArm - ); - let decision = safety.evaluate(time(10 + DWELL_MICROSECONDS), healthy()); - assert_eq!(decision.state(), SafetyState::Armed); - assert!(decision.permit().is_some()); - } - - #[test] - fn prearm_fault_consumes_transaction() { - let mut safety = supervisor(); - safety.request_arm(time(10), healthy()); - let mut stale = healthy(); - stale.link_fresh = false; - assert_eq!( - safety.evaluate(time(20), stale).state(), - SafetyState::Disarmed - ); - assert_eq!( - safety - .evaluate(time(10 + DWELL_MICROSECONDS), healthy()) - .state(), - SafetyState::Disarmed - ); - } - - #[test] - fn armed_fault_latches_failsafe() { - let mut safety = supervisor(); - let armed = arm(&mut safety); - assert!(safety.permit_is_current(armed.permit().unwrap())); - let mut stale = healthy(); - stale.link_fresh = false; - let decision = safety.evaluate(time(111), stale); - assert_eq!(decision.state(), SafetyState::Failsafe); - assert!(decision.permit().is_none()); - assert_eq!(decision.failure(), Some(SafetyFailure::LinkStale)); - assert_eq!( - safety.evaluate(time(112), healthy()).state(), - SafetyState::Failsafe - ); - } - - #[test] - fn time_regression_inhibits_and_latches_failsafe() { - let mut safety = supervisor(); - let armed = arm(&mut safety); - let permit = armed.permit().unwrap(); - assert!(safety.permit_is_current(permit)); - let decision = safety.evaluate(time(109), healthy()); - assert_eq!(decision.state(), SafetyState::Failsafe); - assert!(decision.permit().is_none()); - assert_eq!(decision.failure(), Some(SafetyFailure::ClockRegression)); - assert!(!safety.permit_is_current(permit)); - } - - #[test] - fn failsafe_requires_explicit_disarm_before_a_fresh_arm_request() { - let mut safety = supervisor(); - let _armed = arm(&mut safety); - let mut stale = healthy(); - stale.link_fresh = false; - safety.evaluate(time(111), stale); - assert_eq!( - safety.request_arm(time(112), healthy()).state(), - SafetyState::Failsafe - ); - safety.request_disarm(); - assert_eq!( - safety.request_arm(time(113), healthy()).state(), - SafetyState::PreArm - ); - } - - #[test] - fn stale_permit_cannot_authorize_after_rearm() { - let mut safety = supervisor(); - let first = arm(&mut safety); - let first_permit = first.permit().unwrap(); - assert!(safety.permit_is_current(first_permit)); - safety.request_disarm(); - assert!(!safety.permit_is_current(first_permit)); - assert_eq!( - safety.request_arm(time(120), healthy()).state(), - SafetyState::PreArm - ); - let second = safety.evaluate(time(120 + DWELL_MICROSECONDS), healthy()); - assert!(safety.permit_is_current(second.permit().unwrap())); - assert!(!safety.permit_is_current(first_permit)); - } - - #[test] - fn platform_epoch_reset_accepts_a_new_clock_without_revalidating_permits() { - let mut safety = supervisor(); - let armed = arm(&mut safety); - let permit = armed.permit().unwrap(); - safety.reset_time_epoch(); - assert_eq!(safety.state(), SafetyState::Disarmed); - assert!(!safety.permit_is_current(permit)); - assert_eq!( - safety.request_arm(time(1), healthy()).state(), - SafetyState::PreArm - ); - let rearmed = safety.evaluate(time(1 + DWELL_MICROSECONDS), healthy()); - assert!(safety.permit_is_current(rearmed.permit().unwrap())); - assert!(!safety.permit_is_current(permit)); - } - - #[test] - fn permit_epoch_exhaustion_latches_inhibition() { - let mut safety = supervisor(); - safety.permit_epoch = u64::MAX; - safety.request_arm(time(10), healthy()); - let decision = safety.evaluate(time(10 + DWELL_MICROSECONDS), healthy()); - assert_eq!(decision.state(), SafetyState::Failsafe); - assert_eq!( - decision.failure(), - Some(SafetyFailure::PermitEpochExhausted) - ); - assert!(decision.permit().is_none()); - } - - #[test] - fn preflight_priority_is_stable() { - assert_eq!( - preflight_failure(SafetyInputs { - estimator_healthy: false, - imu_fresh: false, - link_fresh: false, - actuator_healthy: false, - }), - Some(SafetyFailure::EstimatorUnhealthy) - ); - assert_eq!( - preflight_failure(SafetyInputs { - estimator_healthy: true, - imu_fresh: false, - link_fresh: false, - actuator_healthy: false, - }), - Some(SafetyFailure::ImuStale) - ); - assert_eq!( - preflight_failure(SafetyInputs { - estimator_healthy: true, - imu_fresh: true, - link_fresh: true, - actuator_healthy: false, - }), - Some(SafetyFailure::ActuatorFault) - ); - } - - #[test] - fn clock_regressions_and_permit_exhaustion_fail_closed() { - let mut disarmed = supervisor(); - disarmed.evaluate(time(10), healthy()); - let decision = disarmed.request_arm(time(9), healthy()); - assert_eq!(decision.state(), SafetyState::Disarmed); - assert_eq!(decision.failure(), Some(SafetyFailure::ClockRegression)); - - let mut prearm = supervisor(); - prearm.request_arm(time(10), healthy()); - let decision = prearm.evaluate(time(9), healthy()); - assert_eq!(decision.state(), SafetyState::Failsafe); - assert_eq!(decision.failure(), Some(SafetyFailure::ClockRegression)); - - let mut armed = supervisor(); - let armed_decision = arm(&mut armed); - let permit = armed_decision.permit().unwrap(); - assert!(armed.permit_is_current(permit)); - assert_eq!( - armed.evaluate(time(111), healthy()).state(), - SafetyState::Armed - ); - - // This test-only corrupt state models a counter exhausted after a - // previously issued permit. Explicit disarm must still revoke output - // and latch the overflow rather than wrapping a token epoch. - armed.permit_epoch = u64::MAX; - armed.request_disarm(); - assert_eq!(armed.state(), SafetyState::Failsafe); - assert_eq!( - armed.last_failure, - Some(SafetyFailure::PermitEpochExhausted) - ); - - // A failsafe transition uses the same non-wrapping revocation path. - let mut overflow_on_fault = supervisor(); - overflow_on_fault.state = SafetyState::Armed; - overflow_on_fault.permit_epoch = u64::MAX; - let mut stale = healthy(); - stale.link_fresh = false; - let decision = overflow_on_fault.evaluate(time(1), stale); - assert_eq!(decision.state(), SafetyState::Failsafe); - assert_eq!( - decision.failure(), - Some(SafetyFailure::PermitEpochExhausted) - ); - } -} diff --git a/firmware/libs/kaptan-safety/tests/commit.rs b/firmware/libs/kaptan-safety/tests/commit.rs new file mode 100644 index 0000000..e424bd0 --- /dev/null +++ b/firmware/libs/kaptan-safety/tests/commit.rs @@ -0,0 +1,268 @@ +use kaptan_safety::*; +use kaptan_time::Timestamp; + +fn time(value: u64) -> Timestamp { + Timestamp::from_microseconds(value) +} +fn healthy() -> SafetyInputs { + SafetyInputs { + estimator_healthy: true, + imu_fresh: true, + phase_authorized: true, + output_healthy: true, + } +} +fn supervisor() -> SafetySupervisor { + SafetySupervisor::new(SafetyConfig::new(100).unwrap()) +} +fn evaluate(s: &mut SafetySupervisor, now: u64, inputs: SafetyInputs) -> CommitOutcome<(), ()> { + s.evaluate_and_commit(time(now), inputs, || Ok(())) +} + +#[test] +fn dwell_and_owner_local_commit() { + assert_eq!( + SafetyConfig::new(0), + Err(SafetyConfigError::ZeroPrearmDwell) + ); + let mut a = supervisor(); + let mut b = supervisor(); + assert_eq!( + a.request_arm(time(10), healthy()).state(), + SafetyState::PreArm + ); + for now in [10, 109] { + assert!(matches!( + a.evaluate_and_commit(time(now), healthy(), || -> Result<(), ()> { + panic!("prearm cannot commit") + }), + CommitOutcome::Inhibited(_) + )); + } + let mut calls = 0; + assert_eq!( + a.evaluate_and_commit(time(110), healthy(), || { + calls += 1; + Ok::<_, ()>(42) + }), + CommitOutcome::Committed(42) + ); + assert_eq!(calls, 1); + // Healthy armed evaluations keep committing without restarting the dwell. + assert_eq!( + evaluate(&mut a, 111, healthy()), + CommitOutcome::Committed(()) + ); + assert!(matches!( + b.evaluate_and_commit(time(110), healthy(), || -> Result<(), ()> { + panic!("another owner's armed state cannot commit here") + }), + CommitOutcome::Inhibited(_) + )); +} + +#[test] +fn failed_arm_and_prearm_are_consumed() { + let mut s = supervisor(); + let mut stale = healthy(); + stale.imu_fresh = false; + assert_eq!( + s.request_arm(time(1), stale).failure(), + Some(SafetyFailure::ImuStale) + ); + evaluate(&mut s, 200, healthy()); + assert_eq!(s.state(), SafetyState::Disarmed); + s.request_arm(time(201), healthy()); + evaluate(&mut s, 202, stale); + evaluate(&mut s, 400, healthy()); + assert_eq!(s.state(), SafetyState::Disarmed); +} + +#[test] +fn failed_arm_health_priority_and_recovery_require_a_new_transaction() { + for (inputs, expected) in [ + ( + SafetyInputs { + estimator_healthy: false, + imu_fresh: false, + phase_authorized: false, + output_healthy: false, + }, + SafetyFailure::ActuatorFault, + ), + ( + SafetyInputs { + estimator_healthy: false, + imu_fresh: false, + phase_authorized: false, + ..healthy() + }, + SafetyFailure::EstimatorUnhealthy, + ), + ( + SafetyInputs { + imu_fresh: false, + phase_authorized: false, + ..healthy() + }, + SafetyFailure::ImuStale, + ), + ( + SafetyInputs { + phase_authorized: false, + ..healthy() + }, + SafetyFailure::PhaseUnauthorized, + ), + ] { + let mut s = supervisor(); + let expected_state = if expected == SafetyFailure::ActuatorFault { + SafetyState::Failsafe + } else { + SafetyState::Disarmed + }; + let decision = s.request_arm(time(0), inputs); + assert_eq!(decision.failure(), Some(expected)); + assert_eq!(decision.state(), expected_state); + assert!(matches!( + evaluate(&mut s, 100, healthy()), + CommitOutcome::Inhibited(_) + )); + assert_eq!(s.state(), expected_state); + if expected == SafetyFailure::ActuatorFault { + assert_eq!( + s.request_arm(time(101), healthy()).state(), + SafetyState::Failsafe + ); + s.request_disarm(); + } + assert_eq!( + s.request_arm(time(102), healthy()).state(), + SafetyState::PreArm + ); + assert_eq!( + evaluate(&mut s, 202, healthy()), + CommitOutcome::Committed(()) + ); + } +} + +#[test] +fn regressing_arm_request_consumes_prearm_and_requires_explicit_reset() { + let mut s = supervisor(); + assert_eq!( + s.request_arm(time(100), healthy()).state(), + SafetyState::PreArm + ); + let decision = s.request_arm(time(99), healthy()); + assert_eq!(decision.state(), SafetyState::Failsafe); + assert_eq!(decision.failure(), Some(SafetyFailure::ClockRegression)); + assert!(matches!( + evaluate(&mut s, 200, healthy()), + CommitOutcome::Inhibited(_) + )); + s.request_disarm(); + s.reset_time_epoch(|| Ok::<_, ()>(())).unwrap(); + assert_eq!( + s.request_arm(time(0), healthy()).state(), + SafetyState::PreArm + ); +} + +#[test] +fn current_health_revokes_before_callback_and_recovery_cannot_rearm() { + let mut s = supervisor(); + s.request_arm(time(0), healthy()); + assert_eq!( + evaluate(&mut s, 100, healthy()), + CommitOutcome::Committed(()) + ); + let diagnostic = s.decision(); + let mut stale = healthy(); + stale.phase_authorized = false; + assert!(matches!( + s.evaluate_and_commit(time(101), stale, || -> Result<(), ()> { + panic!("stale evidence must inhibit before callback") + }), + CommitOutcome::Inhibited(_) + )); + assert_eq!(diagnostic.state(), SafetyState::Armed); // retained diagnostic only + assert_eq!( + s.decision().failure(), + Some(SafetyFailure::PhaseUnauthorized) + ); + assert!(matches!( + evaluate(&mut s, 102, healthy()), + CommitOutcome::Inhibited(_) + )); + assert_eq!( + s.request_arm(time(103), healthy()).state(), + SafetyState::Failsafe + ); + s.request_disarm(); + assert_eq!( + s.request_arm(time(104), healthy()).state(), + SafetyState::PreArm + ); + assert_eq!( + evaluate(&mut s, 204, healthy()), + CommitOutcome::Committed(()) + ); +} + +#[test] +fn callback_error_and_actuator_health_require_disarm() { + let mut s = supervisor(); + s.request_arm(time(0), healthy()); + assert_eq!( + s.evaluate_and_commit(time(100), healthy(), || Err::<(), _>("commit")), + CommitOutcome::Failed("commit") + ); + assert_eq!(s.state(), SafetyState::Failsafe); + evaluate(&mut s, 101, healthy()); + assert_eq!( + s.request_arm(time(102), healthy()).state(), + SafetyState::Failsafe + ); + s.request_disarm(); + s.request_arm(time(103), healthy()); + let mut broken = healthy(); + broken.output_healthy = false; + evaluate(&mut s, 104, broken); + assert_eq!(s.decision().failure(), Some(SafetyFailure::ActuatorFault)); + evaluate(&mut s, 300, healthy()); + assert_eq!(s.state(), SafetyState::Failsafe); +} + +#[test] +fn clock_fault_survives_disarm_and_failed_reset() { + let mut s = supervisor(); + evaluate(&mut s, 100, healthy()); + evaluate(&mut s, 99, healthy()); + assert_eq!(s.decision().failure(), Some(SafetyFailure::ClockRegression)); + assert_eq!( + s.reset_time_epoch(|| Ok::<_, ()>(())), + Err(ResetError::NotDisarmed) + ); + s.request_disarm(); + assert_eq!( + s.request_arm(time(101), healthy()).state(), + SafetyState::Failsafe + ); + s.request_disarm(); + assert_eq!( + s.reset_time_epoch(|| Err("inhibit")), + Err(ResetError::Inhibit("inhibit")) + ); + s.request_disarm(); + assert_eq!( + s.request_arm(time(102), healthy()).state(), + SafetyState::Failsafe + ); + s.request_disarm(); + s.reset_time_epoch(|| Ok::<_, ()>(())).unwrap(); + assert_eq!( + s.request_arm(time(0), healthy()).state(), + SafetyState::PreArm + ); +} diff --git a/firmware/libs/kaptan-sensor/src/imu.rs b/firmware/libs/kaptan-sensor/src/imu.rs index c81fafb..7ee5021 100644 --- a/firmware/libs/kaptan-sensor/src/imu.rs +++ b/firmware/libs/kaptan-sensor/src/imu.rs @@ -7,14 +7,24 @@ use kaptan_time::Timestamp; /// Acceleration is milligravity, angular rate is milliradians per second, and /// temperature is centidegrees Celsius. The board-frame transform is complete /// before this value is published. +/// +/// Current MPU6000 and ICM-42688-P decoders round integer conversions toward +/// zero. Across their configured ranges, decoded acceleration is within +/// ±16,000 mg and angular rate within ±34,871 mrad/s, including mounting sign +/// changes. Temperature is within -22,234..=27,233 cdegC. These are backend +/// representation bounds over every raw i16 value, not physical operating +/// limits or validation performed by this public snapshot type. #[derive(Clone, Copy, Debug, Eq, PartialEq)] pub struct ImuSnapshot { - /// Monotonic timestamp captured at acquisition interrupt entry. + /// Host monotonic acquisition-request time, captured immediately before the + /// accepted register read is submitted by the worker. This is neither DRDY + /// ISR entry nor the sensor's physical sampling epoch. Register updates, + /// filtering and deferred/coalesced interrupts prevent that inference. pub timestamp: Timestamp, /// Body-frame acceleration in milligravity. pub acceleration_milligravity: [i16; 3], /// Body-frame angular rate in milliradians per second. - pub angular_rate_milliradians_per_second: [i16; 3], + pub angular_rate_milliradians_per_second: [i32; 3], /// Sensor temperature in centidegrees Celsius. pub temperature_centidegrees_celsius: i16, } @@ -24,7 +34,7 @@ impl ImuSnapshot { pub const fn new( timestamp: Timestamp, acceleration_milligravity: [i16; 3], - angular_rate_milliradians_per_second: [i16; 3], + angular_rate_milliradians_per_second: [i32; 3], temperature_centidegrees_celsius: i16, ) -> Self { Self { diff --git a/firmware/libs/kaptan-sensor/src/lib.rs b/firmware/libs/kaptan-sensor/src/lib.rs index ae9d59f..69bd9ac 100644 --- a/firmware/libs/kaptan-sensor/src/lib.rs +++ b/firmware/libs/kaptan-sensor/src/lib.rs @@ -1,8 +1,4 @@ //! Hardware-independent sensor observations and units. -//! -//! Sensor data is immutable after publication. Bus transactions, queueing, -//! protocol encoding, PAC access, RTIC mechanics, and board transforms do not -//! belong in this crate. #![no_std] diff --git a/firmware/libs/kaptan-sensor/src/optical_flow.rs b/firmware/libs/kaptan-sensor/src/optical_flow.rs index dc142de..0fa7236 100644 --- a/firmware/libs/kaptan-sensor/src/optical_flow.rs +++ b/firmware/libs/kaptan-sensor/src/optical_flow.rs @@ -3,10 +3,6 @@ use kaptan_time::Timestamp; /// Coherent ground-referenced optical-flow observation in the sensor frame. -/// -/// Linear velocity and ground distance use integer SI subunits so acquisition -/// remains independent of the floating-point representation required by some -/// outbound wire protocols. #[derive(Clone, Copy, Debug, Eq, PartialEq)] pub struct OpticalFlowSnapshot { /// Flight-controller monotonic time at observation reception. diff --git a/firmware/libs/kaptan-sensor/src/range.rs b/firmware/libs/kaptan-sensor/src/range.rs index fca649e..7a3327b 100644 --- a/firmware/libs/kaptan-sensor/src/range.rs +++ b/firmware/libs/kaptan-sensor/src/range.rs @@ -4,12 +4,11 @@ use kaptan_time::Timestamp; /// Coherent range observation in the sensor frame. /// -/// The timestamp is captured when the measurement is requested. Distance is -/// expressed in millimetres so rangefinders with centimetre resolution can be -/// published without floating-point conversion. +/// Distance is in millimetres. LIDAR-Lite timestamps the request and MTF-01 +/// timestamps frame reception. Neither establishes the physical sample epoch. #[derive(Clone, Copy, Debug, Eq, PartialEq)] pub struct RangeSnapshot { - /// Monotonic timestamp captured when the acquisition was requested. + /// Producer timestamp in the controller monotonic clock domain. pub timestamp: Timestamp, /// Measured distance in millimetres. pub distance_millimetres: u32, diff --git a/firmware/libs/kaptan-spi/src/dma.rs b/firmware/libs/kaptan-spi/src/dma.rs new file mode 100644 index 0000000..e4a6e0c --- /dev/null +++ b/firmware/libs/kaptan-spi/src/dma.rs @@ -0,0 +1,329 @@ +//! Permanently allocated copy-in/copy-out storage for DMA backends. +//! +//! This module manages allocation and exclusive CPU access, not hardware. +//! Backends must prove quiescence before calling the unsafe completion method. + +use core::{ + cell::{Cell, UnsafeCell}, + marker::PhantomData, + sync::atomic::{AtomicBool, Ordering, compiler_fence}, +}; + +use crate::{CompletionError, SpiCompletion}; + +/// One bounded TX/RX pair. Only a static allocation can be claimed, once ever. +pub struct Storage { + claimed: AtomicBool, + transmit: UnsafeCell<[u8; N]>, + receive: UnsafeCell<[u8; N]>, +} + +// SAFETY: The atomic claim creates at most one non-cloneable owner. All access +// to bytes is through that owner with &mut self, and its state excludes CPU +// access during DMA. Neither Drop nor any other helper resets the claim. +unsafe impl Sync for Storage {} + +impl Default for Storage { + fn default() -> Self { + Self::new() + } +} + +impl Storage { + /// Creates unclaimed storage. Place this in DMA-accessible static memory. + pub const fn new() -> Self { + Self { + claimed: AtomicBool::new(false), + transmit: UnsafeCell::new([0; N]), + receive: UnsafeCell::new([0; N]), + } + } + + /// Claims the allocation permanently. Dropping/forgetting the owner leaks + /// its claim deliberately: DMA may still hold the static addresses. + pub fn claim(&'static self) -> Option> { + self.claimed + .compare_exchange(false, true, Ordering::AcqRel, Ordering::Acquire) + .ok()?; + Some(Transfer { + storage: self, + state: State::Idle, + exclusive: PhantomData, + }) + } +} + +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +enum State { + Idle, + Active(usize), + Quarantined, +} + +/// Exclusive, movable owner of a permanent allocation. It is Send: moving it +/// moves only a shared reference to the one-time-claimed static, never bytes. +/// It is not Sync; CPU buffer operations require exclusive ownership. +pub struct Transfer { + storage: &'static Storage, + state: State, + exclusive: PhantomData>, +} + +/// Rejection before any copy-in or hardware submission. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum StartError { + InvalidLength, + Busy, + Quarantined, +} + +/// Static endpoints, valid even after the owner is moved, dropped or forgotten. +/// Access through these raw pointers remains the backend's unsafe obligation. +pub struct Endpoints { + pub transmit: *const u8, + pub receive: *mut u8, + pub length: usize, +} + +impl Transfer { + /// Checks admission without touching storage or changing ownership. + pub fn check_start(&self, length: usize) -> Result<(), StartError> { + match self.state { + State::Quarantined => Err(StartError::Quarantined), + State::Active(_) => Err(StartError::Busy), + State::Idle if length == 0 || length > N => Err(StartError::InvalidLength), + State::Idle => Ok(()), + } + } + + /// Copies in and reserves both slots before returning hardware endpoints. + /// The backend must apply its device/cache handoff barrier before enabling DMA. + pub fn begin(&mut self, transmit: &[u8]) -> Result { + self.check_start(transmit.len())?; + // SAFETY: Idle slots cannot be accessed by DMA: only unsafe completion + // can return them to Idle. The one-time owner excludes other CPU access. + unsafe { + (&mut *self.storage.transmit.get())[..transmit.len()].copy_from_slice(transmit); + } + self.state = State::Active(transmit.len()); + compiler_fence(Ordering::Release); + Ok(Endpoints { + transmit: self.storage.transmit.get().cast::(), + receive: self.storage.receive.get().cast::(), + length: transmit.len(), + }) + } + + /// Checks completion admission without changing the active transfer. + pub fn check_completion(&self, completion: SpiCompletion) -> Result<(), CompletionError> { + if completion == SpiCompletion::NotPending { + return Err(CompletionError::NotPending); + } + match self.state { + State::Idle => Err(CompletionError::NotActive), + State::Active(_) => Ok(()), + State::Quarantined => Err(CompletionError::Quarantined), + } + } + + /// Permanently prevents any subsequent CPU access or transfer admission. + pub fn quarantine(&mut self) { + self.state = State::Quarantined; + } + + /// Releases quiescent slots, copying out only a successful full transfer. + /// Errors never modify `receive`; an invalid destination discards the result. + /// + /// # Safety + /// Both DMA streams must be observed disabled and the SPI bus finished. + /// No hardware may subsequently access these endpoints until a new `begin`. + /// Perform the platform's device/cache completion barrier before this call. + /// `Complete` additionally requires proof that all submitted bytes arrived + /// without a transfer fault. Notifications alone are insufficient evidence. + pub unsafe fn finish( + &mut self, + completion: SpiCompletion, + receive: &mut [u8], + ) -> Result<(), CompletionError> { + self.check_completion(completion)?; + let State::Active(length) = self.state else { + unreachable!(); + }; + compiler_fence(Ordering::Acquire); + self.state = State::Idle; + if completion == SpiCompletion::TransferFault { + return Err(CompletionError::TransferFault); + } + if receive.len() != length { + return Err(CompletionError::InvalidLength); + } + // SAFETY: The caller proved DMA quiescence and initialized the full + // response. The allocation remains exclusively claimed by this owner. + unsafe { + receive.copy_from_slice(&(&*self.storage.receive.get())[..length]); + } + Ok(()) + } +} + +#[cfg(test)] +mod tests { + extern crate std; + + use super::*; + + #[test] + fn capacity_above_u16() { + const CAPACITY: usize = u16::MAX as usize + 1; + static STORAGE: Storage = Storage::new(); + let owner = STORAGE.claim().unwrap(); + assert_eq!(owner.check_start(CAPACITY), Ok(())); + assert_eq!( + owner.check_start(CAPACITY + 1), + Err(StartError::InvalidLength) + ); + } + + #[test] + fn concurrent_claims_create_exactly_one_usable_owner() { + static STORAGE: Storage<4> = Storage::new(); + let start = std::sync::Barrier::new(4); + let mut owner = std::thread::scope(|scope| { + let contenders: [_; 4] = core::array::from_fn(|_| { + scope.spawn(|| { + start.wait(); + STORAGE.claim() + }) + }); + let mut winner = None; + for contender in contenders { + if let Some(owner) = contender.join().unwrap() { + assert!(winner.replace(owner).is_none()); + } + } + winner.expect("exactly one contender claims the static allocation") + }); + assert!(STORAGE.claim().is_none()); + + let endpoints = owner.begin(&[1, 2, 3, 4]).unwrap(); + let mut receive = [0; 4]; + // SAFETY: All contenders have joined. These accesses stand in for DMA, + // initialize the full response, and end before accepting completion. + unsafe { + assert_eq!( + core::slice::from_raw_parts(endpoints.transmit, 4), + &[1, 2, 3, 4] + ); + endpoints + .receive + .copy_from_nonoverlapping([5, 6, 7, 8].as_ptr(), 4); + owner.finish(SpiCompletion::Complete, &mut receive).unwrap(); + } + assert_eq!(receive, [5, 6, 7, 8]); + } + + #[test] + fn copy_in_out_survives_caller_mutation_and_owner_move() { + static STORAGE: Storage<4> = Storage::new(); + fn assert_send() {} + assert_send::>(); + let mut owner = STORAGE.claim().unwrap(); + let mut caller = [1, 2, 3, 4]; + let endpoints = owner.begin(&caller).unwrap(); + caller.fill(9); + let mut moved = owner; + let mut result = [7; 4]; + assert_eq!(moved.check_start(4), Err(StartError::Busy)); + assert_eq!( + moved.check_completion(SpiCompletion::NotPending), + Err(CompletionError::NotPending) + ); + assert_eq!(result, [7; 4]); + // SAFETY: No hardware is involved; simulate DMA using the reserved + // endpoints, then stop accessing them before accepting completion. + unsafe { + assert_eq!( + moved.finish(SpiCompletion::NotPending, &mut result), + Err(CompletionError::NotPending) + ); + assert_eq!(moved.check_start(4), Err(StartError::Busy)); + assert_eq!( + core::slice::from_raw_parts(endpoints.transmit, 4), + &[1, 2, 3, 4] + ); + endpoints + .receive + .copy_from_nonoverlapping([5, 6, 7, 8].as_ptr(), 4); + moved.finish(SpiCompletion::Complete, &mut result).unwrap(); + } + assert_eq!(result, [5, 6, 7, 8]); + assert_eq!(moved.check_start(4), Ok(())); + } + + #[test] + fn invalid_lengths_and_faults_never_copy_stale_bytes() { + static STORAGE: Storage<4> = Storage::new(); + let mut owner = STORAGE.claim().unwrap(); + assert!(matches!(owner.begin(&[]), Err(StartError::InvalidLength))); + assert!(matches!( + owner.begin(&[0; 5]), + Err(StartError::InvalidLength) + )); + let mut receive = [9; 5]; + let endpoints = owner.begin(&[1; 4]).unwrap(); + // SAFETY: Simulated DMA initializes all bytes and is then quiescent. + unsafe { + endpoints.receive.write_bytes(2, 4); + assert_eq!( + owner.finish(SpiCompletion::Complete, &mut receive), + Err(CompletionError::InvalidLength) + ); + } + assert_eq!(receive, [9; 5]); + owner.begin(&[3; 4]).unwrap(); + // SAFETY: No hardware was started; cancellation cannot race any access. + unsafe { + assert_eq!( + owner.finish(SpiCompletion::TransferFault, &mut receive[..4]), + Err(CompletionError::TransferFault) + ); + } + assert_eq!(receive, [9; 5]); + assert_eq!( + owner.check_completion(SpiCompletion::Complete), + Err(CompletionError::NotActive) + ); + } + + #[test] + fn quarantine_drop_and_forget_never_reclaim_storage() { + static QUARANTINED: Storage<4> = Storage::new(); + static DROPPED: Storage<4> = Storage::new(); + static FORGOTTEN: Storage<4> = Storage::new(); + let mut owner = QUARANTINED.claim().unwrap(); + owner.begin(&[1]).unwrap(); + owner.quarantine(); + assert_eq!(owner.check_start(1), Err(StartError::Quarantined)); + assert_eq!( + owner.check_completion(SpiCompletion::Complete), + Err(CompletionError::Quarantined) + ); + assert!(QUARANTINED.claim().is_none()); + let endpoints = { + let mut owner = DROPPED.claim().unwrap(); + owner.begin(&[3]).unwrap() + }; + assert!(DROPPED.claim().is_none()); + // SAFETY: Dropping the handle did not free or reuse the static bytes. + unsafe { assert_eq!(*endpoints.transmit, 3) }; + let mut owner = FORGOTTEN.claim().unwrap(); + let endpoints = owner.begin(&[4]).unwrap(); + // Explicitly exercise forget as well as drop, including if a destructor + // is ever added: correctness must not depend on it running. + #[allow(clippy::forget_non_drop)] + core::mem::forget(owner); + assert!(FORGOTTEN.claim().is_none()); + // SAFETY: Forgetting the handle also leaves the allocation reserved. + unsafe { assert_eq!(*endpoints.transmit, 4) }; + } +} diff --git a/firmware/libs/kaptan-spi/src/lib.rs b/firmware/libs/kaptan-spi/src/lib.rs index e30f986..fb3b32e 100644 --- a/firmware/libs/kaptan-spi/src/lib.rs +++ b/firmware/libs/kaptan-spi/src/lib.rs @@ -1,11 +1,13 @@ //! Non-blocking, chip-select-scoped SPI transfer contract. //! -//! A selected device owns electrical chip-select behavior. A driver retains its -//! transfer storage until a terminal completion event; implementations submit -//! DMA or interrupt-driven work and never wait for a peripheral flag. +//! A selected device owns electrical chip-select behavior and asynchronous +//! transfer storage. Submission copies in; successful completion copies out. +//! No caller borrow or caller address may survive either method call. #![no_std] +pub mod dma; + /// Terminal outcome reported by a selected SPI device. #[derive(Clone, Copy, Debug, Eq, PartialEq)] pub enum SpiCompletion { @@ -17,28 +19,63 @@ pub enum SpiCompletion { TransferFault, } +/// Failure to accept and copy out a completed transfer. No receive bytes are +/// changed on error. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum CompletionError { + /// No terminal event: leave the active transfer entirely untouched. + NotPending, + /// There is no submitted transfer to complete. + NotActive, + /// The destination length differs from the submitted length. + InvalidLength, + /// The transfer failed; no receive bytes are usable. + TransferFault, + /// Quiescence could not be proven. Storage and route are permanently held. + Quarantined, +} + /// Owns one chip-select-scoped, non-blocking SPI route. /// -/// `start_transfer` begins a complete transfer and returns without waiting for -/// a peripheral status flag. The caller owns both buffers until the board -/// reports a terminal `SpiCompletion`; `complete_transfer` releases the route -/// after that terminal outcome. +/// Moving, dropping, or forgetting the device or caller buffers during a +/// transfer must be memory-safe. Hardware accesses only implementation-owned +/// storage whose allocation outlives hardware activity, including cancellation. pub trait SpiDevice { /// Failure to submit a transfer. type StartError; - /// Starts one complete full-duplex transfer. - /// - /// Implementations assert chip select before submitting and deassert it on - /// every terminal completion or submission failure. `receive` receives one - /// octet for each `transmit` octet; the two ranges must have equal length. - fn start_transfer( - &mut self, - transmit: &[u8], - receive: &mut [u8], - ) -> Result<(), Self::StartError>; + /// Identifies a temporary admission rejection. No transfer was accepted. + /// Drivers may retry the same pending operation within their deadline. + fn is_busy(_error: &Self::StartError) -> bool { + false + } - /// Releases transport ownership after a hardware ISR acknowledged a - /// terminal completion. This must not poll or wait. - fn complete_transfer(&mut self, completion: SpiCompletion); + /// Copies `transmit` into bounded route-owned storage and starts one + /// full-duplex transfer. Empty/oversized submissions must fail safely. + /// + /// No pointer into `transmit` is retained. Chip select is asserted before + /// submission; rejecting a concurrent submission must not disturb it. + fn start_transfer(&mut self, transmit: &[u8]) -> Result<(), Self::StartError>; + + /// On `Complete`, proves hardware quiescence before copying exactly the + /// submitted number of bytes to an equally sized `receive` and releasing + /// chip select. An interrupt notification alone is not proof of quiescence. + /// + /// `NotPending` returns that error without aborting or changing anything. + /// Faults cancel the transfer without copying out. Any bounded hardware + /// shutdown that cannot prove quiescence must quarantine the route and its + /// storage permanently, preventing reuse. No destination pointer is retained. + /// Passing `TransferFault` also requests cancellation without an IRQ. It must + /// perform the same bounded shutdown and return `TransferFault` only after + /// quiescence, or `Quarantined` if that cannot be proved. No bytes are copied. + /// + /// Providers must validate current hardware progress, not trust a notification + /// from a previous transfer. Before reuse, drain old hardware flags/data. A + /// delayed notification may fault a new incomplete transfer but must never + /// publish old receive storage as a new result. + fn complete_transfer( + &mut self, + completion: SpiCompletion, + receive: &mut [u8], + ) -> Result<(), CompletionError>; } diff --git a/firmware/libs/kaptan-tx/src/lib.rs b/firmware/libs/kaptan-tx/src/lib.rs index 0ccc497..47e83b9 100644 --- a/firmware/libs/kaptan-tx/src/lib.rs +++ b/firmware/libs/kaptan-tx/src/lib.rs @@ -57,23 +57,31 @@ impl EnqueueResult { /// Result of attempting to begin a transfer. #[derive(Clone, Copy, Debug, Eq, PartialEq)] pub enum StartOutcome { - /// No pending frame was available. + /// No transfer started. Idle, - /// The engine accepted a frame and owns it until a terminal outcome. + /// The transport accepted a frame, which the queue retains until completion or failure. Started, } /// Bounded admission and exactly-one-active-transfer state. /// /// Pending best-effort traffic is replaceable; active traffic is never -/// replaceable. A selected frame moves into its final active storage before -/// the engine sees its address. A rejected start moves it back to the -/// reservation, so a failed submission cannot manufacture a false in-flight -/// frame or discard the selected frame. +/// replaceable. The queue retains the selected frame as logically active until +/// completion or failure. A rejected start returns it to the reservation for +/// retry without discarding it or claiming an active transfer. +/// +/// The queue is movable and provides no stable address for DMA. Transports may +/// copy bytes during a callback or borrow the active frame for bounded writes. +/// Asynchronous device access requires separately owned, address-stable storage. pub struct TransmitQueue { pending: [Option>; N], - reserved: Option>, - active: Option>, + selected: SelectedFrame, +} + +enum SelectedFrame { + Idle, + Reserved(PendingFrame), + Active(PendingFrame), } impl TransmitQueue { @@ -81,8 +89,7 @@ impl TransmitQueue { pub const fn new() -> Self { Self { pending: [None; N], - reserved: None, - active: None, + selected: SelectedFrame::Idle, } } @@ -123,43 +130,46 @@ impl TransmitQueue { } /// Selects and starts at most one pending frame. + /// + /// The callback borrows the frame only for this call. Success retains logical + /// active ownership, not an address or a borrow for later device access. pub fn start_with( &mut self, start: impl FnOnce(&Frame) -> Result<(), Error>, ) -> Result { - if self.active.is_some() { - return Ok(StartOutcome::Idle); - } - if self.reserved.is_none() { - self.reserved = self.take_next(); - } - self.active = self.reserved.take(); - - let Some(active) = self.active.as_ref() else { - return Ok(StartOutcome::Idle); + let frame = match self.selected { + SelectedFrame::Active(_) => return Ok(StartOutcome::Idle), + SelectedFrame::Reserved(frame) => frame, + SelectedFrame::Idle => match self.take_next() { + Some(frame) => frame, + None => return Ok(StartOutcome::Idle), + }, }; - if let Err(error) = start(&active.frame) { - self.reserved = self.active.take(); + self.selected = SelectedFrame::Active(frame); + if let Err(error) = start(&frame.frame) { + self.selected = SelectedFrame::Reserved(frame); return Err(error); } - - self.reserved = None; Ok(StartOutcome::Started) } - /// Releases the active frame after the transport no longer borrows it. + /// Releases the logically active frame after transport completion. pub fn complete(&mut self) -> Option> { - self.active.take() + let SelectedFrame::Active(frame) = self.selected else { + return None; + }; + self.selected = SelectedFrame::Idle; + Some(frame) } /// Releases the active frame after a terminal transport fault. pub fn fail(&mut self) -> Option> { - self.active.take() + self.complete() } - /// Returns whether an engine owns a frame. + /// Returns whether the queue retains a logically active frame. pub const fn is_active(&self) -> bool { - self.active.is_some() + matches!(self.selected, SelectedFrame::Active(_)) } /// Borrows the frame currently owned by the active transport operation. @@ -167,7 +177,10 @@ impl TransmitQueue { /// Packetized transports use this to retain one selected frame while /// advancing it across multiple bounded endpoint writes. pub const fn active(&self) -> Option<&PendingFrame> { - self.active.as_ref() + match &self.selected { + SelectedFrame::Active(frame) => Some(frame), + SelectedFrame::Idle | SelectedFrame::Reserved(_) => None, + } } fn take_next(&mut self) -> Option> { @@ -194,6 +207,8 @@ impl Default for TransmitQueue { #[cfg(test)] mod tests { + extern crate std; + use super::{EnqueueResult, FrameClass, PendingFrame, StartOutcome, TransmitQueue}; #[test] @@ -352,7 +367,7 @@ mod tests { } #[test] - fn rejects_start_without_claiming_active_ownership() { + fn failed_start_retains_selection() { let mut queue = TransmitQueue::::new(); queue.enqueue(PendingFrame { class: FrameClass::BestEffort, @@ -361,6 +376,16 @@ mod tests { assert_eq!(queue.start_with(|_| Err::<(), _>(1)), Err(1)); assert!(!queue.is_active()); + assert_eq!(queue.active(), None); + assert_eq!(queue.complete(), None); + assert_eq!(queue.fail(), None); + assert_eq!( + queue.enqueue(PendingFrame { + class: FrameClass::Vital, + frame: 8, + }), + EnqueueResult::Enqueued + ); assert_eq!( queue.start_with(|frame| { assert_eq!(*frame, 7); @@ -368,31 +393,80 @@ mod tests { }), Ok(StartOutcome::Started) ); + assert_eq!(queue.complete().unwrap().frame, 7); + queue + .start_with(|frame| { + assert_eq!(*frame, 8); + Ok::<_, ()>(()) + }) + .unwrap(); } #[test] - fn keeps_the_dma_source_at_its_active_address() { - let mut queue = TransmitQueue::<[u8; 2], 1>::new(); + fn panicking_start_retains_active() { + let mut queue = TransmitQueue::::new(); queue.enqueue(PendingFrame { + class: FrameClass::Critical, + frame: 7, + }); + let result = std::panic::catch_unwind(std::panic::AssertUnwindSafe(|| { + let _ = queue.start_with::<()>(|_| panic!("transport start panicked")); + })); + assert!(result.is_err()); + assert_eq!(queue.active().unwrap().frame, 7); + assert_eq!( + queue.start_with::<()>(|_| panic!("active transfer cannot restart")), + Ok(StartOutcome::Idle) + ); + assert_eq!(queue.fail().unwrap().frame, 7); + } + + #[test] + fn retains_active_contents_across_pending_admission_and_queue_move() { + let mut queue = TransmitQueue::<[u8; 2], 1>::new(); + let active = PendingFrame { class: FrameClass::BestEffort, frame: [1, 2], - }); + }; + queue.enqueue(active); - let mut submitted_address = 0; assert_eq!( queue.start_with(|frame| { - submitted_address = frame.as_ptr() as usize; + assert_eq!(*frame, active.frame); Ok::<_, ()>(()) }), Ok(StartOutcome::Started) ); - let active_address = queue - .active - .as_ref() - .expect("successful start must retain the active frame") - .frame - .as_ptr() as usize; + assert_eq!( + queue.enqueue(PendingFrame { + class: FrameClass::BestEffort, + frame: [3, 4], + }), + EnqueueResult::Enqueued + ); + let pending = PendingFrame { + class: FrameClass::Critical, + frame: [5, 6], + }; + assert_eq!(queue.enqueue(pending), EnqueueResult::ReplacedBestEffort); + assert_eq!(queue.active(), Some(&active)); - assert_eq!(submitted_address, active_address); + let mut moved = core::hint::black_box(queue); + assert!(moved.is_active()); + assert_eq!(moved.active(), Some(&active)); + assert_eq!( + moved.start_with(|_| panic!("active frame must finish before another start")), + Ok::<_, ()>(StartOutcome::Idle) + ); + assert_eq!(moved.complete(), Some(active)); + assert_eq!(moved.active(), None); + assert_eq!( + moved.start_with(|frame| { + assert_eq!(*frame, pending.frame); + Ok::<_, ()>(()) + }), + Ok(StartOutcome::Started) + ); + assert_eq!(moved.complete(), Some(pending)); } } diff --git a/firmware/platforms/stm32f7/Cargo.toml b/firmware/platforms/stm32f7/Cargo.toml index 8ba59e0..b62916a 100644 --- a/firmware/platforms/stm32f7/Cargo.toml +++ b/firmware/platforms/stm32f7/Cargo.toml @@ -8,18 +8,12 @@ version.workspace = true [lints] workspace = true -[lib] -# This is a bare-metal `no_std` platform library with its own panic handler. -# Host-verifiable invariants live in `tests/platform_safety_boundary.rs`; a -# generated lib-test harness would link `std` and a second panic handler. -test = false - [dependencies] kaptan-err-reportable = { path = "../../libs/kaptan-err-reportable" } kaptan-config = { path = "../../libs/kaptan-config" } +kaptan-ports = { path = "../../libs/kaptan-ports" } kaptan-file-mavlink = { path = "../../libs/kaptan-file-mavlink" } kaptan-data-channel = { path = "../../libs/kaptan-data-channel" } -kaptan-dma = { path = "../../libs/kaptan-dma" } kaptan-drivers = { path = "../../libs/kaptan-drivers" } kaptan-i2c = { path = "../../libs/kaptan-i2c" } kaptan-log = { path = "../../libs/kaptan-log" } @@ -29,7 +23,8 @@ kaptan-sensor = { path = "../../libs/kaptan-sensor" } kaptan-spi = { path = "../../libs/kaptan-spi" } kaptan-time = { path = "../../libs/kaptan-time" } kaptan-tx = { path = "../../libs/kaptan-tx" } -cortex-m = "0.7" +cortex-m = { version = "0.7", features = ["critical-section-single-core"] } +critical-section = "1.2" cortex-m-rt = "0.7" rtic = { version = "2.3", default-features = false, features = [ "thumbv7-backend", @@ -42,3 +37,6 @@ synopsys-usb-otg = { version = "0.4", features = ["cortex-m", "fs"] } usb-device = "0.3" usbd-serial = "0.2" panic-halt = "1" + +[target.'cfg(not(target_os = "none"))'.dev-dependencies] +critical-section = { version = "1.2", features = ["std"] } diff --git a/firmware/platforms/stm32f7/examples/icm_dual_uart.rs b/firmware/platforms/stm32f7/examples/icm_dual_uart.rs new file mode 100644 index 0000000..f204a1a --- /dev/null +++ b/firmware/platforms/stm32f7/examples/icm_dual_uart.rs @@ -0,0 +1,32 @@ +#![cfg_attr(target_os = "none", no_std)] +#![cfg_attr(target_os = "none", no_main)] +#[cfg(target_os = "none")] +mod support; +#[cfg(target_os = "none")] +mod target { + use platform_stm32f7::kaptan_ports::PortCatalog; + use platform_stm32f7::*; + pub struct Board; + impl F7Board for Board { + type UserConfig = super::support::UserConfig; + type ImmutableConfig = F722ImmutableConfig; + const FACTORY_USER_CONFIG: Self::UserConfig = super::support::FACTORY; + const IMMUTABLE_CONFIG: Self::ImmutableConfig = F722ImmutableConfig { + name: "synthetic ICM dual UART", + service_hz: 100, + hardware: F722Hardware::new( + Icm42688Spi3 { + mount: ImuMount::Yaw180, + }, + SerialInventory::new(&super::support::Both::DESCRIPTORS), + StatusLed::ActiveLowPd2, + VerticalSensor::None, + MotorPins::Four, + ), + }; + } +} +#[cfg(target_os = "none")] +platform_stm32f7::flight_firmware!(crate::target::Board); +#[cfg(not(target_os = "none"))] +fn main() {} diff --git a/firmware/platforms/stm32f7/examples/mpu_dual_uart.rs b/firmware/platforms/stm32f7/examples/mpu_dual_uart.rs new file mode 100644 index 0000000..6054bdb --- /dev/null +++ b/firmware/platforms/stm32f7/examples/mpu_dual_uart.rs @@ -0,0 +1,38 @@ +#![cfg_attr(target_os = "none", no_std)] +#![cfg_attr(target_os = "none", no_main)] +#[cfg(target_os = "none")] +mod support; +#[cfg(target_os = "none")] +mod target { + use platform_stm32f7::kaptan_ports::PortCatalog; + use platform_stm32f7::*; + pub struct Board; + impl F7Board for Board { + type UserConfig = super::support::UserConfig; + type ImmutableConfig = F722ImmutableConfig; + const FACTORY_USER_CONFIG: Self::UserConfig = super::support::FACTORY; + const IMMUTABLE_CONFIG: Self::ImmutableConfig = F722ImmutableConfig { + name: "synthetic MPU dual UART", + service_hz: 100, + hardware: F722Hardware::new( + Mpu6000Spi1 { + mount: ImuMount::BodyAligned, + config: Mpu6000Config { + accelerometer: AccelerometerRange::PlusMinus4G, + gyroscope: GyroscopeRange::PlusMinus500DegreesPerSecond, + digital_low_pass_filter: DigitalLowPassFilter::Bandwidth42Hz, + sample_rate_divider: 9, + }, + }, + SerialInventory::new(&super::support::Both::DESCRIPTORS), + StatusLed::ActiveHighPa13, + VerticalSensor::None, + MotorPins::Four, + ), + }; + } +} +#[cfg(target_os = "none")] +platform_stm32f7::flight_firmware!(crate::target::Board); +#[cfg(not(target_os = "none"))] +fn main() {} diff --git a/firmware/platforms/stm32f7/examples/support/mod.rs b/firmware/platforms/stm32f7/examples/support/mod.rs new file mode 100644 index 0000000..545d139 --- /dev/null +++ b/firmware/platforms/stm32f7/examples/support/mod.rs @@ -0,0 +1,11 @@ +//! UART catalog for compile-check examples. +use platform_stm32f7::{ + F722UserConfig, UART1, UART2, + kaptan_ports::{PortCatalog, PortDescriptor}, +}; +pub struct Both; +impl PortCatalog<2> for Both { + const DESCRIPTORS: [PortDescriptor; 2] = [UART1, UART2]; +} +pub type UserConfig = F722UserConfig; +pub const FACTORY: UserConfig = UserConfig::FACTORY; diff --git a/firmware/platforms/stm32f7/src/f722/board.rs b/firmware/platforms/stm32f7/src/f722/board.rs index 898d551..ee0a2c3 100644 --- a/firmware/platforms/stm32f7/src/f722/board.rs +++ b/firmware/platforms/stm32f7/src/f722/board.rs @@ -1,18 +1,23 @@ -//! Physical-sensor telemetry STM32F722 board-I/O state and ownership graph. - -use core::sync::atomic::{AtomicU8, Ordering}; - -use super::peripherals::{ - DwtMicrosecondClock, I2c1, I2c1Config, I2c1Error, ReceiveInterrupt, Tim2Periodic, Uart4, - configure_exti3_pin_pc3, configure_exti4_pin_pa4, configure_i2c1_pins_pb8_pb9, - configure_spi1_pins_pa5_pa6_pa7, configure_spi3_pins_pc10_pc11_pc12, - configure_uart4_pins_pa0_pa1, configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9, - configure_usart2_pins_pa2_pa3, configure_usb_fs_pins_pa11_pa12, +//! F722 initialization and protocol services. +use super::{ + clock, + config::{ + BootDiagnostic, F722ConfigFile, F722ConfigStore, F722Nor, UserConfigModel, + recover_configuration, + }, + data_channel::{F722StatusLed, GeprcStatusLed, IoBatch, Link, PortIo, SERVICE_HZ, UsbIo}, + peripherals::*, + routes::{F722Route, ImuResources}, + sensors::{ImuEvent, ImuService}, + serial::SerialIo, }; -use kaptan_config::Config; +use crate::{ + BoardError, F7ImmutableConfig, F722Hardware, SerialInventory, StatusLed, VerticalSensor, +}; +use kaptan_config::{Config, ConfigModel}; use kaptan_data_channel::{ - DataChannel, DataChannelRates, DataSources, LatestImuMailbox, LatestOpticalFlowMailbox, - LatestRangeMailbox, + DataChannelRates, DataCollector, DataSources, LatestImuMailbox, LatestOpticalFlowMailbox, + LatestRangeMailbox, Producer, }; use kaptan_diagnostics_mavlink::{encode_error, encode_log}; use kaptan_drivers::{ @@ -20,781 +25,706 @@ use kaptan_drivers::{ AcquisitionMode, Error as LidarError, LidarLiteV3, LidarLiteV3Service, LidarLiteV3ServiceEvent, StartupError, }, - mtf01::Mtf01Service, + mtf01::Mtf01, }; use kaptan_err_reportable::{ErrorId, ErrorSink}; +use kaptan_file_mavlink::{ + FileProvider, FileResponse, FileService, IngressId, Principal, TRANSFER_REPLAY_LEASE_SECONDS, + Target, TransferPeer, wire, +}; use kaptan_log::{DeliveryClass, LogSinkQueue, TransportAdmission}; +use kaptan_mavlink::{MavlinkFrame, generated::kaptan, receiver::IntegrityCheckedFrame}; +use kaptan_ports::{PortId, Role}; +use kaptan_sensor::{imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot}; use kaptan_spi::SpiCompletion; +use kaptan_time::Timestamp; +use kaptan_tx::{FrameClass, PendingFrame}; use stm32f7::stm32f722; -use kaptan_file_mavlink::{ - FileResponse, FileService, FileStatus, Principal, TRANSFER_REPLAY_LEASE_SECONDS, Target, wire, -}; -use kaptan_mavlink::{generated::kaptan, receiver::IntegrityMavlinkReceiver}; - -use crate::{BoardError, F7ImmutableConfig, F722Hardware, F722Profile, F722StatusLedRoute}; - -use super::config::{ - F722ConfigFile, F722ConfigManager, F722ConfigStore, F722Nor, UserConfigModel, project_runtime, -}; -use super::{ - VerticalRoute, clock, - data_channel::{ - F722MavlinkLink, F722StatusLed, GeprcStatusLed, MavlinkIngress, SERVICE_HZ, - SERVICE_TIMER_CONFIG, - }, - interrupts, - sensors::{F722Imu, ImuEvent}, -}; - -const UART4_CLOCK_HZ: u32 = 54_000_000; -const I2C1_CONFIG: I2c1Config = I2c1Config::from_timing_register(0x2040_4768); -const LIDAR_MODE: AcquisitionMode = AcquisitionMode::WithReceiverBiasCorrection; -const LIDAR_STARTUP_TICKS: u8 = 3; -const FILE_TRANSFER_LEASE_TICKS: u16 = (SERVICE_HZ * TRANSFER_REPLAY_LEASE_SECONDS as usize) as u16; -// The IMU is the largest current scheduled payload; unsigned MAVLink 2 adds a -// ten-byte header and two-byte checksum. -const SCHEDULED_FRAME_MAXIMUM_BYTES: usize = 12 + kaptan::IMU_DATA::ENCODED_LEN; -// User/immutable validation permits at most one scheduled frame per 100 Hz -// service tick. Configuration replies and diagnostics are event-driven and -// use bounded queue admission; they do not invent a periodic link reservation. -pub(crate) const SCHEDULED_TELEMETRY_BYTES_PER_SECOND: usize = - SERVICE_HZ * SCHEDULED_FRAME_MAXIMUM_BYTES; - -/// Returns whether the immutable board UART configuration can carry its -/// deterministic telemetry schedule at ten UART bits per byte. -pub(crate) const fn scheduled_link_budget_fits(baud: u32) -> bool { - SCHEDULED_TELEMETRY_BYTES_PER_SECOND < baud as usize / 10 -} -static UART_COMPLETION: AtomicU8 = AtomicU8::new(0); -static SAMPLES: LatestImuMailbox = LatestImuMailbox::new(); -static RANGES: LatestRangeMailbox = LatestRangeMailbox::new(); -static OPTICAL_FLOW: LatestOpticalFlowMailbox = LatestOpticalFlowMailbox::new(); - -type F722Range = LidarLiteV3Service; -/// Opaque RTIC-local MTF-01 service type; its route is platform-owned. -pub type F722Mtf01 = Mtf01Service; -/// Opaque RTIC-local UART4 owner; its construction is platform-owned. -pub type F722Mtf01Uart = Uart4; -/// Opaque RTIC-local service timer; its construction is platform-owned. +type F722Range = LidarLiteV3Service; pub type F722ServiceTimer = Tim2Periodic; -/// RTIC-shared MAVLink state. -/// -/// The UART and USB handlers do bounded byte/packet admission at priority 8; -/// protocol dispatch and transmission policy remain in the priority-9 worker. -/// This state therefore cannot inherit the IMU or vertical-sensor IRQ ceiling. -pub struct F722MavlinkState -where - C: Clone + Config, - I: F7ImmutableConfig, -{ - data_channel: DataChannel<'static, SERVICE_HZ>, - link: F722MavlinkLink, - config_file: F722ConfigFile, +// Fixed boot-role storage lives in RTIC static RAM; there is no heap allocator. +#[allow(clippy::large_enum_variant)] +enum SerialEndpoint { + Mavlink(Link), + Mtf01 { + service: Mtf01, + range: Producer<'static, RangeSnapshot>, + optical_flow: Producer<'static, OpticalFlowSnapshot>, + }, +} + +/// Resource types used by the RTIC application. +pub trait F722MavlinkState { + type Protocol; + type Ports; + type Usb; + type Clock; + type ImuPublisher; +} + +/// This entire state is worker-local. No IRQ ever borrows it. +struct ProtocolWorker { + usb: Link, + serial: [Option; N], + inventory: SerialInventory, + collector: DataCollector<'static>, + config_file: P, file_service: FileService, - uart_receiver: IntegrityMavlinkReceiver<1>, - file_lease_ticks: u16, + file_lease: FileLease, mavlink_errors: ErrorSink, mavlink_logs: LogSinkQueue<1, 1>, - led: F722StatusLed, } -/// RTIC-shared LIDAR state. -/// -/// I2C event/error IRQs and the service timer are its only users. MTF-01 is a -/// separate UART-only local resource, so it cannot raise this ceiling. -pub struct F722LidarState { - range: Option, - startup_ticks: u8, -} - -/// Explicit handoff from platform initialization to the RTIC resource graph. -/// -/// This is not runtime state indirection: each field is moved exactly once -/// into the single RTIC owner that accesses it. -pub struct F722InitResources -where +pub struct ProtocolState< C: Clone + Config, I: F7ImmutableConfig, + const N: usize, + const MAX_PAYLOAD_BYTES: usize, +> { + worker: ProtocolWorker, N>, + led: F722StatusLed, +} +impl + ProtocolState { - #[doc(hidden)] - pub imu: F722Imu, - #[doc(hidden)] - pub mtf01: Option, - #[doc(hidden)] - pub mtf01_uart: Option, - #[doc(hidden)] - pub timer: F722ServiceTimer, - #[doc(hidden)] - pub lidar: F722LidarState, - #[doc(hidden)] - pub mavlink: F722MavlinkState, + pub fn process_worker_events( + &mut self, + events: u8, + exchange: impl FnMut(u8, Option>) -> IoBatch, + now: impl FnMut() -> Timestamp, + ) -> bool { + let (pending, completed) = self.worker.process_worker_events(events, exchange, now); + if completed { + self.led.toggle(); + } + pending + } } -/// Validates user-controlled runtime values against immutable electrical and -/// platform constraints before initialization claims a non-safety peripheral. -pub(crate) fn validate_board_config( +/// Lease time is real monotonic time, never a count of coalesced service events. +#[derive(Default)] +struct FileLease { + deadline: Option, +} +impl FileLease { + fn refresh(&mut self, now: u64) { + self.deadline = + Some(now.saturating_add(u64::from(TRANSFER_REPLAY_LEASE_SECONDS) * 1_000_000)); + } + fn expired(&mut self, now: u64) -> bool { + if self.deadline.is_some_and(|deadline| now >= deadline) { + self.deadline = None; + true + } else { + false + } + } +} + +pub struct F722LidarState { + range: Option<(F722Range, Producer<'static, RangeSnapshot>)>, + startup_ticks: u8, +} +pub struct F722InitResources< + C: Clone + Config, + I: F7ImmutableConfig, + const N: usize, + const MAX_PAYLOAD_BYTES: usize, +> { + pub imu: ::Imu, + pub timer: F722ServiceTimer, + pub lidar: F722LidarState, + pub mavlink: ProtocolState, + pub uarts: PortIo, + pub usb: UsbIo, + pub serial_clock: TimerClock, + pub imu_publisher: Producer<'static, ImuSnapshot>, +} +impl + F722MavlinkState for F722InitResources +{ + type Protocol = ProtocolState; + type Ports = PortIo; + type Usb = UsbIo; + type Clock = TimerClock; + type ImuPublisher = Producer<'static, ImuSnapshot>; +} + +pub(crate) fn validate_board_config( config: &C, immutable: &I, - hardware: F722Hardware, -) -> Result<(), BoardError> -where - C: Config, - I: F7ImmutableConfig, -{ - ::validate(config).map_err(|_| BoardError::Configuration)?; - let _ = project_runtime(config, immutable).map_err(|_| BoardError::Configuration)?; - let immutable = immutable.f722(); - if immutable.service_hz != SERVICE_HZ { +) -> Result<(), BoardError> { + if immutable.f722().service_hz != SERVICE_HZ { return Err(BoardError::UnsupportedServiceFrequency); } - let vertical_baud_is_valid = match (hardware.vertical(), hardware.vertical_sensor_baud()) { - (VerticalRoute::Mtf01, Some(baud)) => baud != 0, - (VerticalRoute::None | VerticalRoute::LidarLiteV3, None) => true, - _ => false, - }; - if !vertical_baud_is_valid { - return Err(BoardError::VerticalSensorCapabilityMismatch); - } - if !scheduled_link_budget_fits(hardware.mavlink_baud()) { - return Err(BoardError::TelemetryLinkOvercommitted); - } + UserConfigModel::::new(config.clone()) + .validate(immutable) + .map_err(|_| BoardError::Configuration)?; Ok(()) } -pub(crate) fn initialize( +pub(crate) fn initialize< + C: Clone + Config, + I: F7ImmutableConfig, + const N: usize, + const MAX_PAYLOAD_BYTES: usize, +>( mut core: cortex_m::Peripherals, device: stm32f722::Peripherals, factory: C, immutable: I, - hardware: F722Hardware, -) -> Result, BoardError> -where - C: Clone + Config, - I: F7ImmutableConfig, -{ - let clock = clock::initialize(&mut core, &device).map_err(|_| BoardError::Initialization)?; - - // The profile is written once before any interrupt is enabled. ISR helpers - // read it with Acquire ordering and only access the initialized route. - interrupts::activate_profile(hardware.profile()); - - match hardware.profile() { - F722Profile::Mpu6000Spi1Usart1 => initialize_mpu6000::( - device, clock, factory, immutable, hardware, - ), - F722Profile::Icm42688Spi3Usart2 => initialize_icm42688::( - device, clock, factory, immutable, hardware, - ), - } -} - -fn initialize_mpu6000( - device: stm32f722::Peripherals, - clock: DwtMicrosecondClock, - factory: C, - immutable: I, - hardware: F722Hardware, -) -> Result, BoardError> -where - C: Clone + Config, - I: F7ImmutableConfig, -{ - let configuration = - recover_configuration::(device.FLASH, factory, immutable)?; - let config = configuration.active().clone(); - configure_spi1_pins_pa5_pa6_pa7(&device.RCC, &device.GPIOA); - configure_exti4_pin_pa4(&device.GPIOA); - configure_exti4(&device.RCC, device.SYSCFG, device.EXTI); - configure_usart1_tx_pin_pa9(&device.GPIOA); - configure_usart1_rx_pin_pa10(&device.GPIOA); - let vertical = hardware.vertical(); - configure_vertical_pins(&device.RCC, &device.GPIOA, &device.GPIOB, vertical); - - let (range, mtf01, mtf01_uart) = initialize_vertical( - &device.RCC, - device.I2C1, - device.UART4, - clock.fork(), - clock.fork(), - vertical, - hardware.vertical_sensor_baud(), - )?; - let imu = F722Imu::mpu6000( - &device.RCC, - device.SPI1, - device.DMA2, - device.GPIOB, - clock.fork(), - hardware.imu_mount(), - hardware - .mpu6000_config() - .ok_or(BoardError::Initialization)?, - ); - let usb = initialize_usb( - &device.RCC, - &device.GPIOA, - device.OTG_FS_GLOBAL, - device.OTG_FS_DEVICE, - device.OTG_FS_PWRCLK, - immutable.f722().name, - )?; - let link = F722MavlinkLink::usart1(&device.RCC, device.USART1, hardware.mavlink_baud(), usb) - .map_err(|_| BoardError::Initialization)?; - if hardware.status_led() != F722StatusLedRoute::Pd2ActiveLow { - return Err(BoardError::Initialization); - } - let led = F722StatusLed::Kakute(super::peripherals::ActiveLowLedPd2::new( - &device.RCC, - device.GPIOD, - )); - finish( + hardware: F722Hardware, +) -> Result, BoardError> { + let (clock, timer) = clock::initialize( + &mut core, &device.RCC, + &device.PWR, + &device.FLASH, device.TIM2, - config, - immutable, - imu, - range, - mtf01, - mtf01_uart, - link, - led, - configuration, + &device.DBGMCU, ) -} - -fn initialize_icm42688( - device: stm32f722::Peripherals, - clock: DwtMicrosecondClock, - factory: C, - immutable: I, - hardware: F722Hardware, -) -> Result, BoardError> -where - C: Clone + Config, - I: F7ImmutableConfig, -{ - let configuration = - recover_configuration::(device.FLASH, factory, immutable)?; - let config = configuration.active().clone(); - configure_spi3_pins_pc10_pc11_pc12(&device.RCC, &device.GPIOC); - configure_exti3_pin_pc3(&device.GPIOC); - configure_exti3(&device.RCC, device.SYSCFG, device.EXTI); - // PA2/PA3 writes take effect only after enabling the GPIOA bus clock. - // The ICM route enables GPIOA later for chip select, which is too late for - // the USART2 alternate-function configuration below. + .map_err(|_| BoardError::Initialization)?; + let configuration = recover_configuration( + UserConfigModel::::new(factory), + immutable, + F722ConfigStore::::new(F722Nor::new(device.FLASH)), + ) + .map_err(|_| BoardError::Configuration)?; + match configuration.diagnostic() { + BootDiagnostic::Healthy => {} + BootDiagnostic::InvalidPersisted(_) => { + kaptan_log::error!("invalid stored configuration; UARTs disabled; repair via USB") + } + BootDiagnostic::StorageUnknown => kaptan_log::error!( + "configuration storage unknown; UARTs disabled; restart recovery required" + ), + } + let active = configuration.active(); + let runtime = active.runtime; + let rates = DataChannelRates { + imu_hz: usize::from(runtime.imu_hz), + heartbeat_hz: usize::from(runtime.heartbeat_hz), + range_hz: runtime.range_hz.map(usize::from), + optical_flow_hz: runtime.optical_flow_hz.map(usize::from), + }; + // On the single-core F722, singleton! yields this permanent backing once. + // A later initialization attempt fails instead of creating mutable aliases. + let mailboxes = cortex_m::singleton!( + : (LatestImuMailbox, LatestRangeMailbox, LatestOpticalFlowMailbox) = ( + LatestImuMailbox::new(), + LatestRangeMailbox::new(), + LatestOpticalFlowMailbox::new(), + ) + ) + .ok_or(BoardError::Initialization)?; + let (imu_publisher, imu_source) = mailboxes.0.split(); + let (range_publisher, range_source) = mailboxes.1.split(); + let (flow_publisher, flow_source) = mailboxes.2.split(); + let mut range_publisher = Some(range_publisher); + let mut flow_publisher = Some(flow_publisher); device .RCC .ahb1enr() .modify(|_, write| write.gpioaen().set_bit()); - configure_usart2_pins_pa2_pa3(&device.GPIOA); - let vertical = hardware.vertical(); - configure_vertical_pins(&device.RCC, &device.GPIOA, &device.GPIOB, vertical); - - let (range, mtf01, mtf01_uart) = initialize_vertical( - &device.RCC, - device.I2C1, - device.UART4, - clock.fork(), - clock.fork(), - vertical, - hardware.vertical_sensor_baud(), - )?; - let imu = F722Imu::icm42688( - &device.RCC, - device.SPI3, - device.DMA1, - &device.GPIOA, - clock.fork(), - hardware.imu_mount(), + let _ = device.RCC.ahb1enr().read(); + let mut serial = core::array::from_fn(|_| None); + let mut uarts = PortIo::new(hardware.serial()); + // Taking tokens does not initialize a disabled peripheral or its pins. + let mut uart1 = Some(device.USART1); + let mut uart2 = Some(device.USART2); + let mut uart3 = Some(device.USART3); + let mut uart4 = Some(device.UART4); + let mut uart6 = Some(device.USART6); + for (index, (id, settings)) in active.ports.iter().enumerate() { + if settings.role == Role::Disabled { + continue; + } + let io = match id.get() { + 1 => { + configure_usart1_tx_pin_pa9(&device.GPIOA); + configure_usart1_rx_pin_pa10(&device.GPIOA); + OwnedUart::Uart1( + Usart1Tx::new( + &device.RCC, + uart1.take().unwrap(), + 108_000_000, + settings.baud, + ) + .map_err(|_| BoardError::Initialization)?, + ) + } + 2 => { + configure_usart2_pins_pa2_pa3(&device.GPIOA); + OwnedUart::Uart2( + Usart2Tx::new( + &device.RCC, + uart2.take().unwrap(), + 54_000_000, + settings.baud, + ) + .map_err(|_| BoardError::Initialization)?, + ) + } + 3 => { + configure_usart3_pins_pb10_pb11(&device.RCC, &device.GPIOB); + OwnedUart::Uart3( + Usart3::new( + &device.RCC, + uart3.take().unwrap(), + 54_000_000, + settings.baud, + ) + .map_err(|_| BoardError::Initialization)?, + ) + } + 4 => { + configure_uart4_pins_pa0_pa1(&device.GPIOA); + OwnedUart::Uart4( + Uart4::new( + &device.RCC, + uart4.take().unwrap(), + 54_000_000, + settings.baud, + ) + .map_err(|_| BoardError::Initialization)?, + ) + } + 6 => { + configure_usart6_pins_pc6_pc7(&device.RCC, &device.GPIOC); + OwnedUart::Uart6( + Usart6::new( + &device.RCC, + uart6.take().unwrap(), + 108_000_000, + settings.baud, + ) + .map_err(|_| BoardError::Initialization)?, + ) + } + _ => return Err(BoardError::Configuration), + }; + uarts.ports[index] = SerialIo::new(Some(io), settings.role == Role::Mavlink); + serial[index] = Some(match settings.role { + Role::Mavlink => { + SerialEndpoint::Mavlink(Link::new(runtime.system_id, runtime.component_id, rates)?) + } + Role::Mtf01 => SerialEndpoint::Mtf01 { + service: Mtf01::new(), + range: range_publisher.take().ok_or(BoardError::Configuration)?, + optical_flow: flow_publisher.take().ok_or(BoardError::Configuration)?, + }, + Role::Disabled => unreachable!(), + }); + } + let range = match hardware.vertical() { + VerticalSensor::None => None, + VerticalSensor::LidarLiteV3I2c1 => { + configure_i2c1_pins_pb8_pb9(&device.RCC, &device.GPIOB); + Some(( + LidarLiteV3Service::new( + clock, + LidarLiteV3::new(I2c1::new( + &device.RCC, + device.I2C1, + I2c1Config::from_timing_register(0x2040_4768), + )), + AcquisitionMode::WithReceiverBiasCorrection, + ), + range_publisher.take().ok_or(BoardError::Configuration)?, + )) + } + }; + let imu = hardware.route.initialize( + ImuResources { + rcc: &device.RCC, + gpioa: &device.GPIOA, + gpiob: device.GPIOB, + gpioc: &device.GPIOC, + syscfg: device.SYSCFG, + exti: device.EXTI, + spi1: device.SPI1, + spi3: device.SPI3, + dma1: device.DMA1, + dma2: device.DMA2, + }, + clock, ); - let usb = initialize_usb( - &device.RCC, - &device.GPIOA, + configure_usb_fs_pins_pa11_pa12(&device.RCC, &device.GPIOA); + let (usb, rx_interrupt) = usb_mavlink( device.OTG_FS_GLOBAL, device.OTG_FS_DEVICE, device.OTG_FS_PWRCLK, + clock, immutable.f722().name, - )?; - let link = F722MavlinkLink::usart2(&device.RCC, device.USART2, hardware.mavlink_baud(), usb) - .map_err(|_| BoardError::Initialization)?; - if hardware.status_led() != F722StatusLedRoute::Pa13ActiveHigh { - return Err(BoardError::Initialization); - } - let led = F722StatusLed::Geprc(GeprcStatusLed::new(&device.RCC, device.GPIOA)); - finish( - &device.RCC, - device.TIM2, - config, - immutable, - imu, - range, - mtf01, - mtf01_uart, - link, - led, - configuration, - ) -} - -#[allow(clippy::too_many_arguments)] -fn finish( - rcc: &stm32f722::RCC, - tim2: stm32f722::TIM2, - config: C, - immutable: I, - imu: F722Imu, - range: Option, - mtf01: Option, - mtf01_uart: Option, - link: F722MavlinkLink, - led: F722StatusLed, - configuration: F722ConfigManager, -) -> Result, BoardError> -where - C: Clone + Config, - I: F7ImmutableConfig, -{ - let runtime = project_runtime(&config, &immutable).map_err(|_| BoardError::Configuration)?; - let system_id = runtime.system_id; - let component_id = runtime.component_id; - let rates = DataChannelRates { - imu_hz: usize::from(runtime.imu_hz), - range_hz: runtime.range_hz.map(usize::from), - optical_flow_hz: runtime.optical_flow_hz.map(usize::from), - heartbeat_hz: usize::from(runtime.heartbeat_hz), - }; - let mavlink_log_filter = runtime.mavlink_log_level_filter; - let data_channel = DataChannel::new( - system_id, - component_id, - rates, - 10, - DataSources { - imu: &SAMPLES, - range: &RANGES, - optical_flow: &OPTICAL_FLOW, - }, ) .map_err(|_| BoardError::Initialization)?; + let usb = UsbIo::new(usb, rx_interrupt); + let led = match hardware.led { + StatusLed::ActiveLowPd2 => { + F722StatusLed::Pd2(ActiveLowLedPd2::new(&device.RCC, device.GPIOD)) + } + StatusLed::ActiveHighPa13 => { + F722StatusLed::Pa13(GeprcStatusLed::new(&device.RCC, device.GPIOA)) + } + }; Ok(F722InitResources { + uarts, + usb, + serial_clock: clock, + imu_publisher, imu, - mtf01, - mtf01_uart, - timer: Tim2Periodic::new(rcc, tim2, SERVICE_TIMER_CONFIG), + timer, lidar: F722LidarState { range, startup_ticks: 0, }, - mavlink: F722MavlinkState { - data_channel, - link, - config_file: F722ConfigFile::new(configuration), - file_service: FileService::new(Target::new(system_id, component_id)), - uart_receiver: IntegrityMavlinkReceiver::new(), - file_lease_ticks: 0, - mavlink_errors: ErrorSink::new(), - mavlink_logs: LogSinkQueue::new(mavlink_log_filter), + mavlink: ProtocolState { led, + worker: ProtocolWorker { + usb: Link::new(runtime.system_id, runtime.component_id, rates)?, + serial, + inventory: hardware.serial(), + collector: DataCollector::new(DataSources { + imu: imu_source, + range: range_source, + optical_flow: flow_source, + }), + config_file: F722ConfigFile::new(configuration), + file_service: FileService::new(Target::new( + runtime.system_id, + runtime.component_id, + )), + file_lease: FileLease::default(), + mavlink_errors: ErrorSink::new(), + mavlink_logs: LogSinkQueue::new(runtime.mavlink_log_level_filter), + }, }, }) } -fn recover_configuration( - flash: stm32f722::FLASH, - factory: C, - immutable: I, -) -> Result, BoardError> -where - C: Clone + Config, - I: F7ImmutableConfig, -{ - let candidate = UserConfigModel::::new(factory); - let store = F722ConfigStore::::new(F722Nor::new(flash)); - F722ConfigManager::::recover_compatible_or_factory( - candidate, immutable, store, +fn mavlink_links<'a>( + usb: &'a mut Link, + serial: &'a mut [Option], + inventory: SerialInventory, +) -> impl Iterator { + core::iter::once((0, usb)).chain( + inventory + .descriptors() + .iter() + .zip(serial.iter_mut()) + .filter_map(|(descriptor, endpoint)| match endpoint { + Some(SerialEndpoint::Mavlink(link)) => Some((descriptor.id.get(), link)), + _ => None, + }), ) - .map_err(|_| BoardError::Configuration) } -fn initialize_usb( - rcc: &stm32f722::RCC, - gpioa: &stm32f722::GPIOA, - global: stm32f722::OTG_FS_GLOBAL, - device: stm32f722::OTG_FS_DEVICE, - power_clock: stm32f722::OTG_FS_PWRCLK, - product: &'static str, -) -> Result { - configure_usb_fs_pins_pa11_pa12(rcc, gpioa); - super::peripherals::usb_mavlink(global, device, power_clock, product) - .map_err(|_| BoardError::Initialization) -} - -fn configure_vertical_pins( - rcc: &stm32f722::RCC, - gpioa: &stm32f722::GPIOA, - gpiob: &stm32f722::GPIOB, - vertical: VerticalRoute, -) { - match vertical { - VerticalRoute::None => {} - VerticalRoute::Mtf01 => configure_uart4_pins_pa0_pa1(gpioa), - VerticalRoute::LidarLiteV3 => configure_i2c1_pins_pb8_pb9(rcc, gpiob), - } -} - -type VerticalResources = (Option, Option, Option); - -fn initialize_vertical( - rcc: &stm32f722::RCC, - i2c: stm32f722::I2C1, - uart4: stm32f722::UART4, - mtf01_clock: DwtMicrosecondClock, - range_clock: DwtMicrosecondClock, - vertical: VerticalRoute, - vertical_sensor_baud: Option, -) -> Result { - match vertical { - VerticalRoute::None => Ok((None, None, None)), - VerticalRoute::Mtf01 => { - let baud = vertical_sensor_baud.ok_or(BoardError::Initialization)?; - Ok(( - None, - Some(Mtf01Service::new(mtf01_clock)), - Some( - Uart4::new(rcc, uart4, UART4_CLOCK_HZ, baud) - .map_err(|_| BoardError::Initialization)?, - ), - )) +impl ProtocolWorker { + fn expire_lease(&mut self, now: Timestamp) { + if self.file_lease.expired(now.as_microseconds()) + && self.file_service.expire(&mut self.config_file) + { + kaptan_log::debug!("file transfer expired"); } - VerticalRoute::LidarLiteV3 => Ok(( - Some(LidarLiteV3Service::new( - range_clock, - LidarLiteV3::new(I2c1::new(rcc, i2c, I2C1_CONFIG)), - LIDAR_MODE, - )), - None, - None, - )), + } + fn process_worker_events( + &mut self, + events: u8, + mut exchange: impl FnMut(u8, Option>) -> IoBatch, + mut now: impl FnMut() -> Timestamp, + ) -> (bool, bool) { + self.expire_lease(now()); + let mut pending = false; + let mut completed = false; + // One 16-byte chunk per UART per turn. Parse sensor input before any + // potentially expensive file request; all parsing is outside I/O locks. + for (descriptor, endpoint) in self.inventory.descriptors().iter().zip(&mut self.serial) { + let batch = exchange(descriptor.id.get(), None); + pending |= batch.pending; + completed |= batch.serial.completed; + let chunk = batch.serial; + match endpoint { + Some(SerialEndpoint::Mavlink(link)) => { + if chunk.discontinuity { + link.receiver.reset(); + kaptan_log::warn!("mavlink receive discontinuity; framing reset"); + } + for byte in &chunk.bytes[..chunk.length] { + link.receiver.receive_byte(*byte); + } + } + Some(SerialEndpoint::Mtf01 { + service, + range, + optical_flow, + }) => { + if chunk.discontinuity { + service.reset(); + kaptan_log::error!("mtf01 receive discontinuity; framing reset"); + } + for index in 0..chunk.length { + if let Some(report) = + service.receive_byte(chunk.timestamps[index], chunk.bytes[index]) + { + if let Some(snapshot) = report.range_snapshot() { + range.publish(snapshot); + } + if let Some(flow) = report.optical_flow_snapshot() { + optical_flow.publish(flow); + } + } + } + } + None => {} + } + } + let usb = exchange(0, None); + pending |= usb.pending; + if let Some(frame) = usb.usb { + self.dispatch_frame(0, frame, &mut exchange, &mut now); + } + for descriptor in self.inventory.descriptors() { + let id = descriptor.id.get(); + // Receiver capacity is two, so each turn performs bounded dispatch. + while let Some(frame) = self.link(id).and_then(|link| link.receiver.take_received()) { + self.dispatch_frame(id, frame, &mut exchange, &mut now); + } + } + if events & 1 != 0 { + let samples = self.collector.snapshot(); + // Dispatch and acquisition may delay this turn. Check freshness + // against the current time. + let now = now(); + for (id, link) in mavlink_links(&mut self.usb, &mut self.serial, self.inventory) { + if let Some(frame) = link.channel.on_service_tick(&samples, now) { + exchange(id, Some(frame)); + } + } + self.drain_one_mavlink_diagnostic(&mut exchange); + } + (pending, completed) + } + fn link(&mut self, id: u8) -> Option<&mut Link> { + if id == 0 { + return Some(&mut self.usb); + } + match self.serial[self.inventory.index(PortId::new(id))?].as_mut()? { + SerialEndpoint::Mavlink(link) => Some(link), + _ => None, + } + } + fn dispatch_frame( + &mut self, + id: u8, + frame: IntegrityCheckedFrame, + exchange: &mut impl FnMut(u8, Option>) -> IoBatch, + now: &mut impl FnMut() -> Timestamp, + ) { + let Some(principal) = + Principal::try_new(frame.raw().system_id(), frame.raw().component_id()) + else { + return; + }; + let Some(message) = frame.decode() else { + return; + }; + let Ok(request) = wire::decode_request(&message) else { + return; + }; + let peer = TransferPeer { + principal, + ingress: IngressId(id), + }; + // Recheck before each dispatch so queued requests cannot extend an + // expired session. + self.expire_lease(now()); + let Some(result) = self + .file_service + .dispatch(&mut self.config_file, peer, request) + else { + return; + }; + if result.refresh_lease { + self.file_lease.refresh(now().as_microseconds()); + } + self.send_file_response(id, result.response, exchange); + } + fn send_file_response( + &mut self, + id: u8, + response: FileResponse, + exchange: &mut impl FnMut(u8, Option>) -> IoBatch, + ) { + let Some(link) = self.link(id) else { + return; + }; + let class = FrameClass::Critical; + let frame = match wire::encode_response(response) { + kaptan::MavMessage::FILE_CONTROL(data) => link.channel.encode_message(class, &data), + kaptan::MavMessage::FILE_DATA(data) => link.channel.encode_message(class, &data), + _ => unreachable!("file response encoder returned another message"), + }; + exchange(id, Some(frame)); + } + fn drain_one_mavlink_diagnostic( + &mut self, + exchange: &mut impl FnMut(u8, Option>) -> IoBatch, + ) { + if self.mavlink_errors.is_empty() + && let Some(record) = kaptan_err_reportable::take() + { + let _ = self.mavlink_errors.publish_record(record); + } + let usb = &mut self.usb; + let serial = &mut self.serial; + let inventory = self.inventory; + if self.mavlink_errors.submit_next(|record| { + let payload = encode_error(record); + let mut accepted = false; + for (id, link) in mavlink_links(usb, serial, inventory) { + let frame = link.channel.encode_message(FrameClass::Critical, &payload); + accepted |= exchange(id, Some(frame)).accepted; + } + accepted + }) { + return; + } + if self.mavlink_logs.peek().is_none() { + let _ = kaptan_log::drain_one(|record| self.mavlink_logs.publish_record(*record)); + } + let _ = self.mavlink_logs.submit_next(|record| { + let class = match record.delivery() { + DeliveryClass::Critical => FrameClass::Critical, + DeliveryClass::BestEffort => FrameClass::BestEffort, + }; + let payload = encode_log(record); + let mut accepted = false; + for (id, link) in mavlink_links(usb, serial, inventory) { + let frame = link.channel.encode_message(class, &payload); + accepted |= exchange(id, Some(frame)).accepted; + } + ( + if accepted { + TransportAdmission::Accepted + } else { + TransportAdmission::Retry + }, + (), + ) + }); } } -fn configure_exti4(rcc: &stm32f722::RCC, syscfg: stm32f722::SYSCFG, exti: stm32f722::EXTI) { +pub(super) fn configure_exti4( + rcc: &stm32f722::RCC, + syscfg: stm32f722::SYSCFG, + exti: stm32f722::EXTI, +) { rcc.apb2enr().modify(|_, write| write.syscfgen().set_bit()); - syscfg.exticr2().modify(|_, write| { - // SAFETY: selector zero is GPIOA for EXTI4 on STM32F722. - unsafe { write.exti4().bits(0) } - }); + // SAFETY: selector zero is GPIOA for EXTI4 on STM32F722. + syscfg + .exticr2() + .modify(|_, write| unsafe { write.exti4().bits(0) }); exti.imr().modify(|_, write| write.im4().set_bit()); exti.rtsr().modify(|_, write| write.tr4().set_bit()); exti.ftsr().modify(|_, write| write.tr4().clear_bit()); exti.pr().write(|write| write.pr4().clear()); } - -fn configure_exti3(rcc: &stm32f722::RCC, syscfg: stm32f722::SYSCFG, exti: stm32f722::EXTI) { +pub(super) fn configure_exti3( + rcc: &stm32f722::RCC, + syscfg: stm32f722::SYSCFG, + exti: stm32f722::EXTI, +) { rcc.apb2enr().modify(|_, write| write.syscfgen().set_bit()); - syscfg.exticr1().modify(|_, write| { - // SAFETY: selector two is GPIOC for EXTI3 on STM32F722. - unsafe { write.exti3().bits(2) } - }); + // SAFETY: selector two is GPIOC for EXTI3 on STM32F722. + syscfg + .exticr1() + .modify(|_, write| unsafe { write.exti3().bits(2) }); exti.imr().modify(|_, write| write.im3().set_bit()); exti.rtsr().modify(|_, write| write.tr3().set_bit()); exti.ftsr().modify(|_, write| write.tr3().clear_bit()); exti.pr().write(|write| write.pr3().clear()); } -/// Services the MTF-01 UART route. It is a task-local resource, so it needs -/// no RTIC lock and cannot extend another resource's priority ceiling. -pub fn on_uart4_interrupt(mtf01: &mut Option) { - let Some(mtf01) = mtf01.as_mut() else { - return; - }; - match Uart4::acknowledge_receive_interrupt() { - ReceiveInterrupt::Byte(byte) => { - if let Some(report) = mtf01.on_received_byte(byte) { - if let Some(range) = report.range_snapshot() { - RANGES.publish(range); - } - if let Some(flow) = report.optical_flow_snapshot() { - OPTICAL_FLOW.publish(flow); - } - } - } - ReceiveInterrupt::Fault => { - mtf01.reset(); - kaptan_log::error!("mtf01 uart receive fault"); - } - ReceiveInterrupt::NotPending => {} - } -} - impl F722LidarState { pub fn on_i2c_event(&mut self, error: bool) { - let Some(range) = self.range.as_mut() else { - return; - }; - let event = range.on_controller_interrupt(|i2c| { - if error { - i2c.acknowledge_error_interrupt() - } else { - i2c.acknowledge_event_interrupt() - } - }); - let _ = publish_lidar_event(event); + if let Some((range, publisher)) = self.range.as_mut() { + let event = range.on_controller_interrupt(|i2c, now| { + if error { + i2c.acknowledge_error_interrupt(now) + } else { + i2c.acknowledge_event_interrupt(now) + } + }); + let _ = publish_lidar_event(publisher, event); + } } - #[kaptan_err_reportable::propagate] pub fn on_timer(&mut self) -> Result<(), StartupError> { - if let Some(range) = self.range.as_mut() { - if self.startup_ticks < LIDAR_STARTUP_TICKS { + if let Some((range, publisher)) = self.range.as_mut() { + if self.startup_ticks < 3 { self.startup_ticks += 1; - if self.startup_ticks == LIDAR_STARTUP_TICKS { + if self.startup_ticks == 3 { range.finish_startup()?; kaptan_log::info!("lidar-lite v3 initialized"); } } else { - let _ = publish_lidar_event(range.on_periodic_tick()); + let _ = publish_lidar_event(publisher, range.on_periodic_tick()); } } Ok(()) } } - -/// Advances the IMU state machine in its dedicated worker-local resource. -pub fn process_imu(imu: &mut F722Imu, events: u8) { - const START: u8 = 1 << 4; - const TRANSFER_FAULT: u8 = 1 << 2; - const TRANSFER_COMPLETE: u8 = 1 << 1; - const TIMER: u8 = 1 << 3; - const DRDY: u8 = 1 << 0; - let transfer_terminal = events & (TRANSFER_FAULT | TRANSFER_COMPLETE) != 0; - - if events & START != 0 { - let _ = accept_imu_event(imu.begin()); +pub fn process_imu( + imu: &mut impl ImuService, + publisher: &mut Producer<'_, ImuSnapshot>, + events: u8, +) { + let terminal = events & 6 != 0; + if events & 16 != 0 { + let _ = accept_imu_event(publisher, imu.begin()); } - if events & TRANSFER_FAULT != 0 { - let _ = accept_imu_event(imu.on_transfer_complete(SpiCompletion::TransferFault)); + if events & 4 != 0 { + let _ = accept_imu_event( + publisher, + imu.on_transfer_complete(SpiCompletion::TransferFault), + ); } - if events & TRANSFER_COMPLETE != 0 { - let _ = accept_imu_event(imu.on_transfer_complete(SpiCompletion::Complete)); + if events & 2 != 0 { + let _ = accept_imu_event(publisher, imu.on_transfer_complete(SpiCompletion::Complete)); } - if events & TIMER != 0 && !transfer_terminal { - let _ = accept_imu_event(imu.on_periodic_tick()); + if events & 8 != 0 && !terminal { + let _ = accept_imu_event(publisher, imu.on_periodic_tick()); } - if events & DRDY != 0 && !transfer_terminal { - let _ = accept_imu_event(imu.on_data_ready()); + if events & 1 != 0 && !terminal { + let _ = accept_imu_event(publisher, imu.on_data_ready()); } } - #[kaptan_err_reportable::propagate] -fn accept_imu_event(event: ImuEvent) -> Result<(), ErrorId> { +fn accept_imu_event( + publisher: &mut Producer<'_, ImuSnapshot>, + event: ImuEvent, +) -> Result<(), ErrorId> { match event { ImuEvent::Sample(sample) => { - SAMPLES.publish(sample); + publisher.publish(sample); } ImuEvent::Fault(error) => return Err(error), - ImuEvent::Initialized => { - kaptan_log::info!("imu initialized"); - } + ImuEvent::Initialized => kaptan_log::info!("imu initialized"), ImuEvent::Idle => {} } Ok(()) } - -impl F722MavlinkState -where - C: Clone + Config, - I: F7ImmutableConfig, -{ - /// Handles bounded DMA and timer work for the telemetry image. - pub fn process_worker_events(&mut self, events: u8) { - const TIMER: u8 = 1 << 0; - const RECEIVED: u8 = 1 << 1; - const DMA: u8 = 1 << 2; - - // Poll at least every service tick, including when endpoint input was - // backpressured by a packet the worker had not yet consumed. - let usb_received = events & TIMER != 0 && self.link.poll_usb(); - if events & RECEIVED != 0 || usb_received { - self.drain_file_frames(); - } - - if events & DMA != 0 { - let completion = take_mavlink_completion(); - self.link.on_dma_complete(completion); - if completion == kaptan_dma::TxCompletion::Complete { - self.led.toggle(); - } - } - - if events & TIMER != 0 { - self.tick_file_lease(); - if let Some(frame) = self.data_channel.on_service_tick() { - let _ = self.link.send_active(frame); - } - self.drain_one_mavlink_diagnostic(); - } - } - - pub fn receive_mavlink_interrupt(&mut self, event: ReceiveInterrupt) -> bool { - match event { - ReceiveInterrupt::Byte(byte) => self.uart_receiver.receive_byte(byte), - ReceiveInterrupt::Fault => { - self.uart_receiver.reset(); - kaptan_log::warn!("mavlink receive fault; framing reset"); - false - } - ReceiveInterrupt::NotPending => false, - } - } - - /// Acknowledges one OTG FS interrupt and admits USB MAVLink input. - pub fn on_usb_interrupt(&mut self) -> bool { - self.link.poll_usb() - } - - fn drain_file_frames(&mut self) { - while let Some((ingress, frame)) = self - .uart_receiver - .take_received() - .map(|frame| (MavlinkIngress::Uart, frame)) - .or_else(|| { - self.link - .take_usb_received() - .map(|frame| (MavlinkIngress::Usb, frame)) - }) - { - let Some(principal) = - Principal::try_new(frame.raw().system_id(), frame.raw().component_id()) - else { - continue; - }; - let Some(message) = frame.decode() else { - continue; - }; - let Ok(request) = wire::decode_request(&message) else { - continue; - }; - let Some(response) = - self.file_service - .dispatch(&mut self.config_file, principal, request) - else { - continue; - }; - let refresh_lease = match response { - FileResponse::Data(_) => true, - FileResponse::Control(control) => { - control.transfer_id != 0 - && control.status != FileStatus::Busy - && control.status != FileStatus::InvalidTransfer - && control.status != FileStatus::RequestConflict - } - }; - self.send_file_response(ingress, response); - if refresh_lease { - self.file_lease_ticks = FILE_TRANSFER_LEASE_TICKS; - } - } - } - - fn tick_file_lease(&mut self) { - if self.file_lease_ticks == 0 { - return; - } - self.file_lease_ticks -= 1; - if self.file_lease_ticks == 0 && self.file_service.expire(&mut self.config_file) { - kaptan_log::debug!("file transfer expired"); - } - } - - fn send_file_response(&mut self, ingress: MavlinkIngress, response: FileResponse) { - let message = wire::encode_response(response); - let class = kaptan_mavlink::transmitter::FrameClass::Critical; - match message { - kaptan::MavMessage::FILE_CONTROL(data) => { - let frame = self.data_channel.encode_message(class, &data); - let _ = self.link.send_to(ingress, frame); - } - kaptan::MavMessage::FILE_DATA(data) => { - let frame = self.data_channel.encode_message(class, &data); - let _ = self.link.send_to(ingress, frame); - } - _ => unreachable!("file response encoder returned another message"), - } - } - - fn drain_one_mavlink_diagnostic(&mut self) { - if self.mavlink_errors.is_empty() - && let Some(record) = kaptan_err_reportable::take() - { - let _ = self.mavlink_errors.publish_record(record); - } - if self.submit_pending_mavlink_error() { - return; - } - if self.mavlink_logs.peek().is_none() { - let mavlink_logs = &mut self.mavlink_logs; - let _ = kaptan_log::drain_one(|record| mavlink_logs.publish_record(*record)); - } - self.submit_queued_mavlink_log(); - } - - fn submit_pending_mavlink_error(&mut self) -> bool { - let (data_channel, link) = (&mut self.data_channel, &mut self.link); - self.mavlink_errors.submit_next(|record| { - let payload = encode_error(record); - let frame = data_channel - .encode_message(kaptan_mavlink::transmitter::FrameClass::Critical, &payload); - link.send_active(frame) - }) - } - - fn submit_queued_mavlink_log(&mut self) { - let (data_channel, link) = (&mut self.data_channel, &mut self.link); - let _ = self.mavlink_logs.submit_next(|record| { - let class = match record.delivery() { - DeliveryClass::Critical => kaptan_mavlink::transmitter::FrameClass::Critical, - DeliveryClass::BestEffort => kaptan_mavlink::transmitter::FrameClass::BestEffort, - }; - let payload = encode_log(record); - let frame = data_channel.encode_message(class, &payload); - let outcome = if link.send_active(frame) { - TransportAdmission::Accepted - } else { - TransportAdmission::Retry - }; - (outcome, ()) - }); - } -} - -pub(crate) fn take_mavlink_completion() -> kaptan_dma::TxCompletion { - match UART_COMPLETION.swap(0, Ordering::AcqRel) { - 1 => kaptan_dma::TxCompletion::Complete, - 2 => kaptan_dma::TxCompletion::TransferFault, - _ => kaptan_dma::TxCompletion::NotPending, - } -} - -pub fn record_mavlink_completion(completion: kaptan_dma::TxCompletion) { - let code = match completion { - kaptan_dma::TxCompletion::Complete => 1, - kaptan_dma::TxCompletion::TransferFault | kaptan_dma::TxCompletion::Halted => 2, - kaptan_dma::TxCompletion::NotPending => 0, - }; - if code != 0 { - UART_COMPLETION.store(code, Ordering::Release); - } -} - #[kaptan_err_reportable::propagate] fn publish_lidar_event( + publisher: &mut Producer<'_, RangeSnapshot>, event: LidarLiteV3ServiceEvent, ) -> Result<(), LidarError> { match event { LidarLiteV3ServiceEvent::Measurement(snapshot) => { - RANGES.publish(snapshot); + publisher.publish(snapshot); Ok(()) } LidarLiteV3ServiceEvent::Fault(LidarError::Start(I2c1Error::Busy)) => Ok(()), @@ -803,35 +733,185 @@ fn publish_lidar_event( } } +#[cfg(test)] +#[path = "../../tests/support/io_latency.rs"] +mod io_latency; + #[cfg(test)] mod tests { - use super::{I2c1Error, LidarError, LidarLiteV3ServiceEvent, publish_lidar_event}; + use super::*; + #[test] + fn usb_remains_when_uarts_are_disabled() { + let mut usb = Link::new( + 1, + 1, + DataChannelRates { + imu_hz: 20, + heartbeat_hz: 2, + range_hz: None, + optical_flow_hz: None, + }, + ) + .unwrap(); + let mut serial = [None, None]; + let inventory = SerialInventory::new(&[crate::UART1, crate::UART2]); + let mut links = mavlink_links(&mut usb, &mut serial, inventory); + assert_eq!(links.next().map(|(id, _)| id), Some(0)); + assert!(links.next().is_none()); + } + #[test] + fn declared_storage_has_no_slots_for_missing_ports() { + use core::mem::size_of; + let io_slot = size_of::>(); + let protocol_slot = size_of::>(); + for (count, io, protocol) in [ + ( + 0, + size_of::>(), + size_of::>(), + ), + ( + 1, + size_of::>(), + size_of::>(), + ), + ( + 2, + size_of::>(), + size_of::>(), + ), + ( + 3, + size_of::>(), + size_of::>(), + ), + ] { + assert_eq!(io, size_of::>() + count * io_slot); + assert_eq!( + protocol, + size_of::>() + count * protocol_slot + ); + } + assert_eq!(size_of::>(), size_of::()); + // These calls execute the real IRQ/exchange mapping without any PAC + // device. Absent and boot-disabled UARTs must never access registers. + let mut io = PortIo::<2>::new(SerialInventory::new(&[crate::UART4, crate::UART1])); + assert_eq!(io.ports.len(), 2); + for id in 0..=u8::MAX { + assert!(!io.on_interrupt(PortId::new(id), Timestamp::default())); + let batch = io.exchange(PortId::new(id), None); + assert!(!batch.pending); + assert_eq!(batch.serial.length, 0); + } + } + + #[test] + fn lease_expires_by_elapsed_time_even_when_service_ticks_coalesce() { + let mut lease = FileLease::default(); + assert!(!lease.expired(u64::MAX)); + lease.refresh(1234); + assert!(!lease.expired(10_001_233)); + assert!(lease.expired(10_001_234)); + assert!(!lease.expired(30_000_000)); + lease.refresh(30_000_000); + lease.refresh(39_000_000); + assert!(!lease.expired(40_000_000)); + assert!(lease.expired(49_000_000)); + } + + #[derive(Default)] + struct RecordingImu { + begin: usize, + ticks: usize, + ready: usize, + terminal: Option, + sample: Option, + } + impl ImuService for RecordingImu { + fn begin(&mut self) -> ImuEvent { + self.begin += 1; + ImuEvent::Idle + } + fn on_periodic_tick(&mut self) -> ImuEvent { + self.ticks += 1; + ImuEvent::Idle + } + fn on_data_ready(&mut self) -> ImuEvent { + self.ready += 1; + self.sample.take().map_or(ImuEvent::Idle, ImuEvent::Sample) + } + fn on_transfer_complete(&mut self, completion: SpiCompletion) -> ImuEvent { + self.terminal = Some(completion); + ImuEvent::Idle + } + } + #[test] + fn imu_worker_defers_new_requests_when_a_terminal_event_is_coalesced() { + let mut mailbox = LatestImuMailbox::new(); + let (mut publisher, _) = mailbox.split(); + let mut imu = RecordingImu::default(); + process_imu(&mut imu, &mut publisher, 16 | 8 | 1); + assert_eq!((imu.begin, imu.ticks, imu.ready), (1, 1, 1)); + for (event, completion) in [ + (4, SpiCompletion::TransferFault), + (2, SpiCompletion::Complete), + ] { + process_imu(&mut imu, &mut publisher, event | 8 | 1); + assert_eq!(imu.terminal, Some(completion)); + assert_eq!((imu.ticks, imu.ready), (1, 1)); + } + } #[test] fn transient_i2c_busy_is_retry_not_a_typed_lidar_fault() { - while kaptan_err_reportable::take().is_some() {} + let mut mailbox = LatestRangeMailbox::new(); + let (mut publisher, _) = mailbox.split(); assert_eq!( - publish_lidar_event(LidarLiteV3ServiceEvent::Fault(LidarError::Start( - I2c1Error::Busy, - ))), + publish_lidar_event( + &mut publisher, + LidarLiteV3ServiceEvent::Fault(LidarError::Start(I2c1Error::Busy)) + ), Ok(()) ); - assert!(kaptan_err_reportable::take().is_none()); + assert!( + publish_lidar_event( + &mut publisher, + LidarLiteV3ServiceEvent::Fault(LidarError::Start(I2c1Error::InvalidLength)) + ) + .is_err() + ); + } - assert!(matches!( - publish_lidar_event(LidarLiteV3ServiceEvent::Fault(LidarError::Start( - I2c1Error::InvalidLength, - ))), - Err(LidarError::Start(I2c1Error::InvalidLength)) - )); - assert!(kaptan_err_reportable::take().is_some()); - - assert!(matches!( - publish_lidar_event(LidarLiteV3ServiceEvent::Fault(LidarError::Start( - I2c1Error::InvalidAddress, - ))), - Err(LidarError::Start(I2c1Error::InvalidAddress)) - )); - assert!(kaptan_err_reportable::take().is_some()); + #[test] + fn worker_publications_reach_the_owned_collector() { + let mut imu_mailbox = LatestImuMailbox::new(); + let mut range_mailbox = LatestRangeMailbox::new(); + let mut flow_mailbox = LatestOpticalFlowMailbox::new(); + let (mut imu_publisher, imu) = imu_mailbox.split(); + let (mut range_publisher, range) = range_mailbox.split(); + let (_, optical_flow) = flow_mailbox.split(); + let mut collector = DataCollector::new(DataSources { + imu, + range, + optical_flow, + }); + let timestamp = Timestamp::from_microseconds(123); + let sample = ImuSnapshot::new(timestamp, [1, -2, 3], [-34_906, 0, 34_906], 2500); + let range = RangeSnapshot::new(timestamp, 456); + let mut imu = RecordingImu { + sample: Some(sample), + ..RecordingImu::default() + }; + process_imu(&mut imu, &mut imu_publisher, 1); + publish_lidar_event( + &mut range_publisher, + LidarLiteV3ServiceEvent::Measurement(range), + ) + .unwrap(); + let samples = collector.snapshot(); + assert_eq!(samples.imu, Some(sample)); + assert_eq!(samples.range, Some(range)); + assert_eq!(samples.optical_flow, None); + assert_eq!(collector.snapshot(), samples); } } diff --git a/firmware/platforms/stm32f7/src/f722/clock.rs b/firmware/platforms/stm32f7/src/f722/clock.rs index 37bf3a1..15a75ec 100644 --- a/firmware/platforms/stm32f7/src/f722/clock.rs +++ b/firmware/platforms/stm32f7/src/f722/clock.rs @@ -1,6 +1,6 @@ //! STM32F722 Pll216 clock-source assembly. -use super::peripherals::{DwtMicrosecondClock, select_pll216_profile}; +use super::peripherals::{Tim2Periodic, TimerClock, select_pll216_profile}; use kaptan_err_reportable::KaptanError; use stm32f7::stm32f722; @@ -21,11 +21,31 @@ pub(crate) enum ClockInitializationError { /// Selects the F722 Pll216 profile and starts its monotonic microsecond clock. pub(crate) fn initialize( core: &mut cortex_m::Peripherals, - device: &stm32f722::Peripherals, -) -> Result { - select_pll216_profile(&device.RCC, &device.PWR, &device.FLASH) - .map_err(|_| ClockInitializationError::Profile)?; + rcc: &stm32f722::RCC, + pwr: &stm32f722::PWR, + flash: &stm32f722::FLASH, + tim2: stm32f722::TIM2, + dbgmcu: &stm32f722::DBGMCU, +) -> Result<(TimerClock, Tim2Periodic), ClockInitializationError> { + // Even a clock-profile failure must not leave an inherited TIM2 NVIC IRQ + // enabled while RTIC resources are unpublished. + cortex_m::peripheral::NVIC::mask(stm32f722::Interrupt::TIM2); + core.SCB.clear_sleepdeep(); + select_pll216_profile(rcc, pwr, flash).map_err(|_| ClockInitializationError::Profile)?; core.SCB.disable_dcache(&mut core.CPUID); - DwtMicrosecondClock::start::(&mut core.DCB, &mut core.DWT) - .map_err(|_| ClockInitializationError::Monotonic) + + // Firmware executes through the AXI Flash mapping, outside the ART + // accelerator. Clear inherited instruction state before enabling the core + // I-cache. The helpers provide the required DSB/ISB barriers. + core.SCB.invalidate_icache(); + core.SCB.enable_icache(); + + // DWT remains solely for the existing SRAM flash-poll timeout. That path + // runs the CPU continuously at this fixed 216 MHz and never sleeps. + core.DCB.enable_trace(); + cortex_m::peripheral::DWT::unlock(); + core.DWT.set_cycle_count(0); + core.DWT.enable_cycle_counter(); + + Tim2Periodic::new(rcc, tim2, dbgmcu).ok_or(ClockInitializationError::Monotonic) } diff --git a/firmware/platforms/stm32f7/src/f722/config.rs b/firmware/platforms/stm32f7/src/f722/config.rs index 73dcd1a..b92122d 100644 --- a/firmware/platforms/stm32f7/src/f722/config.rs +++ b/firmware/platforms/stm32f7/src/f722/config.rs @@ -1,18 +1,107 @@ -//! Board-owned user configuration support and the generic F722 flash journal. +//! F722 user configuration and flash journal. use core::{marker::PhantomData, mem}; use kaptan_config::{ - Config, ConfigManager, ConfigModel, ConfigValue, ConfigViolation, DualSlotJournal, DualSlotNor, - FieldDescriptor, FieldId, FieldWriteError, JournalError, KcfgDecodeError, KcfgExportPlan, - KcfgExporter, KcfgImporter, KcfgResultDetail, ReplaceError, ReplaceOutcome, ValidationRule, - kcfg_length, + Config, ConfigManager, ConfigModel, ConfigStore, ConfigValue, ConfigViolation, DualSlotJournal, + DualSlotNor, FieldDescriptor, FieldId, FieldWriteError, JournalError, KcfgDecodeError, + KcfgExportPlan, KcfgExporter, KcfgImporter, KcfgResultDetail, KcfgUpdate, ReplaceError, + ReplaceOutcome, StartupError, ValidationRule, kcfg_length, }; use kaptan_file_mavlink::{ CONTENT_BYTES, Content, FileFailure, FileMetadata, FileProvider, FileStatus, }; use stm32f7::stm32f722; +use super::ports::{disable_ports, project_ports}; use crate::{F7ImmutableConfig, LevelFilter}; +use kaptan_ports::{PortCatalog, PortPlan, PortSettings, Ports, Role}; + +#[derive(Clone, Config)] +struct MavlinkConfig { + #[config(id = 101, key = "mavlink.system_id", unit = "id", range(min = 1))] + system_id: u8, + #[config(id = 102, key = "mavlink.component_id", unit = "id", range(min = 1))] + component_id: u8, + #[config( + enum, + id = 105, + key = "mavlink.logging.level_filter", + unit = "log_level" + )] + log_level_filter: LevelFilter, +} + +#[derive(Clone, Config)] +struct TelemetryConfig { + #[config( + id = 103, + key = "telemetry.imu_hz", + unit = "Hz", + range(min = 1, max = 100) + )] + imu_hz: u8, + #[config( + id = 104, + key = "telemetry.heartbeat_hz", + unit = "Hz", + range(min = 1, max = 10) + )] + heartbeat_hz: u8, + #[config( + id = 106, + key = "telemetry.range_hz", + unit = "Hz", + range(min = 1, max = 100) + )] + range_hz: u8, + #[config( + id = 107, + key = "telemetry.optical_flow_hz", + unit = "Hz", + range(min = 1, max = 100) + )] + optical_flow_hz: u8, +} + +/// User settings applied at boot. +#[derive(Config)] +pub struct F722UserConfig, const N: usize> { + mavlink: MavlinkConfig, + telemetry: TelemetryConfig, + ports: Ports, +} + +impl, const N: usize> Clone for F722UserConfig { + fn clone(&self) -> Self { + Self { + mavlink: self.mavlink.clone(), + telemetry: self.telemetry.clone(), + ports: self.ports, + } + } +} + +impl, const N: usize> F722UserConfig { + pub const FACTORY: Self = Self { + mavlink: MavlinkConfig { + system_id: 1, + component_id: 1, + log_level_filter: LevelFilter::Info, + }, + telemetry: TelemetryConfig { + imu_hz: 20, + heartbeat_hz: 2, + range_hz: 20, + optical_flow_hz: 20, + }, + ports: Ports::new( + [PortSettings { + role: Role::Disabled, + baud: 115_200, + }; N], + ), + }; +} const MAVLINK_SYSTEM_ID: FieldId = FieldId::new(101); const MAVLINK_COMPONENT_ID: FieldId = FieldId::new(102); @@ -22,8 +111,16 @@ const MAVLINK_LOG_LEVEL_FILTER: FieldId = FieldId::new(105); const RANGE_HZ: FieldId = FieldId::new(106); const OPTICAL_FLOW_HZ: FieldId = FieldId::new(107); const USER_CONFIG_PATH: &[u8] = b"/config/user.kcfg"; +const ACTIVE_CONFIG_PATH: &[u8] = b"/config/active.kcfg"; +const RESET_CONFIG_PATH: &[u8] = b"/config/reset.kcfg"; + +#[derive(Clone)] +pub(crate) struct BootActive { + pub(crate) candidate: C, + pub(crate) runtime: F722UserRuntime, + pub(crate) ports: PortPlan, +} -/// Narrow boot projection consumed only by fixed F722 services. #[derive(Clone, Copy, Debug, Eq, PartialEq)] pub(super) struct F722UserRuntime { pub(super) system_id: u8, @@ -35,14 +132,13 @@ pub(super) struct F722UserRuntime { pub(super) heartbeat_hz: u8, } -/// Generic [`ConfigModel`] adapter for one board-composed aggregate. #[derive(Clone)] -pub(crate) struct UserConfigModel { +pub(crate) struct UserConfigModel { candidate: C, _immutable: PhantomData, } -impl UserConfigModel { +impl UserConfigModel { pub(crate) const fn new(candidate: C) -> Self { Self { candidate, @@ -51,12 +147,12 @@ impl UserConfigModel { } } -impl ConfigModel for UserConfigModel +impl ConfigModel for UserConfigModel where C: Clone + Config, I: F7ImmutableConfig, { - type Active = C; + type Active = BootActive; type Context = I; const SCHEMA_ID: [u8; 16] = ::SCHEMA_ID; @@ -78,7 +174,7 @@ where } fn active_field_value(active: &Self::Active, id: FieldId) -> Option { - active.field_value(id) + active.candidate.field_value(id) } fn set_field(&mut self, id: FieldId, value: ConfigValue) -> Result<(), FieldWriteError> { @@ -87,8 +183,17 @@ where fn validate(&self, context: &Self::Context) -> Result { ::validate(&self.candidate)?; - let _ = project_runtime(&self.candidate, context)?; - Ok(self.candidate.clone()) + let runtime = project_runtime(&self.candidate, context)?; + let ports = project_ports( + &self.candidate, + context.f722().hardware.serial(), + scheduled_wire_bytes(runtime), + )?; + Ok(BootActive { + candidate: self.candidate.clone(), + runtime, + ports, + }) } } @@ -121,12 +226,25 @@ where let mavlink_log_level_filter = LevelFilter::try_from_wire(level_filter).ok_or( ConfigViolation::field(MAVLINK_LOG_LEVEL_FILTER, ValidationRule::InvalidEnumValue), )?; - let range_hz = if immutable.hardware.has_range() { + // Full port validation follows with this runtime's publication budget. + let mtf01 = immutable + .hardware + .serial() + .descriptors() + .iter() + .any(|port| { + candidate.field_value(port.role_field) + == Some(ConfigValue::U8(Role::Mtf01.wire_value())) + }); + if mtf01 && immutable.hardware.has_range() { + return Err(ConfigViolation::global(ValidationRule::Inconsistent)); + } + let range_hz = if immutable.hardware.has_range() || mtf01 { Some(required_u8(candidate, RANGE_HZ)?) } else { None }; - let optical_flow_hz = if immutable.hardware.has_optical_flow() { + let optical_flow_hz = if mtf01 { Some(required_u8(candidate, OPTICAL_FLOW_HZ)?) } else { None @@ -153,6 +271,16 @@ where }) } +fn scheduled_wire_bytes(runtime: F722UserRuntime) -> u32 { + use kaptan_mavlink::generated::kaptan; + // Unsigned MAVLink2 header + CRC; no minimum-payload truncation assumption. + u32::from(runtime.imu_hz) * (12 + kaptan::IMU_DATA::ENCODED_LEN as u32) + + u32::from(runtime.heartbeat_hz) * (12 + kaptan::HEARTBEAT_DATA::ENCODED_LEN as u32) + + u32::from(runtime.range_hz.unwrap_or(0)) * (12 + kaptan::RANGE_DATA::ENCODED_LEN as u32) + + u32::from(runtime.optical_flow_hz.unwrap_or(0)) + * (12 + kaptan::OPTICAL_FLOW_DATA::ENCODED_LEN as u32) +} + fn required_u8(candidate: &C, field: FieldId) -> Result { match candidate.field_value(field) { Some(ConfigValue::U8(value)) => Ok(value), @@ -192,24 +320,36 @@ const FLASH_KEY1: u32 = 0x4567_0123; const FLASH_KEY2: u32 = 0xcdef_89ab; const FLASH_ERROR_FLAGS: u32 = 0x01f2; // The F722 datasheet bounds one x32 128-KiB sector erase at 1.322 seconds. -// The already-running 216-MHz DWT counter gives every operation a two-second -// deadline without turning an instruction-count guess into a timing contract. +// Allow two seconds, measured by DWT at the fixed 216 MHz core frequency. const FLASH_OPERATION_TIMEOUT_CYCLES: u32 = 2 * 216_000_000; /// One schema-independent journal shared by every STM32F722 board image. pub(crate) type F722ConfigStore = DualSlotJournal; -/// Concrete manager storage selected when the board macro monomorphizes the -/// common platform task graph for its aggregate and exact snapshot size. -pub(crate) type F722ConfigManager = - ConfigManager, F722ConfigStore>; +pub(crate) type BootDiagnostic = kaptan_config::RecoveryDisposition; +type UserConfigManager = ConfigManager, S>; + +/// Recovers the generic manager using the platform's explicit USB-only fallback. +pub(crate) fn recover_configuration( + factory: UserConfigModel, + context: I, + store: S, +) -> Result, StartupError> +where + C: Clone + Config, + I: F7ImmutableConfig, + S: ConfigStore>, +{ + ConfigManager::recover_with(factory, context, store, |factory, context, _| { + let mut setup = factory.clone(); + disable_ports(&mut setup.candidate, context.f722().hardware.serial()); + Some(setup) + }) +} #[derive(Debug, kaptan_err_reportable::KaptanError)] enum PersistConfigurationError { - /// Reserved journal sectors cannot hold the configured payload bound. - #[kaptan_error(id = 2210)] - JournalGeometry, /// The compiled configuration model cannot produce a private snapshot. #[kaptan_error(id = 2202)] ConfigurationModel, @@ -231,6 +371,12 @@ enum PersistConfigurationError { /// A complete journal record failed post-write verification. #[kaptan_error(id = 2209)] JournalVerification, + /// Reserved journal sectors cannot hold the configured payload bound. + #[kaptan_error(id = 2210)] + JournalGeometry, + /// Unknown durability requires recovery or explicit factory reset before saving. + #[kaptan_error(id = 2211)] + DurableStateUnknown, } impl From> for PersistConfigurationError { @@ -250,30 +396,47 @@ impl From> for PersistConfigurationError { enum ConfigFileTransfer { Idle, - Read(KcfgExportPlan), - Write(KcfgImporter), + Read { + plan: KcfgExportPlan, + active: bool, + }, + Write { + importer: KcfgImporter, + reset: bool, + }, } /// Streaming KCFG endpoint over the private power-loss-safe flash journal. /// -/// The endpoint retains one typed candidate while writing and generates read -/// chunks directly from compiled metadata. It never buffers the public file. -pub(crate) struct F722ConfigFile -where +/// The endpoint retains requested and expected-base candidates while writing +/// and generates read chunks from metadata. It never buffers the public file. +/// User writes are guarded full replacements. Reset writes deliberately ignore +/// the base, but accept only exact factory values. Active is read-only. +pub(crate) struct F722ConfigFile< + C, + I, + const N: usize, + const MAX_PAYLOAD_BYTES: usize, + S = F722ConfigStore, +> where C: Clone + Config, I: F7ImmutableConfig, { - manager: F722ConfigManager, - transfer: ConfigFileTransfer>, + manager: UserConfigManager, + active_export: UserConfigModel, + transfer: ConfigFileTransfer>, } -impl F722ConfigFile +impl + F722ConfigFile where C: Clone + Config, I: F7ImmutableConfig, + S: ConfigStore, Error = JournalError>, { - pub(crate) const fn new(manager: F722ConfigManager) -> Self { + pub(crate) fn new(manager: UserConfigManager) -> Self { Self { + active_export: UserConfigModel::new(manager.active().candidate.clone()), manager, transfer: ConfigFileTransfer::Idle, } @@ -284,33 +447,75 @@ where #[kaptan_err_reportable::propagate] fn persist_candidate( &mut self, - candidate: UserConfigModel, - ) -> Result, PersistConfigurationError> { - match self.manager.replace(candidate) { + update: KcfgUpdate>, + reset: bool, + ) -> Result, PersistConfigurationError> { + let result = if reset { + if !self.manager.is_factory(&update.candidate) { + return Ok(Err(kcfg_file_failure( + FileStatus::ContentRejected, + KcfgResultDetail::failure(), + b"reset requires exact factory values", + ))); + } + self.manager.reset_to_factory() + } else { + self.manager + .replace_if_current(update.candidate, &update.expected_base) + }; + match result { Ok(outcome) => Ok(Ok(outcome)), - Err(ReplaceError::InvalidCandidate(violation)) => Ok(Err(violation)), + Err(ReplaceError::InvalidCandidate(violation)) => Ok(Err(kcfg_file_failure( + FileStatus::ContentRejected, + KcfgResultDetail::violation(violation), + b"configuration validation failed", + ))), + Err(ReplaceError::Conflict) => Ok(Err(kcfg_file_failure( + FileStatus::RequestConflict, + KcfgResultDetail::failure(), + b"configuration changed; reload before saving", + ))), Err(ReplaceError::Store(error)) => Err(error.into()), + Err(ReplaceError::DurableStateUnknown) => { + Err(PersistConfigurationError::DurableStateUnknown) + } } } + + fn committed(&self) -> Result<&UserConfigModel, FileFailure> { + self.manager.committed().ok_or_else(|| { + file_failure( + FileStatus::StorageFailure, + b"configuration durable state unknown", + ) + }) + } } -impl FileProvider for F722ConfigFile +impl FileProvider + for F722ConfigFile where C: Clone + Config, I: F7ImmutableConfig, + S: ConfigStore, Error = JournalError>, { fn open_read( &mut self, path: &kaptan_file_mavlink::FilePath, ) -> Result { - if path.as_bytes() != USER_CONFIG_PATH { + let active = path.as_bytes() == ACTIVE_CONFIG_PATH; + if !active && path.as_bytes() != USER_CONFIG_PATH { return Err(file_failure(FileStatus::NotFound, b"file not found")); } if !matches!(self.transfer, ConfigFileTransfer::Idle) { return Err(file_failure(FileStatus::Busy, b"file transfer busy")); } let exporter = KcfgExporter::new( - self.manager.committed(), + if active { + &self.active_export + } else { + self.committed()? + }, self.manager.factory(), self.manager.restart_required(), ) @@ -320,7 +525,10 @@ where .map_err(|_| file_failure(FileStatus::TooLarge, b"configuration file too large"))?, checksum_crc32c: exporter.checksum_crc32c(), }; - self.transfer = ConfigFileTransfer::Read(exporter.plan()); + self.transfer = ConfigFileTransfer::Read { + plan: exporter.plan(), + active, + }; Ok(metadata) } @@ -329,13 +537,17 @@ where path: &kaptan_file_mavlink::FilePath, metadata: FileMetadata, ) -> Result<(), FileFailure> { - if path.as_bytes() != USER_CONFIG_PATH { + let reset = path.as_bytes() == RESET_CONFIG_PATH; + if !reset && path.as_bytes() != USER_CONFIG_PATH { return Err(file_failure(FileStatus::NotFound, b"file not found")); } if !matches!(self.transfer, ConfigFileTransfer::Idle) { return Err(file_failure(FileStatus::Busy, b"file transfer busy")); } - let expected = kcfg_length::>() + if !reset { + self.committed()?; + } + let expected = kcfg_length::>() .map_err(|_| file_failure(FileStatus::Internal, b"cannot import configuration"))?; if usize::try_from(metadata.size) != Ok(expected) { return Err(kcfg_file_failure( @@ -346,19 +558,23 @@ where } let importer = KcfgImporter::new(self.manager.factory()) .map_err(|_| file_failure(FileStatus::Internal, b"cannot import configuration"))?; - self.transfer = ConfigFileTransfer::Write(importer); + self.transfer = ConfigFileTransfer::Write { importer, reset }; Ok(()) } fn read(&mut self, position: u32, output: &mut [u8]) -> Result<(), FileFailure> { - let ConfigFileTransfer::Read(plan) = &self.transfer else { + let ConfigFileTransfer::Read { plan, active } = &self.transfer else { return Err(file_failure( FileStatus::InvalidState, b"file is not open for read", )); }; let exporter = KcfgExporter::from_plan_unchecked( - self.manager.committed(), + if *active { + &self.active_export + } else { + self.committed()? + }, self.manager.factory(), *plan, ); @@ -375,7 +591,7 @@ where } fn write(&mut self, _: u32, data: &[u8]) -> Result<(), FileFailure> { - let ConfigFileTransfer::Write(importer) = &mut self.transfer else { + let ConfigFileTransfer::Write { importer, .. } = &mut self.transfer else { return Err(file_failure( FileStatus::InvalidState, b"file is not open for write", @@ -387,25 +603,22 @@ where } fn close(&mut self) -> Result { - match mem::replace(&mut self.transfer, ConfigFileTransfer::Idle) { + let transfer = mem::replace(&mut self.transfer, ConfigFileTransfer::Idle); + match transfer { ConfigFileTransfer::Idle => { Err(file_failure(FileStatus::InvalidState, b"file is not open")) } - ConfigFileTransfer::Read(_) => Ok(Content::EMPTY), - ConfigFileTransfer::Write(importer) => { - let candidate = importer.finish().map_err(kcfg_failure)?; - let outcome = self - .persist_candidate(candidate) - .map_err(|_| { - file_failure(FileStatus::StorageFailure, b"configuration storage failed") - })? - .map_err(|violation| { - kcfg_file_failure( - FileStatus::ContentRejected, - KcfgResultDetail::violation(violation), - b"configuration validation failed", - ) - })?; + ConfigFileTransfer::Read { active, .. } => { + if !active { + self.committed()?; + } + Ok(Content::EMPTY) + } + ConfigFileTransfer::Write { importer, reset } => { + let update = importer.finish().map_err(kcfg_failure)?; + let outcome = self.persist_candidate(update, reset).map_err(|_| { + file_failure(FileStatus::StorageFailure, b"configuration storage failed") + })??; Ok(kcfg_content( KcfgResultDetail::success(outcome.restart_required), b"", diff --git a/firmware/platforms/stm32f7/src/f722/data_channel.rs b/firmware/platforms/stm32f7/src/f722/data_channel.rs index 8eea44f..204fd02 100644 --- a/firmware/platforms/stm32f7/src/f722/data_channel.rs +++ b/firmware/platforms/stm32f7/src/f722/data_channel.rs @@ -1,162 +1,212 @@ -//! F722 transport and vertical-route adapters for the portable data channel. - -use super::peripherals; -use kaptan_dma::TxCompletion; -use kaptan_mavlink::{ - MavlinkFrame, receiver::IntegrityCheckedFrame, transmitter::MavlinkTransmitter, +//! Independent per-link scheduling, encoding, framing and bounded transmission. +use super::{ + peripherals::{self, F722UsbMavlink, OwnedUart, ReceiveInterrupt}, + serial::{ReceiveEvent, RxChunk, SerialDevice, SerialIo}, }; +use crate::SerialInventory; +use kaptan_data_channel::{DataChannel, DataChannelRates}; +use kaptan_mavlink::{ + MavlinkFrame, + receiver::{IntegrityCheckedFrame, IntegrityMavlinkReceiver}, +}; +use kaptan_ports::PortId; use kaptan_tx::PendingFrame; -use peripherals::F722UsbMavlink; -// Baud, sensor topology, and mounting remain immutable build-time policy. The -// output-inhibited board image may recover endpoint identity and selected -// operational rates before this channel is constructed. -pub(crate) const UART1_CLOCK_HZ: u32 = 108_000_000; -pub(crate) const UART2_CLOCK_HZ: u32 = 54_000_000; pub(crate) const SERVICE_HZ: usize = 100; -/// Fixed service-timer configuration for the validated Pll216 profile. -pub(crate) const SERVICE_TIMER_CONFIG: peripherals::Tim2PeriodicConfig = - peripherals::Tim2PeriodicConfig { - prescaler: 10_799, - auto_reload: 99, - }; - -/// The two F722 DMA engines behind the portable data-channel transport. -enum F722MavlinkTransport { - Usart1(MavlinkTransmitter), - Usart2(MavlinkTransmitter), +pub(crate) struct Link { + pub(crate) channel: DataChannel, + pub(crate) receiver: IntegrityMavlinkReceiver<2>, +} +impl Link { + pub(crate) fn new( + system: u8, + component: u8, + rates: DataChannelRates, + ) -> Result { + Ok(Self { + channel: DataChannel::new(system, component, rates, 100_000) + .map_err(|_| crate::BoardError::Initialization)?, + receiver: IntegrityMavlinkReceiver::new(), + }) + } } -/// Physical link on which an inbound frame arrived. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub(crate) enum MavlinkIngress { - Uart, - Usb, +impl SerialDevice for OwnedUart { + fn receive(&mut self) -> ReceiveEvent { + match OwnedUart::receive(self) { + ReceiveInterrupt::Byte(byte) => ReceiveEvent::Byte(byte), + ReceiveInterrupt::Fault => ReceiveEvent::Fault, + ReceiveInterrupt::NotPending => ReceiveEvent::Idle, + } + } } -/// Concrete F722 MAVLink link ownership. -pub(crate) struct F722MavlinkLink { - uart: F722MavlinkTransport, +/// Bounded data exchanged between the worker and I/O owners. Decoding, +/// scheduling and persistence cannot borrow the physical owners through this API. +#[derive(Default)] +pub struct IoBatch { + pub(crate) serial: RxChunk, + pub(crate) usb: Option, + pub(crate) accepted: bool, + pub(crate) pending: bool, +} +impl SerialIo { + pub fn exchange(&mut self, frame: Option>) -> IoBatch { + match frame { + Some(frame) => IoBatch { + accepted: self.send(frame), + ..IoBatch::default() + }, + None => { + let serial = self.take_chunk(); + IoBatch { + pending: serial.pending, + serial, + ..IoBatch::default() + } + } + } + } +} + +/// One short-lock RTIC resource for the board's declared UARTs. All UART IRQs +/// have priority 14. Protocol work stays outside this bounded I/O handoff. +pub struct PortIo { + inventory: SerialInventory, + pub(crate) ports: [SerialIo; N], +} +impl PortIo { + pub(crate) fn new(inventory: SerialInventory) -> Self { + assert_eq!(inventory.len(), N, "validated board port count"); + Self { + inventory, + ports: core::array::from_fn(|_| SerialIo::new(None, false)), + } + } + pub fn on_interrupt(&mut self, id: PortId, now: kaptan_time::Timestamp) -> bool { + self.inventory + .index(id) + .is_some_and(|index| self.ports[index].on_interrupt(now)) + } + pub fn exchange(&mut self, id: PortId, frame: Option>) -> IoBatch { + self.inventory + .index(id) + .map_or_else(IoBatch::default, |index| self.ports[index].exchange(frame)) + } +} + +pub struct UsbIo { usb: F722UsbMavlink, + rx_interrupt: RxFifoInterruptGate, +} +impl UsbIo { + pub(crate) fn new(usb: F722UsbMavlink, rx_interrupt: RxFifoInterruptGate) -> Self { + Self { usb, rx_interrupt } + } + pub fn on_interrupt(&mut self) -> bool { + self.usb.with_rx_interrupt_gate( + |usb| usb.poll(), + |enabled| self.rx_interrupt.set_enabled(enabled), + ) + } + pub fn exchange(&mut self, frame: Option>) -> IoBatch { + exchange_usb(&mut self.usb, frame, |enabled| { + self.rx_interrupt.set_enabled(enabled) + }) + } } -impl F722MavlinkLink { - pub(crate) fn usart1( - rcc: &stm32f7::stm32f722::RCC, - uart: stm32f7::stm32f722::USART1, - baud: u32, - usb: F722UsbMavlink, - ) -> Result { - let uart = peripherals::Usart1Tx::new(rcc, uart, UART1_CLOCK_HZ, baud).map_err(|_| ())?; - // SAFETY: platform initialization disables D-cache before this object - // is created, and the active MAVLink frame remains queue-owned until - // this platform receives a terminal DMA completion notification. - let memory = unsafe { peripherals::DcacheDisabledDmaMemory::new() }; - Ok(Self { - uart: F722MavlinkTransport::Usart1(MavlinkTransmitter::new( - memory, - peripherals::Usart1TxDma::new(rcc, uart), - )), - usb, - }) +/// Private F722 capability delegated alongside the UsbPeripheral in its factory. +/// The USB I/O owner retains it at runtime. No register-block reference is made. +pub(crate) struct RxFifoInterruptGate { + _private: (), +} +impl RxFifoInterruptGate { + pub(crate) fn new(_global: &stm32f7::stm32f722::OTG_FS_GLOBAL) -> Self { + Self { _private: () } } - pub(crate) fn usart2( - rcc: &stm32f7::stm32f722::RCC, - uart: stm32f7::stm32f722::USART2, - baud: u32, - usb: F722UsbMavlink, - ) -> Result { - let uart = peripherals::Usart2Tx::new(rcc, uart, UART2_CLOCK_HZ, baud).map_err(|_| ())?; - // SAFETY: identical DMA-memory invariant to the USART1 route above; - // only the fixed platform DMA endpoint differs. - let memory = unsafe { peripherals::DcacheDisabledDmaMemory::new() }; - Ok(Self { - uart: F722MavlinkTransport::Usart2(MavlinkTransmitter::new( - memory, - peripherals::Usart2TxDma::new(rcc, uart), - )), - usb, - }) + /// Call after bus enable in init, or inside the USB RTIC resource lock. + /// Defers RXFLVL only. IN, reset and suspend can still interrupt. EP0 shares + /// the RX FIFO and cannot bypass a bulk packet blocked at its head. + pub(crate) fn set_enabled(&mut self, enabled: bool) { + // RM0431: OTG_FS_GINTMSK is an aligned, 32-bit RW register at +0x18. + // Form only its raw address, without borrowing the driver's register block. + let gintmsk = (stm32f7::stm32f722::OTG_FS_GLOBAL::ptr() as usize + 0x18) as *mut u32; + // SAFETY: the factory consumes the PAC tokens and enables the bus before + // use. This single-core device keeps its USB clock on. UsbIo's RTIC lock + // excludes every runtime USB caller through this RMW. The pinned 0.4.0 + // driver writes GINTMSK only in enable(), before this capability is used. + unsafe { set_rx_fifo_interrupt_enabled(gintmsk, enabled) }; + // Complete the device write before unlocking or returning from the IRQ. + cortex_m::asm::dsb(); } +} - /// Sends publications on the open USB link, or on UART when USB is closed. - pub(crate) fn send_active(&mut self, pending: PendingFrame) -> bool { - if self.usb.is_open() { - self.usb.send(pending) +/// # Safety +/// `gintmsk` must permit aligned 32-bit volatile reads/writes, with no concurrent +/// writer. The caller must complete the device write before releasing exclusion. +unsafe fn set_rx_fifo_interrupt_enabled(gintmsk: *mut u32, enabled: bool) { + const RXFLVLM: u32 = 1 << 4; + // SAFETY: the caller provides exclusive access to this RW register. GINTMSK + // has no read/clear or write-one-to-clear fields. Preserve all other bits. + unsafe { + let mask = gintmsk.read_volatile(); + gintmsk.write_volatile(if enabled { + mask | RXFLVLM } else { - self.send_uart(pending) - } - } - - /// Sends a protocol reply only on the link that supplied its request. - pub(crate) fn send_to( - &mut self, - ingress: MavlinkIngress, - pending: PendingFrame, - ) -> bool { - match ingress { - MavlinkIngress::Uart => self.send_uart(pending), - MavlinkIngress::Usb => self.usb.send(pending), - } - } - - fn send_uart(&mut self, pending: PendingFrame) -> bool { - match &mut self.uart { - F722MavlinkTransport::Usart1(transmitter) => transmitter - .admit_and_start(pending.class, pending.frame) - .admission - .is_retained(), - F722MavlinkTransport::Usart2(transmitter) => transmitter - .admit_and_start(pending.class, pending.frame) - .admission - .is_retained(), - } - } - - pub(crate) fn on_dma_complete(&mut self, completion: TxCompletion) { - match &mut self.uart { - F722MavlinkTransport::Usart1(transmitter) => { - let _ = transmitter.on_dma_complete(completion); - } - F722MavlinkTransport::Usart2(transmitter) => { - let _ = transmitter.on_dma_complete(completion); - } - } - } - - /// Services USB and reports whether the protocol worker has bytes to parse. - pub(crate) fn poll_usb(&mut self) -> bool { - self.usb.poll() - } - - pub(crate) fn take_usb_received(&mut self) -> Option { - self.usb.take_received() + mask & !RXFLVLM + }); } } -/// Board-route status light; it is not part of a board declaration API. +pub(crate) fn exchange_usb( + usb: &mut super::usb::UsbMavlink<'_, B>, + frame: Option>, + set_rx_interrupt_enabled: impl FnOnce(bool), +) -> IoBatch { + usb.with_rx_interrupt_gate( + |usb| match frame { + Some(frame) => IoBatch { + accepted: usb.send(frame), + ..IoBatch::default() + }, + None => { + usb.poll(); + // The existing USB adapter consumes at most one 64-byte packet + // here, including bounded framing. Message decode/dispatch stays + // worker-local. Bus polling has separate hardware wait constraints. + let frame = usb.take_received(); + let pending = usb.poll(); + IoBatch { + usb: frame, + pending, + ..IoBatch::default() + } + } + }, + set_rx_interrupt_enabled, + ) +} + pub(crate) enum F722StatusLed { - Kakute(peripherals::ActiveLowLedPd2), - Geprc(GeprcStatusLed), + Pd2(peripherals::ActiveLowLedPd2), + Pa13(GeprcStatusLed), } - impl F722StatusLed { pub(crate) fn toggle(&mut self) { match self { - Self::Kakute(led) => led.toggle(), - Self::Geprc(led) => led.toggle(), + Self::Pd2(led) => led.toggle(), + Self::Pa13(led) => led.toggle(), } } } -/// GEPRC GEP-F722-HD V2 blue status LED on PA13. pub(crate) struct GeprcStatusLed { port: stm32f7::stm32f722::GPIOA, is_high: bool, } - impl GeprcStatusLed { pub(crate) fn new(rcc: &stm32f7::stm32f722::RCC, port: stm32f7::stm32f722::GPIOA) -> Self { rcc.ahb1enr().modify(|_, write| write.gpioaen().set_bit()); @@ -171,7 +221,6 @@ impl GeprcStatusLed { is_high: false, } } - fn toggle(&mut self) { self.is_high = !self.is_high; self.port.bsrr().write(|write| { @@ -183,3 +232,7 @@ impl GeprcStatusLed { }); } } + +#[cfg(test)] +#[path = "../../tests/support/usb_irq.rs"] +mod usb_irq; diff --git a/firmware/platforms/stm32f7/src/f722/firmware.rs b/firmware/platforms/stm32f7/src/f722/firmware.rs index 9e30917..83fca25 100644 --- a/firmware/platforms/stm32f7/src/f722/firmware.rs +++ b/firmware/platforms/stm32f7/src/f722/firmware.rs @@ -1,8 +1,6 @@ //! The one platform-owned RTIC task graph for all supported F722 profiles. -/// Defines the F722 application in the platform crate. All interrupt bindings -/// are the finite platform-owned superset. Profile activation determines which -/// acknowledged route has initialized hardware. +/// Defines the F722 application with handlers selected by the board's route. #[doc(hidden)] #[macro_export] macro_rules! __f722_flight_application { @@ -12,10 +10,8 @@ macro_rules! __f722_flight_application { #[allow(unused_imports)] use $crate::__platform_internal::panic as _; - // RTIC 2.3 expands hardware-vector shims that call unsafe PAC token - // constructors inside generated unsafe functions. Those shims are - // generated by RTIC, not hand-written platform accesses; all platform - // unsafe register operations remain documented at their source. + // RTIC 2.3 generates unsafe vector functions that call PAC token + // constructors without explicit unsafe blocks. #[allow(unsafe_op_in_unsafe_fn)] #[$crate::__platform_internal::rtic_app::app( device = $crate::__platform_internal::pac::stm32f722, @@ -23,36 +19,34 @@ macro_rules! __f722_flight_application { dispatchers = [TIM6_DAC, TIM7] )] mod kaptan_f722_platform_app { - use core::sync::atomic::{AtomicBool, AtomicU8, Ordering}; - use $crate::f722::{ - F722Imu, F722ImuTransferCompletion, F722InitResources, F722LidarState, - F722MavlinkState, F722Mtf01, F722Mtf01Uart, F722ServiceTimer, - acknowledge_exti3, acknowledge_exti4, - acknowledge_imu_rx_dma1, acknowledge_imu_rx_dma2, acknowledge_imu_spi1_error, - acknowledge_imu_spi3_error, acknowledge_imu_tx_dma1, acknowledge_imu_tx_dma2, - acknowledge_mavlink_dma1, acknowledge_mavlink_dma2, acknowledge_mavlink_rx_usart1, - acknowledge_mavlink_rx_usart2, initialize, on_uart4_interrupt, process_imu, - record_mavlink_completion, + F722Route, F722Interrupts, F722ImuTransferCompletion, F722InitResources, F722LidarState, + F722MavlinkState, F722ServiceTimer, EventNotification, + initialize, process_imu, }; #[allow(unused_imports)] use $crate::__platform_internal::rtic_app::Mutex; + use $crate::kaptan_ports::PortId; type BoardUserConfig = <$board as $crate::F7Board>::UserConfig; type BoardImmutableConfig = <$board as $crate::F7Board>::ImmutableConfig; + type BoardRoute = ::Route; + type BoardImu = ::Imu; + const PORT_COUNT: usize = <$board as $crate::F7Board>::IMMUTABLE_CONFIG.hardware.serial().len(); const CONFIG_PAYLOAD_BYTES: usize = $crate::config_snapshot_length::< BoardUserConfig, >(); type BoardInitResources = F722InitResources< BoardUserConfig, BoardImmutableConfig, + PORT_COUNT, CONFIG_PAYLOAD_BYTES, >; - type BoardMavlinkState = F722MavlinkState< - BoardUserConfig, - BoardImmutableConfig, - CONFIG_PAYLOAD_BYTES, - >; + type BoardMavlinkState = ::Protocol; + type BoardPorts = ::Ports; + type BoardUsb = ::Usb; + type BoardClock = ::Clock; + type BoardImuPublisher = ::ImuPublisher; const IMU_EVENT_DRDY: u8 = 1 << 0; const IMU_EVENT_TRANSFER_COMPLETE: u8 = 1 << 1; @@ -61,41 +55,37 @@ macro_rules! __f722_flight_application { const IMU_EVENT_START: u8 = 1 << 4; const MAVLINK_EVENT_TIMER: u8 = 1 << 0; const MAVLINK_EVENT_RECEIVED: u8 = 1 << 1; - const MAVLINK_EVENT_DMA: u8 = 1 << 2; - - // Hardware IRQs and both software workers coalesce event bits. - // Release/Acquire pairs make each terminal acknowledgement visible - // to the worker that consumes it; the queued bit prevents unbounded - // software-task backlog while preserving the latest pending work. - static IMU_EVENTS: AtomicU8 = AtomicU8::new(0); - static MAVLINK_EVENTS: AtomicU8 = AtomicU8::new(0); - static IMU_QUEUED: AtomicBool = AtomicBool::new(false); - static MAVLINK_QUEUED: AtomicBool = AtomicBool::new(false); + static IMU_EVENTS: EventNotification = EventNotification::new(); + static MAVLINK_EVENTS: EventNotification = EventNotification::new(); #[shared] struct Shared { - mavlink: BoardMavlinkState, + uarts: BoardPorts, + usb: BoardUsb, + serial_clock: BoardClock, lidar: F722LidarState, } #[local] struct Local { - imu: F722Imu, - mtf01: Option, - _mtf01_uart: Option, + imu: BoardImu, + imu_publisher: BoardImuPublisher, timer: F722ServiceTimer, + mavlink: BoardMavlinkState, } #[init] fn init(context: init::Context) -> (Shared, Local) { let BoardInitResources { imu, - mtf01, - mtf01_uart, - timer, + imu_publisher, + mut timer, lidar, mavlink, - } = initialize::( + uarts, + usb, + serial_clock, + } = initialize::( context.core, context.device, <$board as $crate::F7Board>::FACTORY_USER_CONFIG, @@ -103,17 +93,31 @@ macro_rules! __f722_flight_application { ) .unwrap_or_else(|_| safe_idle()); $crate::__platform_internal::logging::info!("flight controller initialized"); + imu_worker::spawn().expect("IMU worker starts once"); + mavlink_worker::spawn().expect("MAVLink worker starts once"); schedule_imu(IMU_EVENT_START); + // All fallible construction has succeeded. RTIC keeps IRQs + // masked until the returned resources have been published. + timer.enable_interrupt(); + // SAFETY: RTIC 2.3 sets PRIMASK before init and releases it only + // after publishing Shared and Local. All fallible work is done, + // the vector's priority is configured, and TIM2 is initialized. + // Restore the NVIC enable masked by clock initialization. + unsafe { + $crate::__platform_internal::cm::peripheral::NVIC::unmask( + $crate::__platform_internal::pac::stm32f722::Interrupt::TIM2, + ); + } ( Shared { - mavlink, + uarts, usb, serial_clock, lidar, }, Local { imu, - mtf01, - _mtf01_uart: mtf01_uart, + imu_publisher, timer, + mavlink, }, ) } @@ -127,21 +131,21 @@ macro_rules! __f722_flight_application { #[task(binds = EXTI4, priority = 15)] fn exti4(_: exti4::Context) { - if acknowledge_exti4() { + if BoardRoute::acknowledge_exti4() { schedule_imu(IMU_EVENT_DRDY); } } #[task(binds = EXTI3, priority = 15)] fn exti3(_: exti3::Context) { - if acknowledge_exti3() { + if BoardRoute::acknowledge_exti3() { schedule_imu(IMU_EVENT_DRDY); } } #[task(binds = DMA2_STREAM0, priority = 14)] fn imu_rx_dma2(_: imu_rx_dma2::Context) { - match acknowledge_imu_rx_dma2() { + match BoardRoute::acknowledge_imu_rx_dma2() { F722ImuTransferCompletion::Complete => schedule_imu(IMU_EVENT_TRANSFER_COMPLETE), F722ImuTransferCompletion::TransferFault => schedule_imu(IMU_EVENT_TRANSFER_FAULT), F722ImuTransferCompletion::NotPending => {} @@ -150,21 +154,21 @@ macro_rules! __f722_flight_application { #[task(binds = DMA2_STREAM3, priority = 14)] fn imu_tx_dma2(_: imu_tx_dma2::Context) { - if acknowledge_imu_tx_dma2() == F722ImuTransferCompletion::TransferFault { + if BoardRoute::acknowledge_imu_tx_dma2() == F722ImuTransferCompletion::TransferFault { schedule_imu(IMU_EVENT_TRANSFER_FAULT); } } #[task(binds = SPI1, priority = 14)] fn imu_spi1_error(_: imu_spi1_error::Context) { - if acknowledge_imu_spi1_error() { + if BoardRoute::acknowledge_imu_spi1_error() { schedule_imu(IMU_EVENT_TRANSFER_FAULT); } } #[task(binds = DMA1_STREAM0, priority = 14)] fn imu_rx_dma1(_: imu_rx_dma1::Context) { - match acknowledge_imu_rx_dma1() { + match BoardRoute::acknowledge_imu_rx_dma1() { F722ImuTransferCompletion::Complete => schedule_imu(IMU_EVENT_TRANSFER_COMPLETE), F722ImuTransferCompletion::TransferFault => schedule_imu(IMU_EVENT_TRANSFER_FAULT), F722ImuTransferCompletion::NotPending => {} @@ -173,21 +177,40 @@ macro_rules! __f722_flight_application { #[task(binds = DMA1_STREAM5, priority = 14)] fn imu_tx_dma1(_: imu_tx_dma1::Context) { - if acknowledge_imu_tx_dma1() == F722ImuTransferCompletion::TransferFault { + if BoardRoute::acknowledge_imu_tx_dma1() == F722ImuTransferCompletion::TransferFault { schedule_imu(IMU_EVENT_TRANSFER_FAULT); } } #[task(binds = SPI3, priority = 14)] fn imu_spi3_error(_: imu_spi3_error::Context) { - if acknowledge_imu_spi3_error() { + if BoardRoute::acknowledge_imu_spi3_error() { schedule_imu(IMU_EVENT_TRANSFER_FAULT); } } - #[task(binds = UART4, priority = 14, local = [mtf01])] - fn uart4(context: uart4::Context) { - on_uart4_interrupt(context.local.mtf01); + #[task(binds = USART3, priority = 14, shared = [uarts, serial_clock])] + fn usart3(mut context: usart3::Context) { + let now = context.shared.serial_clock.lock(|clock| clock.now()); + if context.shared.uarts.lock(|uarts| uarts.on_interrupt(PortId::new(3), now)) { + schedule_mavlink(MAVLINK_EVENT_RECEIVED); + } + } + + #[task(binds = UART4, priority = 14, shared = [uarts, serial_clock])] + fn uart4(mut context: uart4::Context) { + let now = context.shared.serial_clock.lock(|clock| clock.now()); + if context.shared.uarts.lock(|uarts| uarts.on_interrupt(PortId::new(4), now)) { + schedule_mavlink(MAVLINK_EVENT_RECEIVED); + } + } + + #[task(binds = USART6, priority = 14, shared = [uarts, serial_clock])] + fn usart6(mut context: usart6::Context) { + let now = context.shared.serial_clock.lock(|clock| clock.now()); + if context.shared.uarts.lock(|uarts| uarts.on_interrupt(PortId::new(6), now)) { + schedule_mavlink(MAVLINK_EVENT_RECEIVED); + } } #[task(binds = I2C1_EV, priority = 11, shared = [lidar])] @@ -206,109 +229,82 @@ macro_rules! __f722_flight_application { .lock(|lidar| lidar.on_i2c_event(true)); } - #[task(binds = DMA2_STREAM7, priority = 10)] - fn mavlink_dma2(_: mavlink_dma2::Context) { - let completion = acknowledge_mavlink_dma2(); - if completion != $crate::__platform_internal::dma::TxCompletion::NotPending { - record_mavlink_completion(completion); - schedule_mavlink(MAVLINK_EVENT_DMA); - } - } - - #[task(binds = DMA1_STREAM6, priority = 10)] - fn mavlink_dma1(_: mavlink_dma1::Context) { - let completion = acknowledge_mavlink_dma1(); - if completion != $crate::__platform_internal::dma::TxCompletion::NotPending { - record_mavlink_completion(completion); - schedule_mavlink(MAVLINK_EVENT_DMA); - } - } - - #[task(binds = USART1, priority = 8, shared = [mavlink])] + #[task(binds = USART1, priority = 14, shared = [uarts, serial_clock])] fn mavlink_rx_usart1(mut context: mavlink_rx_usart1::Context) { - let event = acknowledge_mavlink_rx_usart1(); + let now = context.shared.serial_clock.lock(|clock| clock.now()); if context .shared - .mavlink - .lock(|mavlink| mavlink.receive_mavlink_interrupt(event)) + .uarts + .lock(|uarts| uarts.on_interrupt(PortId::new(1), now)) { schedule_mavlink(MAVLINK_EVENT_RECEIVED); } } - #[task(binds = USART2, priority = 8, shared = [mavlink])] + #[task(binds = USART2, priority = 14, shared = [uarts, serial_clock])] fn mavlink_rx_usart2(mut context: mavlink_rx_usart2::Context) { - let event = acknowledge_mavlink_rx_usart2(); + let now = context.shared.serial_clock.lock(|clock| clock.now()); if context .shared - .mavlink - .lock(|mavlink| mavlink.receive_mavlink_interrupt(event)) + .uarts + .lock(|uarts| uarts.on_interrupt(PortId::new(2), now)) { schedule_mavlink(MAVLINK_EVENT_RECEIVED); } } - #[task(binds = OTG_FS, priority = 8, shared = [mavlink])] + #[task(binds = OTG_FS, priority = 8, shared = [usb])] fn mavlink_usb(mut context: mavlink_usb::Context) { + // on_interrupt gates only RXFLVLM inside this lock before returning. + // Buffered RX must wake its consumer while other USB sources stay enabled. if context .shared - .mavlink - .lock(F722MavlinkState::on_usb_interrupt) + .usb + .lock(|usb| usb.on_interrupt()) { schedule_mavlink(MAVLINK_EVENT_RECEIVED); } } - #[task(binds = TIM2, priority = 4, local = [timer], shared = [lidar])] + #[task(binds = TIM2, priority = 10, local = [timer], shared = [lidar])] fn tim2(mut context: tim2::Context) { - context.local.timer.acknowledge_update(); + if !context.local.timer.on_interrupt() { + return; + } let _ = context.shared.lidar.lock(F722LidarState::on_timer); schedule_imu(IMU_EVENT_TIMER); schedule_mavlink(MAVLINK_EVENT_TIMER); } - #[task(priority = 13, local = [imu])] + #[task(priority = 13, local = [imu, imu_publisher])] async fn imu_worker(context: imu_worker::Context) { - let events = IMU_EVENTS.swap(0, Ordering::AcqRel); - process_imu(context.local.imu, events); - IMU_QUEUED.store(false, Ordering::Release); - if IMU_EVENTS.load(Ordering::Acquire) != 0 - && !IMU_QUEUED.swap(true, Ordering::AcqRel) - && imu_worker::spawn().is_err() - { - IMU_QUEUED.store(false, Ordering::Release); + loop { + let events = IMU_EVENTS.wait().await; + process_imu(context.local.imu, context.local.imu_publisher, events); } } - #[task(priority = 9, shared = [mavlink])] + #[task(priority = 7, local = [mavlink], shared = [uarts, usb, serial_clock])] async fn mavlink_worker(mut context: mavlink_worker::Context) { - let events = MAVLINK_EVENTS.swap(0, Ordering::AcqRel); - context - .shared - .mavlink - .lock(|mavlink| mavlink.process_worker_events(events)); - MAVLINK_QUEUED.store(false, Ordering::Release); - if MAVLINK_EVENTS.load(Ordering::Acquire) != 0 - && !MAVLINK_QUEUED.swap(true, Ordering::AcqRel) - && mavlink_worker::spawn().is_err() - { - MAVLINK_QUEUED.store(false, Ordering::Release); + loop { + let events = MAVLINK_EVENTS.wait().await; + let pending = context.local.mavlink.process_worker_events(events, + |id, frame| match id { + 0 => context.shared.usb.lock(|usb| usb.exchange(frame)), + _ => context.shared.uarts.lock(|uarts| uarts.exchange(PortId::new(id), frame)), + }, + || context.shared.serial_clock.lock(|clock| clock.now()), + ); + if pending { schedule_mavlink(MAVLINK_EVENT_RECEIVED); } } } fn schedule_imu(event: u8) { - IMU_EVENTS.fetch_or(event, Ordering::Release); - if !IMU_QUEUED.swap(true, Ordering::AcqRel) && imu_worker::spawn().is_err() { - IMU_QUEUED.store(false, Ordering::Release); - } + IMU_EVENTS.notify(event); } fn schedule_mavlink(event: u8) { - MAVLINK_EVENTS.fetch_or(event, Ordering::Release); - if !MAVLINK_QUEUED.swap(true, Ordering::AcqRel) && mavlink_worker::spawn().is_err() - { - MAVLINK_QUEUED.store(false, Ordering::Release); - } + MAVLINK_EVENTS.notify(event); } fn safe_idle() -> ! { diff --git a/firmware/platforms/stm32f7/src/f722/interrupts.rs b/firmware/platforms/stm32f7/src/f722/interrupts.rs index 271c702..e098bfa 100644 --- a/firmware/platforms/stm32f7/src/f722/interrupts.rs +++ b/firmware/platforms/stm32f7/src/f722/interrupts.rs @@ -1,127 +1,110 @@ -//! Platform-owned interrupt acknowledgement for all supported F722 profiles. +//! Static interrupt acknowledgement for the finite F722 route variants. -use core::sync::atomic::{AtomicU8, Ordering}; - -use super::peripherals::{Spi1, Spi3, Usart2Tx, Usart2TxDma}; use super::{ F722ImuTransferCompletion, - peripherals::{ReceiveInterrupt, Usart1Tx, Usart1TxDma}, + peripherals::{Spi1, Spi3}, }; -use kaptan_dma::TxCompletion; +use crate::{Icm42688Spi3, Mpu6000Spi1, sealed}; use stm32f7::stm32f722; -use crate::F722Profile; - -const PROFILE_NONE: u8 = 0; -const PROFILE_MPU6000_SPI1_USART1: u8 = 1; -const PROFILE_ICM42688_SPI3_USART2: u8 = 2; - -static ACTIVE_PROFILE: AtomicU8 = AtomicU8::new(PROFILE_NONE); - -pub(crate) fn activate_profile(profile: F722Profile) { - let encoded = match profile { - F722Profile::Mpu6000Spi1Usart1 => PROFILE_MPU6000_SPI1_USART1, - F722Profile::Icm42688Spi3Usart2 => PROFILE_ICM42688_SPI3_USART2, - }; - ACTIVE_PROFILE.store(encoded, Ordering::Release); -} - -fn active_profile() -> u8 { - ACTIVE_PROFILE.load(Ordering::Acquire) -} - -pub fn acknowledge_exti4() -> bool { - if active_profile() != PROFILE_MPU6000_SPI1_USART1 { - return false; +/// RTIC binds a literal superset of vectors. Inactive vectors do not touch +/// MMIO; active vectors are called only after successful RTIC initialization. +/// Keeping this sealed couples the handlers to the reviewed initialization. +pub trait F722Interrupts: sealed::Route { + fn acknowledge_exti4() -> bool { + false } - // SAFETY: the selected profile configured EXTI4 once before interrupts. - // This ISR writes only the W1C pending bit for PA4 and does not alias the - // worker-owned SPI or DMA registers. - unsafe { - (&*stm32f722::EXTI::ptr()) - .pr() - .write(|write| write.pr4().clear()); + fn acknowledge_exti3() -> bool { + false } - true -} - -pub fn acknowledge_exti3() -> bool { - if active_profile() != PROFILE_ICM42688_SPI3_USART2 { - return false; + fn acknowledge_imu_rx_dma2() -> F722ImuTransferCompletion { + F722ImuTransferCompletion::NotPending } - // SAFETY: the selected profile configured EXTI3 once before interrupts. - // This ISR writes only the W1C pending bit for PC3, disjoint from every - // peripheral state owned by the deferred IMU worker. - unsafe { - (&*stm32f722::EXTI::ptr()) - .pr() - .write(|write| write.pr3().clear()); + fn acknowledge_imu_tx_dma2() -> F722ImuTransferCompletion { + F722ImuTransferCompletion::NotPending } - true -} - -pub fn acknowledge_imu_rx_dma2() -> F722ImuTransferCompletion { - if active_profile() == PROFILE_MPU6000_SPI1_USART1 { - return Spi1::acknowledge_rx_interrupt().into(); + fn acknowledge_imu_spi1_error() -> bool { + false } - F722ImuTransferCompletion::NotPending -} - -pub fn acknowledge_imu_tx_dma2() -> F722ImuTransferCompletion { - if active_profile() == PROFILE_MPU6000_SPI1_USART1 { - return Spi1::acknowledge_tx_interrupt().into(); + fn acknowledge_imu_rx_dma1() -> F722ImuTransferCompletion { + F722ImuTransferCompletion::NotPending } - F722ImuTransferCompletion::NotPending -} - -pub fn acknowledge_imu_spi1_error() -> bool { - active_profile() == PROFILE_MPU6000_SPI1_USART1 && Spi1::acknowledge_error_interrupt() -} - -pub fn acknowledge_imu_rx_dma1() -> F722ImuTransferCompletion { - if active_profile() == PROFILE_ICM42688_SPI3_USART2 { - return Spi3::acknowledge_rx_interrupt().into(); + fn acknowledge_imu_tx_dma1() -> F722ImuTransferCompletion { + F722ImuTransferCompletion::NotPending } - F722ImuTransferCompletion::NotPending -} - -pub fn acknowledge_imu_tx_dma1() -> F722ImuTransferCompletion { - if active_profile() == PROFILE_ICM42688_SPI3_USART2 { - return Spi3::acknowledge_tx_interrupt().into(); - } - F722ImuTransferCompletion::NotPending -} - -pub fn acknowledge_imu_spi3_error() -> bool { - active_profile() == PROFILE_ICM42688_SPI3_USART2 && Spi3::acknowledge_error_interrupt() -} - -pub fn acknowledge_mavlink_dma2() -> TxCompletion { - if active_profile() == PROFILE_MPU6000_SPI1_USART1 { - Usart1TxDma::acknowledge_interrupt() - } else { - TxCompletion::NotPending + fn acknowledge_imu_spi3_error() -> bool { + false } } -pub fn acknowledge_mavlink_dma1() -> TxCompletion { - if active_profile() == PROFILE_ICM42688_SPI3_USART2 { - return Usart2TxDma::acknowledge_interrupt(); +impl F722Interrupts for Mpu6000Spi1 { + fn acknowledge_exti4() -> bool { + // SAFETY: RTIC initialization configured EXTI4 before unmasking IRQs. + // This ISR writes only PA4's W1C pending bit, disjoint from the worker. + unsafe { + (&*stm32f722::EXTI::ptr()) + .pr() + .write(|write| write.pr4().clear()); + } + true } - TxCompletion::NotPending -} -pub fn acknowledge_mavlink_rx_usart1() -> ReceiveInterrupt { - if active_profile() == PROFILE_MPU6000_SPI1_USART1 { - Usart1Tx::acknowledge_receive_interrupt() - } else { - ReceiveInterrupt::NotPending + fn acknowledge_imu_rx_dma2() -> F722ImuTransferCompletion { + Spi1::acknowledge_rx_interrupt() + } + + fn acknowledge_imu_tx_dma2() -> F722ImuTransferCompletion { + Spi1::acknowledge_tx_interrupt() + } + + fn acknowledge_imu_spi1_error() -> bool { + Spi1::acknowledge_error_interrupt() } } -pub fn acknowledge_mavlink_rx_usart2() -> ReceiveInterrupt { - if active_profile() == PROFILE_ICM42688_SPI3_USART2 { - return Usart2Tx::acknowledge_receive_interrupt(); +impl F722Interrupts for Icm42688Spi3 { + fn acknowledge_exti3() -> bool { + // SAFETY: RTIC initialization configured EXTI3 before unmasking IRQs. + // This ISR writes only PC3's W1C pending bit, disjoint from the worker. + unsafe { + (&*stm32f722::EXTI::ptr()) + .pr() + .write(|write| write.pr3().clear()); + } + true + } + + fn acknowledge_imu_rx_dma1() -> F722ImuTransferCompletion { + Spi3::acknowledge_rx_interrupt() + } + + fn acknowledge_imu_tx_dma1() -> F722ImuTransferCompletion { + Spi3::acknowledge_tx_interrupt() + } + + fn acknowledge_imu_spi3_error() -> bool { + Spi3::acknowledge_error_interrupt() + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn inactive_vectors_are_noops_without_initialized_hardware() { + type Mpu = Mpu6000Spi1; + type Icm = Icm42688Spi3; + let not_pending = F722ImuTransferCompletion::NotPending; + + assert!(!Mpu::acknowledge_exti3()); + assert!(!Mpu::acknowledge_imu_spi3_error()); + assert_eq!(Mpu::acknowledge_imu_rx_dma1(), not_pending); + assert_eq!(Mpu::acknowledge_imu_tx_dma1(), not_pending); + + assert!(!Icm::acknowledge_exti4()); + assert!(!Icm::acknowledge_imu_spi1_error()); + assert_eq!(Icm::acknowledge_imu_rx_dma2(), not_pending); + assert_eq!(Icm::acknowledge_imu_tx_dma2(), not_pending); } - ReceiveInterrupt::NotPending } diff --git a/firmware/platforms/stm32f7/src/f722/mod.rs b/firmware/platforms/stm32f7/src/f722/mod.rs index 1df35fb..491e8b8 100644 --- a/firmware/platforms/stm32f7/src/f722/mod.rs +++ b/firmware/platforms/stm32f7/src/f722/mod.rs @@ -1,9 +1,4 @@ -//! Finite STM32F722 execution profiles. -//! -//! This module is the sole owner of STM32F722 register routes and execution -//! lifecycle. Its public-in-crate profile names describe useful electrical -//! meaning; the RTIC bindings, PAC names, DMA streams and synchronization are -//! intentionally contained here and in the macro template. +//! STM32F722 initialization and RTIC runtime. mod board; mod clock; @@ -12,70 +7,60 @@ mod data_channel; mod firmware; mod interrupts; mod motor_inhibit; +mod notification; mod peripherals; +pub mod ports; mod routes; mod sensors; +mod serial; mod usb; +#[cfg(test)] +#[path = "../../../../boards/geprc-f722-v2/src/board.rs"] +mod geprc; +#[cfg(test)] +#[path = "../../../../boards/kakute-f7-mini-v3/src/board.rs"] +mod kakute; + +#[cfg(test)] +#[path = "../../tests/support/config_active_state.rs"] +mod config_active_state; +#[cfg(test)] +#[path = "../../tests/support/config_files.rs"] +mod config_files; +#[cfg(test)] +#[path = "../../tests/support/route_variants.rs"] +mod route_variants; + use kaptan_config::Config; use crate::{BoardError, F7ImmutableConfig}; -/// Completion classification crossing the RTIC interrupt template. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub enum F722ImuTransferCompletion { - Complete, - TransferFault, - NotPending, -} - -impl From for F722ImuTransferCompletion { - fn from(value: kaptan_spi::SpiCompletion) -> Self { - match value { - kaptan_spi::SpiCompletion::Complete => Self::Complete, - kaptan_spi::SpiCompletion::TransferFault => Self::TransferFault, - kaptan_spi::SpiCompletion::NotPending => Self::NotPending, - } - } -} - -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub(crate) enum VerticalRoute { - None, - Mtf01, - LidarLiteV3, -} +pub use kaptan_spi::SpiCompletion as F722ImuTransferCompletion; pub use board::{ - F722InitResources, F722LidarState, F722MavlinkState, F722Mtf01, F722Mtf01Uart, - F722ServiceTimer, on_uart4_interrupt, process_imu, record_mavlink_completion, + F722InitResources, F722LidarState, F722MavlinkState, F722ServiceTimer, process_imu, }; -pub use interrupts::{ - acknowledge_exti3, acknowledge_exti4, acknowledge_imu_rx_dma1, acknowledge_imu_rx_dma2, - acknowledge_imu_spi1_error, acknowledge_imu_spi3_error, acknowledge_imu_tx_dma1, - acknowledge_imu_tx_dma2, acknowledge_mavlink_dma1, acknowledge_mavlink_dma2, - acknowledge_mavlink_rx_usart1, acknowledge_mavlink_rx_usart2, -}; -pub use sensors::F722Imu; +pub use config::F722UserConfig; +pub use interrupts::F722Interrupts; +pub use notification::EventNotification; +pub use routes::F722Route; +pub use sensors::{Icm42688Imu, ImuEvent, ImuService, Mpu6000Imu}; -/// Materializes a board declaration after RTIC has exclusively taken the PAC -/// peripherals. The output-inhibit write is the first hardware action; route -/// and configuration checks precede all non-safety hardware initialization. -pub fn initialize( +/// Inhibits motor outputs, validates configuration, then initializes hardware. +/// Called after RTIC takes the PAC peripherals. +pub fn initialize( core: cortex_m::Peripherals, device: stm32f7::stm32f722::Peripherals, factory: C, immutable: I, -) -> Result, BoardError> +) -> Result, BoardError> where C: Clone + Config, I: F7ImmutableConfig, { - // This is deliberately the first hardware access, before configuration - // validation, clocks, sensor routes, interrupts, or UART setup. It keeps - // the former four motor pins low even for malformed configuration. - motor_inhibit::inhibit_pb0_pb1_pb4_pb5(&device.RCC, &device.GPIOB); let hardware = immutable.f722().hardware; - board::validate_board_config(&factory, &immutable, hardware)?; - board::initialize::(core, device, factory, immutable, hardware) + motor_inhibit::inhibit(&device.RCC, &device.GPIOB, hardware.motors); + board::validate_board_config::(&factory, &immutable)?; + board::initialize::(core, device, factory, immutable, hardware) } diff --git a/firmware/platforms/stm32f7/src/f722/motor_inhibit.rs b/firmware/platforms/stm32f7/src/f722/motor_inhibit.rs index 8f85099..cdb0e52 100644 --- a/firmware/platforms/stm32f7/src/f722/motor_inhibit.rs +++ b/firmware/platforms/stm32f7/src/f722/motor_inhibit.rs @@ -1,13 +1,13 @@ -//! Common former-motor-pin interlock for every F722 image. -//! -//! This is intentionally independent of profile initialization and is called -//! as the first PAC access. The GPIO-low interlock is a separate electrical -//! boundary from all telemetry initialization. +//! Motor GPIO-low interlock, called before all fallible initialization. +use crate::MotorPins; use stm32f7::stm32f722; -/// Drives PB0/PB1/PB4/PB5 low before selecting push-pull GPIO output mode. -pub(crate) fn inhibit_pb0_pb1_pb4_pb5(rcc: &stm32f722::RCC, port: &stm32f722::GPIOB) { +pub(crate) fn inhibit( + rcc: &stm32f722::rcc::RegisterBlock, + port: &stm32f722::gpiob::RegisterBlock, + pins: MotorPins, +) { rcc.ahb1enr().modify(|_, write| write.gpioben().set_bit()); // RM0431 section 5.2.12 requires two peripheral clock cycles before the // first access. The volatile enable-register read supplies the standard @@ -15,7 +15,7 @@ pub(crate) fn inhibit_pb0_pb1_pb4_pb5(rcc: &stm32f722::RCC, port: &stm32f722::GP // ahead of the clock enable. let _ = rcc.ahb1enr().read().gpioben().bit_is_set(); port.bsrr().write(|write| { - write + let write = write .br0() .set_bit() .br1() @@ -23,10 +23,15 @@ pub(crate) fn inhibit_pb0_pb1_pb4_pb5(rcc: &stm32f722::RCC, port: &stm32f722::GP .br4() .set_bit() .br5() - .set_bit() + .set_bit(); + if pins == MotorPins::Six { + write.br6().set_bit().br7().set_bit() + } else { + write + } }); port.otyper().modify(|_, write| { - write + let write = write .ot0() .push_pull() .ot1() @@ -34,10 +39,15 @@ pub(crate) fn inhibit_pb0_pb1_pb4_pb5(rcc: &stm32f722::RCC, port: &stm32f722::GP .ot4() .push_pull() .ot5() - .push_pull() + .push_pull(); + if pins == MotorPins::Six { + write.ot6().push_pull().ot7().push_pull() + } else { + write + } }); port.ospeedr().modify(|_, write| { - write + let write = write .ospeedr0() .low_speed() .ospeedr1() @@ -45,10 +55,15 @@ pub(crate) fn inhibit_pb0_pb1_pb4_pb5(rcc: &stm32f722::RCC, port: &stm32f722::GP .ospeedr4() .low_speed() .ospeedr5() - .low_speed() + .low_speed(); + if pins == MotorPins::Six { + write.ospeedr6().low_speed().ospeedr7().low_speed() + } else { + write + } }); port.pupdr().modify(|_, write| { - write + let write = write .pupdr0() .floating() .pupdr1() @@ -56,10 +71,15 @@ pub(crate) fn inhibit_pb0_pb1_pb4_pb5(rcc: &stm32f722::RCC, port: &stm32f722::GP .pupdr4() .floating() .pupdr5() - .floating() + .floating(); + if pins == MotorPins::Six { + write.pupdr6().floating().pupdr7().floating() + } else { + write + } }); port.moder().modify(|_, write| { - write + let write = write .moder0() .output() .moder1() @@ -67,6 +87,51 @@ pub(crate) fn inhibit_pb0_pb1_pb4_pb5(rcc: &stm32f722::RCC, port: &stm32f722::GP .moder4() .output() .moder5() - .output() + .output(); + if pins == MotorPins::Six { + write.moder6().output().moder7().output() + } else { + write + } }); } + +#[cfg(test)] +mod tests { + use super::*; + use core::mem::MaybeUninit; + + #[test] + fn inhibits_only_board_motor_pins() { + for (pins, pin_mask, field_mask, output_mode) in [ + (MotorPins::Four, 0x0033, 0x0f0f, 0x0505), + (MotorPins::Six, 0x00f3, 0xff0f, 0x5505), + ] { + // SAFETY: these PAC blocks contain u32 volatile cells and reserved + // bytes. Zero is valid, and this test owns their ordinary RAM storage. + let (rcc, port) = unsafe { + ( + MaybeUninit::::zeroed().assume_init(), + MaybeUninit::::zeroed().assume_init(), + ) + }; + // SAFETY: writes initialize only the test's local register storage. + unsafe { + rcc.ahb1enr().write(|write| write.bits(1)); + port.moder().write(|write| write.bits(u32::MAX)); + port.otyper().write(|write| write.bits(0xffff)); + port.ospeedr().write(|write| write.bits(u32::MAX)); + port.pupdr().write(|write| write.bits(0xaaaa_aaaa)); + } + inhibit(&rcc, &port, pins); + assert_eq!(rcc.ahb1enr().read().bits(), 3); + // SAFETY: this reads the local backing cell, not a hardware write-only register. + let reset = unsafe { port.bsrr().as_ptr().read_volatile() }; + assert_eq!(reset, pin_mask << 16); + assert_eq!(port.moder().read().bits(), !field_mask | output_mode); + assert_eq!(port.otyper().read().bits(), 0xffff & !pin_mask); + assert_eq!(port.ospeedr().read().bits(), !field_mask); + assert_eq!(port.pupdr().read().bits(), 0xaaaa_aaaa & !field_mask); + } + } +} diff --git a/firmware/platforms/stm32f7/src/f722/notification.rs b/firmware/platforms/stm32f7/src/f722/notification.rs new file mode 100644 index 0000000..f795917 --- /dev/null +++ b/firmware/platforms/stm32f7/src/f722/notification.rs @@ -0,0 +1,87 @@ +//! A single permanently spawned RTIC worker, with bounded event coalescing. +use core::{ + cell::RefCell, + future::poll_fn, + task::{Poll, Waker}, +}; +use critical_section::Mutex; + +pub struct EventNotification { + state: Mutex)>>, +} +impl Default for EventNotification { + fn default() -> Self { + Self::new() + } +} +impl EventNotification { + pub const fn new() -> Self { + Self { + state: Mutex::new(RefCell::new((0, None))), + } + } + pub fn notify(&self, events: u8) { + let waker = critical_section::with(|cs| { + let mut state = self.state.borrow(cs).borrow_mut(); + state.0 |= events; + state.1.take() + }); + if let Some(waker) = waker { + waker.wake(); + } + } + pub async fn wait(&self) -> u8 { + poll_fn(|cx| { + critical_section::with(|cs| { + let mut state = self.state.borrow(cs).borrow_mut(); + if state.0 != 0 { + let events = core::mem::take(&mut state.0); + state.1 = None; + Poll::Ready(events) + } else { + state.1 = Some(cx.waker().clone()); + Poll::Pending + } + }) + }) + .await + } +} + +#[cfg(test)] +mod tests { + extern crate std; + use super::*; + use core::{future::Future, pin::pin, task::Context}; + use std::{ + sync::{ + Arc, + atomic::{AtomicUsize, Ordering}, + }, + task::Wake, + }; + struct Counter(AtomicUsize); + impl Wake for Counter { + fn wake(self: Arc) { + self.0.fetch_add(1, Ordering::Relaxed); + } + } + #[test] + fn before_poll_during_work_and_after_registration_never_lose_events() { + let signal = EventNotification::new(); + let counter = Arc::new(Counter(AtomicUsize::new(0))); + let waker = Waker::from(counter.clone()); + let mut cx = Context::from_waker(&waker); + signal.notify(1); + assert_eq!(pin!(signal.wait()).poll(&mut cx), Poll::Ready(1)); + signal.notify(2); + signal.notify(4); + assert_eq!(pin!(signal.wait()).poll(&mut cx), Poll::Ready(6)); + let mut wait = pin!(signal.wait()); + assert_eq!(wait.as_mut().poll(&mut cx), Poll::Pending); + signal.notify(8); + signal.notify(16); + assert_eq!(counter.0.load(Ordering::Relaxed), 1); + assert_eq!(wait.poll(&mut cx), Poll::Ready(24)); + } +} diff --git a/firmware/platforms/stm32f7/src/f722/peripherals/clock.rs b/firmware/platforms/stm32f7/src/f722/peripherals/clock.rs index 00fe059..5263625 100644 --- a/firmware/platforms/stm32f7/src/f722/peripherals/clock.rs +++ b/firmware/platforms/stm32f7/src/f722/peripherals/clock.rs @@ -85,8 +85,8 @@ pub fn select_pll216_profile( return Err(ClockProfileError::OverDriveSwitchNotReady); } flash.acr().modify(|read, write| { - // SAFETY: these are the STM32F7-documented latency, instruction-cache and - // data-cache fields for 216 MHz; the read-modify-write retains other ACR bits. + // SAFETY: these are the F722 latency, prefetch-enable and ART-enable + // fields for 216 MHz; the read-modify-write retains other ACR bits. unsafe { write.bits((read.bits() & !0x0f) | 7 | (1 << 8) | (1 << 9)) } }); @@ -102,6 +102,9 @@ pub fn select_pll216_profile( }) { return Err(ClockProfileError::PllNotReady); } + // Make the APB timer multiplier explicit rather than inheriting boot state. + // With APB1=/4, TIMPRE=0 selects 2 * PCLK1 = 108 MHz for TIM2. + rcc.dckcfgr1().modify(|_, write| write.timpre().clear_bit()); rcc.cfgr().modify(|read, write| { // AHB /1, APB1 /4, APB2 /2, SYSCLK = PLL. // SAFETY: this preserves every CFGR field except the documented prescaler and diff --git a/firmware/platforms/stm32f7/src/f722/peripherals/dma_buffer.rs b/firmware/platforms/stm32f7/src/f722/peripherals/dma_buffer.rs deleted file mode 100644 index 517399d..0000000 --- a/firmware/platforms/stm32f7/src/f722/peripherals/dma_buffer.rs +++ /dev/null @@ -1,35 +0,0 @@ -//! D-cache-disabled DMA memory policy for STM32F722 boards. - -use kaptan_dma::{DmaReadableBuffer, DmaTxBufferFactory}; -/// -/// DMA transmit memory policy for a platform with D-cache disabled. -/// -/// The platform linker policy must also place transfer frames in SRAM reachable -/// by the selected DMA controller. This policy does not support a cached board; -/// such a platform requires a distinct policy that performs cache maintenance. -pub struct DcacheDisabledDmaMemory { - _private: (), -} - -impl DcacheDisabledDmaMemory { - /// Declares that the selected platform disabled D-cache before DMA setup. - /// - /// # Safety - /// - /// - D-cache must remain disabled for every CPU access to DMA-owned frames. - /// - The linker policy must locate those frames in DMA-visible SRAM. - pub const unsafe fn new() -> Self { - Self { _private: () } - } -} - -impl DmaTxBufferFactory for DcacheDisabledDmaMemory { - type Error = core::convert::Infallible; - - fn prepare<'a>(&mut self, bytes: &'a [u8]) -> Result, Self::Error> { - // SAFETY: `Self` can exist only after the platform proved its D-cache and - // linker-memory policy. The runtime retains frame ownership until a - // terminal DMA outcome or successful recovery. - Ok(unsafe { DmaReadableBuffer::new_unchecked(bytes) }) - } -} diff --git a/firmware/platforms/stm32f7/src/f722/peripherals/gpio.rs b/firmware/platforms/stm32f7/src/f722/peripherals/gpio.rs index 7df909d..1b85189 100644 --- a/firmware/platforms/stm32f7/src/f722/peripherals/gpio.rs +++ b/firmware/platforms/stm32f7/src/f722/peripherals/gpio.rs @@ -152,10 +152,7 @@ pub fn configure_usart1_rx_pin_pa10(port: &stm32f722::GPIOA) { // SAFETY: AF7 selects USART1 RX on PA10 on STM32F722. unsafe { write.afrh10().bits(0b0111) } }); - // USART is idle-high. Leaving PA10 floating before its peer drives the - // line can manufacture framing and noise errors. The weak internal pull-up - // supplies only the electrically correct idle level; it does not mask a - // driven low, framing, noise, or overrun fault. + // Hold RX idle-high while the peer is disconnected to avoid spurious errors. port.pupdr().modify(|_, write| write.pupdr10().pull_up()); } @@ -171,7 +168,41 @@ pub fn configure_usart2_pins_pa2_pa3(port: &stm32f722::GPIOA) { port.ospeedr() .modify(|_, write| write.ospeedr2().high_speed()); port.pupdr() - .modify(|_, write| write.pupdr2().floating().pupdr3().floating()); + .modify(|_, write| write.pupdr2().floating().pupdr3().pull_up()); +} + +/// Configures PB10/PB11 for USART3 alternate function 7. +pub fn configure_usart3_pins_pb10_pb11(rcc: &stm32f722::RCC, port: &stm32f722::GPIOB) { + rcc.ahb1enr().modify(|_, write| write.gpioben().set_bit()); + let _ = rcc.ahb1enr().read(); + port.moder() + .modify(|_, write| write.moder10().alternate().moder11().alternate()); + port.afrh().modify(|_, write| { + // SAFETY: AF7 selects USART3 TX/RX on PB10/PB11 on STM32F722. + unsafe { write.afrh10().bits(7).afrh11().bits(7) } + }); + port.otyper().modify(|_, write| write.ot10().push_pull()); + port.ospeedr() + .modify(|_, write| write.ospeedr10().high_speed()); + port.pupdr() + .modify(|_, write| write.pupdr10().floating().pupdr11().pull_up()); +} + +/// Configures PC6/PC7 for USART6 alternate function 8. +pub fn configure_usart6_pins_pc6_pc7(rcc: &stm32f722::RCC, port: &stm32f722::GPIOC) { + rcc.ahb1enr().modify(|_, write| write.gpiocen().set_bit()); + let _ = rcc.ahb1enr().read(); + port.moder() + .modify(|_, write| write.moder6().alternate().moder7().alternate()); + port.afrl().modify(|_, write| { + // SAFETY: AF8 selects USART6 TX/RX on PC6/PC7 on STM32F722. + unsafe { write.afrl6().bits(8).afrl7().bits(8) } + }); + port.otyper().modify(|_, write| write.ot6().push_pull()); + port.ospeedr() + .modify(|_, write| write.ospeedr6().high_speed()); + port.pupdr() + .modify(|_, write| write.pupdr6().floating().pupdr7().pull_up()); } /// Configures PA11/PA12 for the STM32F722 OTG FS data pair. @@ -203,7 +234,7 @@ pub fn configure_uart4_pins_pa0_pa1(port: &stm32f722::GPIOA) { port.ospeedr() .modify(|_, write| write.ospeedr0().high_speed()); port.pupdr() - .modify(|_, write| write.pupdr0().floating().pupdr1().floating()); + .modify(|_, write| write.pupdr0().floating().pupdr1().pull_up()); } /// Active-low status LED connected to PD2. diff --git a/firmware/platforms/stm32f7/src/f722/peripherals/i2c1.rs b/firmware/platforms/stm32f7/src/f722/peripherals/i2c1.rs index a3b1508..4f71257 100644 --- a/firmware/platforms/stm32f7/src/f722/peripherals/i2c1.rs +++ b/firmware/platforms/stm32f7/src/f722/peripherals/i2c1.rs @@ -1,7 +1,8 @@ //! STM32F722 I²C1 interrupt-driven controller. use kaptan_err_reportable::KaptanError; -use kaptan_i2c::{I2cBus, I2cCompletion}; +use kaptan_i2c::{CompletionError, I2cBus, I2cCompletion}; +use kaptan_time::Timestamp; use stm32f7::stm32f722; const MAX_BYTES: usize = 255; @@ -20,6 +21,9 @@ const ICR_ERRORS: u32 = (1 << 8) | (1 << 9) | (1 << 10); const ICR_TERMINAL: u32 = ICR_NACKCF | ICR_STOPCF | ICR_ERRORS; const ICR_FAULTS: u32 = ICR_NACKCF | ICR_ERRORS; const CR2_STOP: u32 = 1 << 14; +// Disable the controller after 10 ms without STOP. This is a shutdown budget, +// not an I²C clock-stretch limit. +const STOP_TIMEOUT_US: u64 = 10_000; #[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)] pub enum I2c1Error { @@ -29,6 +33,8 @@ pub enum I2c1Error { InvalidAddress, #[kaptan_error(id = 11703)] Busy, + #[kaptan_error(id = 11704)] + Halted, } /// Platform-selected I²C1 electrical timing. @@ -54,7 +60,9 @@ enum Direction { enum TransferState { Idle, Active, - Stopping, + Completed(I2cCompletion), + Stopping { since: Timestamp }, + Halted, } fn read_index_after_event(direction: Direction, flags: u32, index: usize, length: usize) -> usize { @@ -79,25 +87,60 @@ impl TransferState { } fn after_stop(self) -> Self { - if self == Self::Stopping { + if matches!(self, Self::Stopping { .. }) { Self::Idle } else { self } } - fn after_driver_completion(self) -> Self { - if self == Self::Active { - Self::Idle + fn completion(self, notification: I2cCompletion) -> Result<(), CompletionError> { + if notification == I2cCompletion::NotPending { + return Err(CompletionError::NotPending); + } + match self { + Self::Completed(I2cCompletion::Complete) if notification == I2cCompletion::Complete => { + Ok(()) + } + Self::Completed(_) | Self::Stopping { .. } => Err(CompletionError::TransferFault), + Self::Halted => Err(CompletionError::StopUnproven), + Self::Active => Err(CompletionError::NotPending), + Self::Idle => Err(CompletionError::NotActive), + } + } + + fn expire_stop(&mut self, now: Timestamp) -> Result<(), CompletionError> { + if matches!(*self, Self::Stopping { since } if now.as_microseconds().checked_sub(since.as_microseconds()).is_some_and(|elapsed| elapsed >= STOP_TIMEOUT_US)) + { + *self = Self::Halted; + Err(CompletionError::StopUnproven) } else { - self + Ok(()) } } } /// Owns I²C1 while one interrupt-driven transaction is active. +/// Only CPU ISR code accesses `buffer`, under the owner's exclusive borrow. +/// DMA is never enabled, so moving/dropping this owner cannot free DMA memory. pub struct I2c1 { - peripheral: stm32f722::I2C1, + controller: Controller, +} + +// Register access shared by the PAC driver and test model. +trait Registers { + fn status(&self) -> u32; + fn read_receive(&mut self) -> u8; + fn write_transmit(&mut self, byte: u8); + fn clear_flags(&mut self, flags: u32); + fn control1(&self) -> u32; + fn write_control1(&mut self, value: u32); + fn control2(&self) -> u32; + fn write_control2(&mut self, value: u32); +} + +struct Controller { + peripheral: R, buffer: [u8; MAX_BYTES], length: usize, index: usize, @@ -122,30 +165,42 @@ impl I2c1 { .cr1() .write(|w| unsafe { w.bits(CR1_INTERRUPTS) }); Self { - peripheral, - buffer: [0; MAX_BYTES], - length: 0, - index: 0, - direction: Direction::Write, - state: TransferState::Idle, + controller: Controller { + peripheral, + buffer: [0; MAX_BYTES], + length: 0, + index: 0, + direction: Direction::Write, + state: TransferState::Idle, + }, } } - /// Advances the event IRQ; returns a terminal result only after STOPF. - pub fn acknowledge_event_interrupt(&mut self) -> I2cCompletion { - let flags = self.peripheral.isr().read().bits(); + /// Advances the event IRQ. Success requires STOPF. Faults request STOP and + /// retain closed admission until STOP is observed or service halts the route. + pub fn acknowledge_event_interrupt(&mut self, now: Timestamp) -> I2cCompletion { + self.controller.acknowledge_event_interrupt(now) + } + + pub fn acknowledge_error_interrupt(&mut self, now: Timestamp) -> I2cCompletion { + self.controller.acknowledge_error_interrupt(now) + } +} + +impl Controller { + fn acknowledge_event_interrupt(&mut self, now: Timestamp) -> I2cCompletion { + let flags = self.peripheral.status(); if self.state != TransferState::Active { - self.clear_inactive_event_flags(flags); + self.clear_inactive_event_flags(flags, now); return I2cCompletion::NotPending; } - if flags & ISR_ERRORS != 0 { - self.abort(); - self.acknowledge_stop_if_present(flags); - return I2cCompletion::TransferFault; - } - if flags & ISR_NACKF != 0 { - self.abort(); - self.acknowledge_stop_if_present(flags); + if flags & (ISR_ERRORS | ISR_NACKF) != 0 + || (flags & ISR_RXNE != 0 + && (!matches!(self.direction, Direction::Read) || self.index >= self.length)) + { + // RXNE on a write or beyond the read length is unusable data. It + // must be consumed and byte IRQs disabled, not left level-pending. + self.fail_transfer(flags, now); return I2cCompletion::TransferFault; } // With AUTOEND a final RXNE and STOPF may be visible in one ISR @@ -156,44 +211,62 @@ impl I2c1 { && flags & ISR_TXIS != 0 && self.index < self.length { - // SAFETY: the value is an eight-bit TXDR payload. - self.peripheral - .txdr() - .write(|w| unsafe { w.bits(u32::from(self.buffer[self.index])) }); + self.peripheral.write_transmit(self.buffer[self.index]); self.index += 1; } let next_read_index = read_index_after_event(self.direction, flags, self.index, self.length); if next_read_index != self.index { - self.buffer[self.index] = self.peripheral.rxdr().read().bits() as u8; + self.buffer[self.index] = self.peripheral.read_receive(); self.index = next_read_index; } if flags & ISR_STOPF != 0 { self.acknowledge_stop(); - return completion_after_stop(self.index, self.length); + let completion = completion_after_stop(self.index, self.length); + self.state = TransferState::Completed(completion); + return completion; } I2cCompletion::NotPending } - pub fn acknowledge_error_interrupt(&mut self) -> I2cCompletion { - let flags = self.peripheral.isr().read().bits(); + fn acknowledge_error_interrupt(&mut self, now: Timestamp) -> I2cCompletion { + let flags = self.peripheral.status(); if flags & ISR_ERRORS == 0 { return I2cCompletion::NotPending; } if self.state == TransferState::Active { - self.abort(); - self.acknowledge_stop_if_present(flags); + self.fail_transfer(flags, now); I2cCompletion::TransferFault } else { // A STOP may have already retired the software transaction before // the error IRQ is serviced. Its flags still must be acknowledged // or this high-priority IRQ will continuously retrigger. - self.clear_inactive_event_flags(flags); + self.clear_inactive_event_flags(flags, now); I2cCompletion::NotPending } } + fn fail_transfer(&mut self, flags: u32, now: Timestamp) { + self.abort(now); + // STOPCF does not clear RXNE. Discard RXDR before STOP retirement can + // reopen admission, including simultaneous BERR/ARLO/OVR + STOPF + RXNE. + self.discard_receive(); + self.acknowledge_stop_if_present(flags); + if flags & ISR_STOPF != 0 { + self.state = TransferState::Completed(I2cCompletion::TransferFault); + } + } + + fn discard_receive(&mut self) { + if self.peripheral.status() & ISR_RXNE != 0 { + let _ = self.peripheral.read_receive(); + } + } + fn start(&mut self, address: u8, length: usize, direction: Direction) -> Result<(), I2c1Error> { + if self.state == TransferState::Halted { + return Err(I2c1Error::Halted); + } if length == 0 || length > MAX_BYTES { return Err(I2c1Error::InvalidLength); } @@ -203,14 +276,12 @@ impl I2c1 { if !self.state.accepts_start(self.bus_is_busy()) { return Err(I2c1Error::Busy); } - // SAFETY: ICR_TERMINAL contains only documented write-one-to-clear flags. - self.peripheral - .icr() - .write(|w| unsafe { w.bits(ICR_TERMINAL) }); + self.peripheral.clear_flags(ICR_TERMINAL); self.length = length; self.index = 0; self.direction = direction; self.state = TransferState::Active; + self.peripheral.write_control1(CR1_INTERRUPTS); let direction_bit = if matches!(direction, Direction::Read) { 1 << 10 } else { @@ -221,35 +292,36 @@ impl I2c1 { | ((length as u32) << 16) | (1 << 25) | (1 << 13); - // SAFETY: `cr2` contains a validated 7-bit address and documented transfer fields. - self.peripheral.cr2().write(|w| unsafe { w.bits(cr2) }); + self.peripheral.write_control2(cr2); Ok(()) } - fn abort(&mut self) { + fn abort(&mut self, now: Timestamp) { // Keep software ownership closed until STOPF is observed. A requested // STOP may be delayed indefinitely by a held bus, so this state keeps // a new START from being issued before the terminal STOPF arrives. - self.state = TransferState::Stopping; - // SAFETY: this only requests STOP while preserving the current CR2 fields. - self.peripheral - .cr2() - .modify(|read, write| unsafe { write.bits(read.bits() | CR2_STOP) }); - // SAFETY: ICR_FAULTS contains only documented fault write-one-to-clear flags. - self.peripheral - .icr() - .write(|w| unsafe { w.bits(ICR_FAULTS) }); + if matches!( + self.state, + TransferState::Stopping { .. } | TransferState::Halted + ) { + return; + } + self.state = TransferState::Stopping { since: now }; + // Only terminal/error interrupts are useful during STOP. In particular, + // TXIS must not continually retrigger while a slave holds the bus. + let cr1 = self.peripheral.control1() & !((1 << 1) | (1 << 2)); + self.peripheral.write_control1(cr1); + let cr2 = self.peripheral.control2() | CR2_STOP; + self.peripheral.write_control2(cr2); + self.peripheral.clear_flags(ICR_FAULTS); } fn bus_is_busy(&self) -> bool { - self.peripheral.isr().read().bits() & (1 << 15) != 0 + self.peripheral.status() & (1 << 15) != 0 } fn acknowledge_stop(&mut self) { - // SAFETY: ICR_STOPCF is the documented STOP write-one-to-clear bit. - self.peripheral - .icr() - .write(|w| unsafe { w.bits(ICR_STOPCF) }); + self.peripheral.clear_flags(ICR_STOPCF); self.state = self.state.after_stop(); } @@ -259,30 +331,28 @@ impl I2c1 { } } - fn clear_inactive_event_flags(&mut self, flags: u32) { + fn clear_inactive_event_flags(&mut self, flags: u32, now: Timestamp) { // An abort may request STOP after the driver has already released the // transaction. Drain that terminal IRQ instead of leaving I2C1_EV // pending forever. RXNE is normally impossible here, but draining it // keeps a malformed terminal transfer from becoming an IRQ storm. - if flags & ISR_RXNE != 0 { - let _ = self.peripheral.rxdr().read().bits(); - } + self.discard_receive(); if flags & (ISR_NACKF | ISR_ERRORS) != 0 { - // SAFETY: ICR_FAULTS contains only documented fault write-one-to-clear flags. - self.peripheral - .icr() - .write(|w| unsafe { w.bits(ICR_FAULTS) }); + self.peripheral.clear_flags(ICR_FAULTS); } self.acknowledge_stop_if_present(flags); - if flags & ISR_TXIS != 0 { - self.abort(); + if flags & ISR_TXIS != 0 && flags & ISR_STOPF == 0 && self.state == TransferState::Idle { + self.abort(now); } } } -impl I2cBus for I2c1 { +impl I2cBus for Controller { type StartError = I2c1Error; fn start_write(&mut self, address: u8, write: &[u8]) -> Result<(), Self::StartError> { + if self.state == TransferState::Halted { + return Err(I2c1Error::Halted); + } if write.is_empty() || write.len() > MAX_BYTES { return Err(I2c1Error::InvalidLength); } @@ -298,55 +368,331 @@ impl I2cBus for I2c1 { fn start_read(&mut self, address: u8, length: usize) -> Result<(), Self::StartError> { self.start(address, length, Direction::Read) } - fn complete_transaction(&mut self, completion: I2cCompletion, read: Option<&mut [u8]>) { - if completion == I2cCompletion::Complete - && matches!(self.direction, Direction::Read) - && let Some(read) = read + fn complete_transaction( + &mut self, + completion: I2cCompletion, + read: Option<&mut [u8]>, + ) -> Result<(), CompletionError> { + let result = self.state.completion(completion); + if !matches!(self.state, TransferState::Completed(_)) + || completion == I2cCompletion::NotPending { - debug_assert_eq!(read.len(), self.length); + return result; + } + self.state = TransferState::Idle; + result?; + if matches!(self.direction, Direction::Read) { + let read = read + .filter(|read| read.len() == self.length) + .ok_or(CompletionError::InvalidLength)?; read.copy_from_slice(&self.buffer[..self.length]); } - self.state = self.state.after_driver_completion(); self.length = 0; self.index = 0; + Ok(()) } - fn cancel_transaction(&mut self) { + fn cancel_transaction(&mut self, now: Timestamp) { if self.state == TransferState::Active { - self.abort(); + self.abort(now); + } else if matches!(self.state, TransferState::Completed(_)) { + self.state = TransferState::Idle; } - self.length = 0; - self.index = 0; + } + + fn service(&mut self, now: Timestamp) -> Result<(), CompletionError> { + if matches!(self.state, TransferState::Stopping { .. }) { + let flags = self.peripheral.status(); + self.clear_inactive_event_flags(flags, now); + if let Err(error) = self.state.expire_stop(now) { + // RM0431 I2C_CR1.PE: disable peripheral and all IRQ sources. + // This halts local work but does not prove a physical STOP. + self.peripheral.write_control1(0); + return Err(error); + } + } + Ok(()) + } +} + +impl I2cBus for I2c1 { + type StartError = I2c1Error; + + fn start_write(&mut self, address: u8, write: &[u8]) -> Result<(), Self::StartError> { + self.controller.start_write(address, write) + } + + fn start_read(&mut self, address: u8, length: usize) -> Result<(), Self::StartError> { + self.controller.start_read(address, length) + } + + fn complete_transaction( + &mut self, + completion: I2cCompletion, + read: Option<&mut [u8]>, + ) -> Result<(), CompletionError> { + self.controller.complete_transaction(completion, read) + } + + fn cancel_transaction(&mut self, now: Timestamp) { + self.controller.cancel_transaction(now); + } + + fn service(&mut self, now: Timestamp) -> Result<(), CompletionError> { + self.controller.service(now) + } +} + +impl Registers for stm32f722::I2C1 { + fn status(&self) -> u32 { + self.isr().read().bits() + } + + fn read_receive(&mut self) -> u8 { + self.rxdr().read().bits() as u8 + } + + fn write_transmit(&mut self, byte: u8) { + // SAFETY: byte is an eight-bit TXDR payload. + self.txdr().write(|w| unsafe { w.bits(u32::from(byte)) }); + } + + fn clear_flags(&mut self, flags: u32) { + // SAFETY: the private controller supplies only ICR W1C flag masks. + self.icr().write(|w| unsafe { w.bits(flags) }); + } + + fn control1(&self) -> u32 { + self.cr1().read().bits() + } + + fn write_control1(&mut self, value: u32) { + // SAFETY: the private controller writes documented CR1 enables or + // preserves their current values while clearing TXIE/RXIE. + self.cr1().write(|w| unsafe { w.bits(value) }); + } + + fn control2(&self) -> u32 { + self.cr2().read().bits() + } + + fn write_control2(&mut self, value: u32) { + // SAFETY: the private controller validates transfer fields for START, + // or preserves those fields while requesting STOP. + self.cr2().write(|w| unsafe { w.bits(value) }); } } #[cfg(test)] mod tests { - use super::{ - Direction, I2cCompletion, ICR_ERRORS, ICR_FAULTS, ICR_NACKCF, ICR_STOPCF, ISR_RXNE, - ISR_STOPF, TransferState, completion_after_stop, read_index_after_event, - }; + use super::*; + + fn t(us: u64) -> Timestamp { + Timestamp::from_microseconds(us) + } + + #[derive(Default)] + struct RegisterModel { + flags: u32, + cr1: u32, + cr2: u32, + rxdr: u8, + receive_reads: usize, + transmitted: [u8; 2], + transmit_writes: usize, + stop_cleared_with_rxne: bool, + } + + impl RegisterModel { + fn receive(&mut self, byte: u8) { + assert_eq!(self.flags & ISR_RXNE, 0, "unread RXDR would overrun"); + self.rxdr = byte; + self.flags |= ISR_RXNE; + } + + fn stop(&mut self) { + self.flags = (self.flags & !(1 << 15)) | ISR_STOPF; + self.cr2 &= !CR2_STOP; + } + + fn interrupt_pending(&self) -> bool { + self.cr1 & 1 != 0 + && (self.cr1 & self.flags & (ISR_TXIS | ISR_RXNE | ISR_NACKF | ISR_STOPF) != 0 + || (self.cr1 & (1 << 7) != 0 && self.flags & ISR_ERRORS != 0)) + } + } + + impl Registers for RegisterModel { + fn status(&self) -> u32 { + self.flags + } + + fn read_receive(&mut self) -> u8 { + self.receive_reads += 1; + self.flags &= !ISR_RXNE; + self.rxdr + } + + fn write_transmit(&mut self, byte: u8) { + self.transmitted[self.transmit_writes] = byte; + self.transmit_writes += 1; + self.flags &= !ISR_TXIS; + } + + fn clear_flags(&mut self, flags: u32) { + if flags & ICR_STOPCF != 0 && self.flags & ISR_RXNE != 0 { + self.stop_cleared_with_rxne = true; + } + // RM0431: ICR W1C bits clear the corresponding terminal/error flags. + // Neither STOPCF nor any other ICR write consumes RXDR/RXNE. + self.flags &= !(flags & ICR_TERMINAL); + } + + fn control1(&self) -> u32 { + self.cr1 + } + fn write_control1(&mut self, value: u32) { + self.cr1 = value; + } + fn control2(&self) -> u32 { + self.cr2 + } + + fn write_control2(&mut self, value: u32) { + self.cr2 = value & !(1 << 13); + if value & (1 << 13) != 0 { + self.flags |= 1 << 15; + if value & (1 << 10) == 0 { + self.flags |= ISR_TXIS; + } + } + } + } + + fn software_controller(state: TransferState) -> Controller { + Controller { + peripheral: RegisterModel::default(), + buffer: [0xa5; MAX_BYTES], + length: 2, + index: 1, + direction: Direction::Read, + state, + } + } #[test] fn rejects_starts_while_a_transaction_or_recovery_is_pending() { assert!(TransferState::Idle.accepts_start(false)); assert!(!TransferState::Idle.accepts_start(true)); assert!(!TransferState::Active.accepts_start(false)); - assert!(!TransferState::Stopping.accepts_start(false)); + assert!(!TransferState::Stopping { since: t(0) }.accepts_start(false)); + assert!(!TransferState::Halted.accepts_start(false)); + assert!(!TransferState::Completed(I2cCompletion::Complete).accepts_start(false)); } #[test] - fn abort_stays_closed_until_stop_and_normal_completion_releases_ownership() { - assert_eq!(TransferState::Stopping.after_stop(), TransferState::Idle); + fn abort_stays_closed_until_stop_and_expires_by_elapsed_time() { + let mut stopping = TransferState::Stopping { since: t(123) }; + assert_eq!(stopping.after_stop(), TransferState::Idle); assert_eq!(TransferState::Active.after_stop(), TransferState::Active); + assert_eq!(stopping.expire_stop(t(123)), Ok(())); + assert_eq!(stopping.expire_stop(t(10_122)), Ok(())); assert_eq!( - TransferState::Active.after_driver_completion(), - TransferState::Idle + stopping.expire_stop(t(10_123)), + Err(CompletionError::StopUnproven) ); + assert_eq!(stopping, TransferState::Halted); + assert_eq!(stopping.expire_stop(t(u64::MAX)), Ok(())); + assert_eq!(stopping, TransferState::Halted); + assert_eq!(TransferState::Halted.after_stop(), TransferState::Halted); + let mut controller = software_controller(TransferState::Halted); + assert_eq!(controller.start_read(98, 2), Err(I2c1Error::Halted)); + assert_eq!(controller.start_write(98, &[1]), Err(I2c1Error::Halted)); + } + + #[test] + fn not_pending_never_changes_active_completed_or_stopping_ownership_or_bytes() { + for state in [ + TransferState::Active, + TransferState::Completed(I2cCompletion::Complete), + TransferState::Stopping { since: t(123) }, + TransferState::Halted, + ] { + let mut controller = software_controller(state); + let mut destination = [0x55; 2]; + assert_eq!( + controller.complete_transaction(I2cCompletion::NotPending, Some(&mut destination)), + Err(CompletionError::NotPending) + ); + assert_eq!(controller.state, state); + assert_eq!((controller.length, controller.index), (2, 1)); + assert_eq!(controller.buffer, [0xa5; MAX_BYTES]); + assert_eq!(destination, [0x55; 2]); + } + } + + #[test] + fn delayed_notification_cannot_complete_new_active_transaction() { + let mut controller = software_controller(TransferState::Active); + let mut destination = [0x55; 2]; assert_eq!( - TransferState::Stopping.after_driver_completion(), - TransferState::Stopping + controller.complete_transaction(I2cCompletion::Complete, Some(&mut destination)), + Err(CompletionError::NotPending) ); + assert_eq!(controller.state, TransferState::Active); + assert_eq!(destination, [0x55; 2]); + controller.state = TransferState::Completed(I2cCompletion::Complete); + assert_eq!( + controller.complete_transaction(I2cCompletion::Complete, Some(&mut destination)), + Ok(()) + ); + assert_eq!(destination, [0xa5; 2]); + assert_eq!(controller.state, TransferState::Idle); + destination.fill(0x55); + assert_eq!( + controller.complete_transaction(I2cCompletion::Complete, Some(&mut destination)), + Err(CompletionError::NotActive) + ); + assert_eq!(destination, [0x55; 2]); + } + + #[test] + fn fault_length_error_and_unproven_stop_never_copy_out() { + for (state, length, error) in [ + ( + TransferState::Completed(I2cCompletion::TransferFault), + 2, + CompletionError::TransferFault, + ), + ( + TransferState::Completed(I2cCompletion::Complete), + 1, + CompletionError::InvalidLength, + ), + ( + TransferState::Stopping { since: t(100) }, + 2, + CompletionError::TransferFault, + ), + (TransferState::Halted, 2, CompletionError::StopUnproven), + ] { + let mut controller = software_controller(state); + let mut destination = [0x55; 2]; + assert_eq!( + controller.complete_transaction( + I2cCompletion::Complete, + Some(&mut destination[..length]) + ), + Err(error) + ); + assert_eq!(destination, [0x55; 2]); + if matches!( + state, + TransferState::Stopping { .. } | TransferState::Halted + ) { + assert_eq!(controller.state, state); + } + } } #[test] @@ -362,4 +708,141 @@ mod tests { assert_eq!(index, 2); assert_eq!(completion_after_stop(index, 2), I2cCompletion::Complete); } + + #[test] + fn both_irq_paths_discard_fault_stop_rxne_before_following_write_or_read() { + for error_irq in [false, true] { + for fault in [ISR_BERR, ISR_ARLO, ISR_OVR, ISR_NACKF] { + if error_irq && fault == ISR_NACKF { + continue; + } + for next_direction in [Direction::Write, Direction::Read] { + let mut controller = software_controller(TransferState::Idle); + controller.start_read(98, 2).unwrap(); + controller.peripheral.receive(0xde); + controller.peripheral.flags |= fault; + controller.peripheral.stop(); + assert!(controller.peripheral.interrupt_pending()); + let completion = if error_irq { + controller.acknowledge_error_interrupt(t(100)) + } else { + controller.acknowledge_event_interrupt(t(100)) + }; + assert_eq!(completion, I2cCompletion::TransferFault); + assert_eq!(controller.peripheral.receive_reads, 1); + assert!(!controller.peripheral.stop_cleared_with_rxne); + assert!(!controller.peripheral.interrupt_pending()); + assert_eq!(controller.buffer, [0xa5; MAX_BYTES]); + assert_eq!( + controller.complete_transaction(completion, None), + Err(CompletionError::TransferFault) + ); + assert_eq!(controller.state, TransferState::Idle); + + match next_direction { + Direction::Write => { + controller.start_write(98, &[0x10, 0x20]).unwrap(); + for _ in 0..2 { + controller.peripheral.flags |= ISR_TXIS; + assert_eq!( + controller.acknowledge_event_interrupt(t(200)), + I2cCompletion::NotPending + ); + assert!(!controller.peripheral.interrupt_pending()); + } + controller.peripheral.stop(); + assert_eq!( + controller.acknowledge_event_interrupt(t(300)), + I2cCompletion::Complete + ); + controller + .complete_transaction(I2cCompletion::Complete, None) + .unwrap(); + assert_eq!(controller.peripheral.transmitted, [0x10, 0x20]); + assert_eq!(controller.peripheral.receive_reads, 1); + } + Direction::Read => { + controller.start_read(98, 2).unwrap(); + assert!(!controller.peripheral.interrupt_pending()); + assert_eq!( + controller.acknowledge_event_interrupt(t(200)), + I2cCompletion::NotPending + ); + assert_eq!( + controller.index, 0, + "no old RXDR byte may enter the new read" + ); + controller.peripheral.receive(0x12); + assert_eq!( + controller.acknowledge_event_interrupt(t(250)), + I2cCompletion::NotPending + ); + controller.peripheral.receive(0x34); + controller.peripheral.stop(); + assert_eq!( + controller.acknowledge_event_interrupt(t(300)), + I2cCompletion::Complete + ); + let mut read = [0; 2]; + controller + .complete_transaction(I2cCompletion::Complete, Some(&mut read)) + .unwrap(); + assert_eq!(read, [0x12, 0x34]); + assert_eq!(controller.peripheral.receive_reads, 3); + } + } + assert_eq!(controller.state, TransferState::Idle); + assert!(!controller.peripheral.interrupt_pending()); + } + } + } + } + + #[test] + fn unexpected_rxne_during_write_is_drained_and_cannot_starve_stop_deadline() { + let mut controller = software_controller(TransferState::Idle); + controller.start_write(98, &[1, 2]).unwrap(); + controller.peripheral.receive(0xde); + assert_eq!( + controller.acknowledge_event_interrupt(t(100)), + I2cCompletion::TransferFault + ); + assert_eq!(controller.peripheral.receive_reads, 1); + assert_eq!(controller.peripheral.transmit_writes, 0); + assert_eq!(controller.state, TransferState::Stopping { since: t(100) }); + assert!(!controller.peripheral.interrupt_pending()); + assert_eq!(controller.start_read(98, 2), Err(I2c1Error::Busy)); + controller.service(t(10_099)).unwrap(); + assert_eq!( + controller.service(t(10_100)), + Err(CompletionError::StopUnproven) + ); + assert_eq!(controller.peripheral.cr1, 0); + assert_eq!(controller.state, TransferState::Halted); + controller.peripheral.stop(); + assert_eq!( + controller.acknowledge_event_interrupt(t(10_101)), + I2cCompletion::NotPending + ); + assert_eq!(controller.state, TransferState::Halted); + assert!(!controller.peripheral.interrupt_pending()); + } + + #[test] + fn extra_rxne_at_read_end_is_discarded_as_fault_even_with_stop() { + let mut controller = software_controller(TransferState::Idle); + controller.start_read(98, 1).unwrap(); + controller.peripheral.receive(0x12); + controller.acknowledge_event_interrupt(t(100)); + controller.peripheral.receive(0xde); + controller.peripheral.stop(); + assert_eq!( + controller.acknowledge_event_interrupt(t(200)), + I2cCompletion::TransferFault + ); + assert_eq!(controller.buffer[0], 0x12); + assert_eq!(controller.peripheral.receive_reads, 2); + assert!(!controller.peripheral.stop_cleared_with_rxne); + assert!(!controller.peripheral.interrupt_pending()); + } } diff --git a/firmware/platforms/stm32f7/src/f722/peripherals/mod.rs b/firmware/platforms/stm32f7/src/f722/peripherals/mod.rs index 2e09605..d65df9c 100644 --- a/firmware/platforms/stm32f7/src/f722/peripherals/mod.rs +++ b/firmware/platforms/stm32f7/src/f722/peripherals/mod.rs @@ -1,35 +1,29 @@ -//! STM32F722 peripheral implementations selected by supported boards. -//! -//! This module exports only typed peripheral building blocks. The family -//! platform selects pins, clocks, interrupt priorities, and static ownership. +//! STM32F722 peripheral drivers. mod clock; -mod dma_buffer; mod gpio; mod i2c1; mod spi1; mod spi3; +mod spi_dma; mod tim2; mod time; mod usart; -mod usart1_tx_dma2_stream7; -mod usart2_tx_dma1_stream6; mod usb; pub use clock::select_pll216_profile; -pub use dma_buffer::DcacheDisabledDmaMemory; pub use gpio::{ ActiveLowLedPd2, configure_exti3_pin_pc3, configure_exti4_pin_pa4, configure_i2c1_pins_pb8_pb9, configure_spi1_pins_pa5_pa6_pa7, configure_spi3_pins_pc10_pc11_pc12, configure_uart4_pins_pa0_pa1, configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9, - configure_usart2_pins_pa2_pa3, configure_usb_fs_pins_pa11_pa12, + configure_usart2_pins_pa2_pa3, configure_usart3_pins_pb10_pb11, configure_usart6_pins_pc6_pc7, + configure_usb_fs_pins_pa11_pa12, }; pub use i2c1::{I2c1, I2c1Config, I2c1Error}; pub use spi1::{Spi1, Spi1Error}; pub use spi3::{Spi3, Spi3Error}; -pub use tim2::{Tim2Periodic, Tim2PeriodicConfig}; -pub use time::DwtMicrosecondClock; -pub use usart::{ReceiveInterrupt, Uart4, Usart1Tx, Usart2Tx}; -pub use usart1_tx_dma2_stream7::Usart1TxDma; -pub use usart2_tx_dma1_stream6::Usart2TxDma; +pub use tim2::Tim2Periodic; +pub use time::TimerClock; +pub(crate) use usart::OwnedUart; +pub use usart::{ReceiveInterrupt, Uart4, Usart1Tx, Usart2Tx, Usart3, Usart6}; pub(crate) use usb::{F722UsbMavlink, usb_mavlink}; diff --git a/firmware/platforms/stm32f7/src/f722/peripherals/spi1.rs b/firmware/platforms/stm32f7/src/f722/peripherals/spi1.rs index 42ce3ac..308e660 100644 --- a/firmware/platforms/stm32f7/src/f722/peripherals/spi1.rs +++ b/firmware/platforms/stm32f7/src/f722/peripherals/spi1.rs @@ -1,44 +1,71 @@ //! STM32F722 SPI1 full-duplex DMA controller. //! //! SPI1 uses DMA2 Stream0/Channel3 for receive and DMA2 Stream3/Channel3 for -//! transmit. The receive terminal event is the transaction boundary because it -//! is generated after the last serial frame; transmit completion can occur -//! while the final serial clock is still in progress. +//! transmit. Receive completion requests finalization; the worker independently +//! proves both streams and the bus quiescent before copying out any bytes. +use core::sync::atomic::{AtomicBool, Ordering}; use kaptan_err_reportable::KaptanError; -use kaptan_spi::SpiCompletion; +use kaptan_spi::{ + CompletionError, SpiCompletion, + dma::{StartError, Storage, Transfer}, +}; use stm32f7::stm32f722; -const MAX_TRANSFER_LENGTH: usize = u16::MAX as usize; +use super::spi_dma; + +const MAX_TRANSFER_LENGTH: usize = 32; + +// The F722 linker places .bss in DMA-accessible SRAM1/2, not DTCM. Platform +// clock initialization disables D-cache before constructing any SPI route. +// This pair is never freed or reclaimed, even if Spi1 is dropped/forgotten. +static DMA_STORAGE: Storage = Storage::new(); +// Preserve errors even if an ISR clears their flags while finalization runs. +static FAULT_SEEN: AtomicBool = AtomicBool::new(false); /// Failure while submitting one SPI1 DMA transaction. #[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)] pub enum Spi1Error { - /// The transaction is malformed or exceeds the DMA counter range. + /// The transaction is empty or exceeds the route's 32-byte capacity. #[kaptan_error(id = 11501)] InvalidLength, /// A previous DMA transaction still owns SPI1. #[kaptan_error(id = 11502)] Busy, + /// Hardware shutdown could not prove quiescence; this route cannot restart. + #[kaptan_error(id = 11503)] + Quarantined, +} + +impl From for Spi1Error { + fn from(error: StartError) -> Self { + match error { + StartError::InvalidLength => Self::InvalidLength, + StartError::Busy => Self::Busy, + StartError::Quarantined => Self::Quarantined, + } + } } /// Owns SPI1 and its fixed full-duplex DMA2 request route. pub struct Spi1 { peripheral: stm32f722::SPI1, controller: stm32f722::DMA2, - active: bool, + // Transfer is Send through its one-time static owner; moving Spi1 never + // moves DMA bytes. ISR helpers touch status only and cannot reclaim it. + transfer: Transfer, } impl Spi1 { - /// Configures SPI1 mode 0 master operation at APB2/128 and reserves DMA2. - /// - /// This keeps the MPU6000 route below 1 MHz with the board's 108 MHz - /// APB2 clock while retaining substantial margin for board-level wiring. + /// Configures SPI1 mode 0 at APB2/128 (below 1 MHz) and reserves DMA2. pub fn new( rcc: &stm32f722::RCC, peripheral: stm32f722::SPI1, controller: stm32f722::DMA2, ) -> Self { + // Claim before touching hardware. A second construction fails closed, + // including after the original owner was dropped during a transfer. + let transfer = DMA_STORAGE.claim().expect("SPI1 storage already claimed"); rcc.apb2rstr().modify(|_, write| write.spi1rst().set_bit()); rcc.apb2rstr() .modify(|_, write| write.spi1rst().clear_bit()); @@ -63,24 +90,26 @@ impl Spi1 { Self { peripheral, controller, - active: false, + transfer, } } - /// Starts a full-duplex DMA transaction without polling a peripheral flag. - pub fn start_transfer(&mut self, transmit: &[u8], receive: &mut [u8]) -> Result<(), Spi1Error> { - if transmit.is_empty() - || transmit.len() != receive.len() - || transmit.len() > MAX_TRANSFER_LENGTH - { - return Err(Spi1Error::InvalidLength); - } - if self.active - || self.rx_stream().cr().read().en().is_enabled() + /// Checks admission before the electrical route asserts chip select. + pub(crate) fn check_start(&self, length: usize) -> Result<(), Spi1Error> { + self.transfer.check_start(length)?; + if self.rx_stream().cr().read().en().is_enabled() || self.tx_stream().cr().read().en().is_enabled() + || !spi_dma::bus_finished(&self.peripheral) { return Err(Spi1Error::Busy); } + Ok(()) + } + + /// Copies into permanent storage and starts DMA without waiting for flags. + pub fn start_transfer(&mut self, transmit: &[u8]) -> Result<(), Spi1Error> { + self.check_start(transmit.len())?; + let endpoints = self.transfer.begin(transmit)?; // Arm the complete DMA receive path before SPI starts generating // clocks. This avoids an RX FIFO overrun between transactions. @@ -90,19 +119,21 @@ impl Spi1 { self.clear_spi_overrun(); self.clear_rx_flags(); self.clear_tx_flags(); - self.configure_rx(receive); - self.configure_tx(transmit); + FAULT_SEEN.store(false, Ordering::Release); + self.configure_rx(endpoints.receive, endpoints.length); + self.configure_tx(endpoints.transmit, endpoints.length); self.peripheral.cr2().modify(|_, write| { let write = write.rxdmaen().set_bit().txdmaen().set_bit(); if transmit.len() & 1 == 0 { write.ldma_rx().even().ldma_tx().even() } else { - // With 8-bit frames, F7 SPI needs this for the final - // byte of an odd-length DMA transaction. MPU samples are - // one command byte plus fourteen data bytes. + // Mark the final byte of an odd-length DMA transfer. write.ldma_rx().odd().ldma_tx().odd() } }); + // begin supplied the compiler Release fence. With D-cache disabled, + // DSB commits SRAM writes and peripheral setup before DMA can access it. + cortex_m::asm::dsb(); self.rx_stream() .cr() .modify(|_, write| write.en().enabled()); @@ -112,34 +143,53 @@ impl Spi1 { self.peripheral .cr1() .modify(|_, write| write.spe().set_bit()); - self.active = true; Ok(()) } - /// Releases controller-side ownership after a DMA terminal event. - /// - /// Both streams are explicitly disabled for every outcome, including an - /// SPI peripheral fault. Disabling the SPI DMA requests first prevents a - /// new request while the stream hardware drains its enable bit. This - /// method deliberately does not wait for that bit: the next submission - /// observes it and returns `Busy` until the F7 DMA controller completes - /// the documented disable sequence. - pub fn complete_transfer(&mut self, _: SpiCompletion) { - self.peripheral - .cr2() - .modify(|_, write| write.rxdmaen().clear_bit().txdmaen().clear_bit()); - self.peripheral - .cr1() - .modify(|_, write| write.spe().clear_bit()); - self.rx_stream() - .cr() - .modify(|_, write| write.en().clear_bit()); - self.tx_stream() - .cr() - .modify(|_, write| write.en().clear_bit()); + /// Accepts a terminal notification only after bounded hardware shutdown. + pub fn complete_transfer( + &mut self, + completion: SpiCompletion, + receive: &mut [u8], + ) -> Result<(), CompletionError> { + // In particular, NotPending must not disable an active transfer. + self.transfer.check_completion(completion)?; + let outcome = match spi_dma::complete_transfer( + &self.peripheral, + self.rx_stream(), + self.tx_stream(), + completion, + || self.has_transfer_error(), + ) { + Ok(outcome) => outcome, + Err(error) => { + self.transfer.quarantine(); + return Err(error); + } + }; self.clear_rx_flags(); self.clear_tx_flags(); - self.active = false; + // SAFETY: Both EN bits were observed clear, TX FIFO empty and BSY=0 + // before disabling SPI. DMA requests remain disabled. The helper's DSB and + // finish's Acquire compiler fence order DMA writes before copy-out. + // Complete additionally requires both counters exhausted and no fault. + unsafe { self.transfer.finish(outcome, receive) } + } + + fn has_transfer_error(&self) -> bool { + let dma = self.controller.lisr().read(); + let spi = self.peripheral.sr().read(); + dma.teif0().is_error() + || dma.dmeif0().is_error() + || dma.feif0().is_error() + || dma.teif3().is_error() + || dma.dmeif3().is_error() + || dma.feif3().is_error() + || spi.ovr().is_overrun() + || spi.modf().is_fault() + || spi.fre().is_error() + || spi.crcerr().is_no_match() + || FAULT_SEEN.load(Ordering::Acquire) } /// Acknowledges an SPI peripheral overrun interrupt. @@ -153,6 +203,7 @@ impl Spi1 { if !peripheral.sr().read().ovr().is_overrun() { return false; } + FAULT_SEEN.store(true, Ordering::Release); let _ = peripheral.dr().read().bits(); let _ = peripheral.sr().read().bits(); true @@ -166,6 +217,9 @@ impl Spi1 { let status = controller.lisr().read(); let fault = status.teif0().is_error() || status.dmeif0().is_error() || status.feif0().is_error(); + if fault { + FAULT_SEEN.store(true, Ordering::Release); + } let complete = status.tcif0().is_complete(); controller.lifcr().write(|write| { write @@ -197,6 +251,9 @@ impl Spi1 { let status = controller.lisr().read(); let fault = status.teif3().is_error() || status.dmeif3().is_error() || status.feif3().is_error(); + if fault { + FAULT_SEEN.store(true, Ordering::Release); + } controller.lifcr().write(|write| { write .cfeif3() @@ -217,7 +274,7 @@ impl Spi1 { } } - fn configure_rx(&self, receive: &mut [u8]) { + fn configure_rx(&self, receive: *mut u8, length: usize) { self.rx_stream().cr().write(|write| { // SAFETY: Channel 3 is SPI1_RX on DMA2 Stream0; checked slice // length fits NDTR, and all values map directly to documented bits. @@ -250,18 +307,19 @@ impl Spi1 { self.rx_stream() .ndtr() // SAFETY: `start_transfer` bounds the length to the 16-bit NDTR field. - .write(|write| unsafe { write.bits(receive.len() as u32) }); + .write(|write| unsafe { write.bits(length as u32) }); self.rx_stream() .par() // SAFETY: `DR` is the memory-mapped SPI1 DMA peripheral endpoint. .write(|write| unsafe { write.bits(self.peripheral.dr().as_ptr() as u32) }); self.rx_stream() .m0ar() - // SAFETY: the caller retains `receive` until terminal completion. - .write(|write| unsafe { write.bits(receive.as_mut_ptr() as u32) }); + // SAFETY: begin reserved the exclusively claimed static RX slot. + // Its allocation survives owner drop/forget and cannot be reclaimed. + .write(|write| unsafe { write.bits(receive as u32) }); } - fn configure_tx(&self, transmit: &[u8]) { + fn configure_tx(&self, transmit: *const u8, length: usize) { self.tx_stream().cr().write(|write| { // SAFETY: Channel 3 is SPI1_TX on DMA2 Stream3; checked slice // length fits NDTR, and all values map directly to documented bits. @@ -292,15 +350,16 @@ impl Spi1 { self.tx_stream() .ndtr() // SAFETY: `start_transfer` bounds the length to the 16-bit NDTR field. - .write(|write| unsafe { write.bits(transmit.len() as u32) }); + .write(|write| unsafe { write.bits(length as u32) }); self.tx_stream() .par() // SAFETY: `DR` is the memory-mapped SPI1 DMA peripheral endpoint. .write(|write| unsafe { write.bits(self.peripheral.dr().as_ptr() as u32) }); self.tx_stream() .m0ar() - // SAFETY: the caller retains `transmit` until terminal completion. - .write(|write| unsafe { write.bits(transmit.as_ptr() as u32) }); + // SAFETY: begin copied into and reserved the permanent TX slot; + // no pointer to caller or movable driver storage reaches hardware. + .write(|write| unsafe { write.bits(transmit as u32) }); } fn rx_stream(&self) -> &stm32f722::dma2::ST { diff --git a/firmware/platforms/stm32f7/src/f722/peripherals/spi3.rs b/firmware/platforms/stm32f7/src/f722/peripherals/spi3.rs index 1ab3417..78103a9 100644 --- a/firmware/platforms/stm32f7/src/f722/peripherals/spi3.rs +++ b/firmware/platforms/stm32f7/src/f722/peripherals/spi3.rs @@ -1,44 +1,71 @@ //! STM32F722 SPI3 full-duplex DMA controller. //! //! SPI3 uses DMA1 Stream0/Channel0 for receive and DMA1 Stream5/Channel0 for -//! transmit. The receive terminal event is the transaction boundary because it -//! is generated after the last serial frame; transmit completion can occur -//! while the final serial clock is still in progress. +//! transmit. Receive completion requests finalization; the worker independently +//! proves both streams and the bus quiescent before copying out any bytes. +use core::sync::atomic::{AtomicBool, Ordering}; use kaptan_err_reportable::KaptanError; -use kaptan_spi::SpiCompletion; +use kaptan_spi::{ + CompletionError, SpiCompletion, + dma::{StartError, Storage, Transfer}, +}; use stm32f7::stm32f722; -const MAX_TRANSFER_LENGTH: usize = u16::MAX as usize; +use super::spi_dma; + +const MAX_TRANSFER_LENGTH: usize = 32; + +// The F722 linker places .bss in DMA-accessible SRAM1/2, not DTCM. Platform +// clock initialization disables D-cache before constructing any SPI route. +// This pair is never freed or reclaimed, even if Spi3 is dropped/forgotten. +static DMA_STORAGE: Storage = Storage::new(); +// Preserve errors even if an ISR clears their flags while finalization runs. +static FAULT_SEEN: AtomicBool = AtomicBool::new(false); /// Failure while submitting one SPI3 DMA transaction. #[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)] pub enum Spi3Error { - /// The transaction is malformed or exceeds the DMA counter range. + /// The transaction is empty or exceeds the route's 32-byte capacity. #[kaptan_error(id = 11601)] InvalidLength, /// A previous DMA transaction still owns SPI3. #[kaptan_error(id = 11602)] Busy, + /// Hardware shutdown could not prove quiescence; this route cannot restart. + #[kaptan_error(id = 11603)] + Quarantined, +} + +impl From for Spi3Error { + fn from(error: StartError) -> Self { + match error { + StartError::InvalidLength => Self::InvalidLength, + StartError::Busy => Self::Busy, + StartError::Quarantined => Self::Quarantined, + } + } } /// Owns SPI3 and its fixed full-duplex DMA1 request route. pub struct Spi3 { peripheral: stm32f722::SPI3, controller: stm32f722::DMA1, - active: bool, + // Transfer is Send through its one-time static owner; moving Spi3 never + // moves DMA bytes. ISR helpers touch status only and cannot reclaim it. + transfer: Transfer, } impl Spi3 { - /// Configures SPI3 mode 0 master operation at APB1/128 and reserves DMA1. - /// - /// This keeps the ICM-42688-P route below 1 MHz with the board's 54 MHz - /// APB1 clock while retaining substantial margin for board-level wiring. + /// Configures SPI3 mode 0 at APB1/128 (below 1 MHz) and reserves DMA1. pub fn new( rcc: &stm32f722::RCC, peripheral: stm32f722::SPI3, controller: stm32f722::DMA1, ) -> Self { + // Claim before touching hardware. A second construction fails closed, + // including after the original owner was dropped during a transfer. + let transfer = DMA_STORAGE.claim().expect("SPI3 storage already claimed"); rcc.apb1rstr().modify(|_, write| write.spi3rst().set_bit()); rcc.apb1rstr() .modify(|_, write| write.spi3rst().clear_bit()); @@ -63,24 +90,26 @@ impl Spi3 { Self { peripheral, controller, - active: false, + transfer, } } - /// Starts a full-duplex DMA transaction without polling a peripheral flag. - pub fn start_transfer(&mut self, transmit: &[u8], receive: &mut [u8]) -> Result<(), Spi3Error> { - if transmit.is_empty() - || transmit.len() != receive.len() - || transmit.len() > MAX_TRANSFER_LENGTH - { - return Err(Spi3Error::InvalidLength); - } - if self.active - || self.rx_stream().cr().read().en().is_enabled() + /// Checks admission before the electrical route asserts chip select. + pub(crate) fn check_start(&self, length: usize) -> Result<(), Spi3Error> { + self.transfer.check_start(length)?; + if self.rx_stream().cr().read().en().is_enabled() || self.tx_stream().cr().read().en().is_enabled() + || !spi_dma::bus_finished(&self.peripheral) { return Err(Spi3Error::Busy); } + Ok(()) + } + + /// Copies into permanent storage and starts DMA without waiting for flags. + pub fn start_transfer(&mut self, transmit: &[u8]) -> Result<(), Spi3Error> { + self.check_start(transmit.len())?; + let endpoints = self.transfer.begin(transmit)?; // Arm the complete DMA receive path before SPI starts generating // clocks. This avoids an RX FIFO overrun between transactions. @@ -90,19 +119,21 @@ impl Spi3 { self.clear_spi_overrun(); self.clear_rx_flags(); self.clear_tx_flags(); - self.configure_rx(receive); - self.configure_tx(transmit); + FAULT_SEEN.store(false, Ordering::Release); + self.configure_rx(endpoints.receive, endpoints.length); + self.configure_tx(endpoints.transmit, endpoints.length); self.peripheral.cr2().modify(|_, write| { let write = write.rxdmaen().set_bit().txdmaen().set_bit(); if transmit.len() & 1 == 0 { write.ldma_rx().even().ldma_tx().even() } else { - // With 8-bit frames, F7 SPI needs this for the final - // byte of an odd-length DMA transaction. IMU samples are - // one command byte plus fourteen data bytes. + // Mark the final byte of an odd-length DMA transfer. write.ldma_rx().odd().ldma_tx().odd() } }); + // begin supplied the compiler Release fence. With D-cache disabled, + // DSB commits SRAM writes and peripheral setup before DMA can access it. + cortex_m::asm::dsb(); self.rx_stream() .cr() .modify(|_, write| write.en().enabled()); @@ -112,34 +143,54 @@ impl Spi3 { self.peripheral .cr1() .modify(|_, write| write.spe().set_bit()); - self.active = true; Ok(()) } - /// Releases controller-side ownership after a DMA terminal event. - /// - /// Both streams are explicitly disabled for every outcome, including an - /// SPI peripheral fault. Disabling the SPI DMA requests first prevents a - /// new request while the stream hardware drains its enable bit. This - /// method deliberately does not wait for that bit: the next submission - /// observes it and returns `Busy` until the F7 DMA controller completes - /// the documented disable sequence. - pub fn complete_transfer(&mut self, _: SpiCompletion) { - self.peripheral - .cr2() - .modify(|_, write| write.rxdmaen().clear_bit().txdmaen().clear_bit()); - self.peripheral - .cr1() - .modify(|_, write| write.spe().clear_bit()); - self.rx_stream() - .cr() - .modify(|_, write| write.en().clear_bit()); - self.tx_stream() - .cr() - .modify(|_, write| write.en().clear_bit()); + /// Accepts a terminal notification only after bounded hardware shutdown. + pub fn complete_transfer( + &mut self, + completion: SpiCompletion, + receive: &mut [u8], + ) -> Result<(), CompletionError> { + // In particular, NotPending must not disable an active transfer. + self.transfer.check_completion(completion)?; + let outcome = match spi_dma::complete_transfer( + &self.peripheral, + self.rx_stream(), + self.tx_stream(), + completion, + || self.has_transfer_error(), + ) { + Ok(outcome) => outcome, + Err(error) => { + self.transfer.quarantine(); + return Err(error); + } + }; self.clear_rx_flags(); self.clear_tx_flags(); - self.active = false; + // SAFETY: Both EN bits were observed clear, TX FIFO empty and BSY=0 + // before disabling SPI. DMA requests remain disabled. The helper's DSB and + // finish's Acquire compiler fence order DMA writes before copy-out. + // Complete additionally requires both counters exhausted and no fault. + unsafe { self.transfer.finish(outcome, receive) } + } + + fn has_transfer_error(&self) -> bool { + let rx = self.controller.lisr().read(); + let tx = self.controller.hisr().read(); + let spi = self.peripheral.sr().read(); + rx.teif0().is_error() + || rx.dmeif0().is_error() + || rx.feif0().is_error() + || tx.teif5().is_error() + || tx.dmeif5().is_error() + || tx.feif5().is_error() + || spi.ovr().is_overrun() + || spi.modf().is_fault() + || spi.fre().is_error() + || spi.crcerr().is_no_match() + || FAULT_SEEN.load(Ordering::Acquire) } /// Acknowledges an SPI peripheral overrun interrupt. @@ -153,6 +204,7 @@ impl Spi3 { if !peripheral.sr().read().ovr().is_overrun() { return false; } + FAULT_SEEN.store(true, Ordering::Release); let _ = peripheral.dr().read().bits(); let _ = peripheral.sr().read().bits(); true @@ -166,6 +218,9 @@ impl Spi3 { let status = controller.lisr().read(); let fault = status.teif0().is_error() || status.dmeif0().is_error() || status.feif0().is_error(); + if fault { + FAULT_SEEN.store(true, Ordering::Release); + } let complete = status.tcif0().is_complete(); controller.lifcr().write(|write| { write @@ -197,6 +252,9 @@ impl Spi3 { let status = controller.hisr().read(); let fault = status.teif5().is_error() || status.dmeif5().is_error() || status.feif5().is_error(); + if fault { + FAULT_SEEN.store(true, Ordering::Release); + } controller.hifcr().write(|write| { write .cfeif5() @@ -217,7 +275,7 @@ impl Spi3 { } } - fn configure_rx(&self, receive: &mut [u8]) { + fn configure_rx(&self, receive: *mut u8, length: usize) { self.rx_stream().cr().write(|write| { // SAFETY: Channel 0 is SPI3_RX on DMA1 Stream0; checked slice // length fits NDTR, and all values map directly to documented bits. @@ -250,18 +308,19 @@ impl Spi3 { self.rx_stream() .ndtr() // SAFETY: `start_transfer` bounds the length to the 16-bit NDTR field. - .write(|write| unsafe { write.bits(receive.len() as u32) }); + .write(|write| unsafe { write.bits(length as u32) }); self.rx_stream() .par() // SAFETY: `DR` is the memory-mapped SPI3 DMA peripheral endpoint. .write(|write| unsafe { write.bits(self.peripheral.dr().as_ptr() as u32) }); self.rx_stream() .m0ar() - // SAFETY: the caller retains `receive` until terminal completion. - .write(|write| unsafe { write.bits(receive.as_mut_ptr() as u32) }); + // SAFETY: begin reserved the exclusively claimed static RX slot. + // Its allocation survives owner drop/forget and cannot be reclaimed. + .write(|write| unsafe { write.bits(receive as u32) }); } - fn configure_tx(&self, transmit: &[u8]) { + fn configure_tx(&self, transmit: *const u8, length: usize) { self.tx_stream().cr().write(|write| { // SAFETY: Channel 0 is SPI3_TX on DMA1 Stream5; checked slice // length fits NDTR, and all values map directly to documented bits. @@ -292,15 +351,16 @@ impl Spi3 { self.tx_stream() .ndtr() // SAFETY: `start_transfer` bounds the length to the 16-bit NDTR field. - .write(|write| unsafe { write.bits(transmit.len() as u32) }); + .write(|write| unsafe { write.bits(length as u32) }); self.tx_stream() .par() // SAFETY: `DR` is the memory-mapped SPI3 DMA peripheral endpoint. .write(|write| unsafe { write.bits(self.peripheral.dr().as_ptr() as u32) }); self.tx_stream() .m0ar() - // SAFETY: the caller retains `transmit` until terminal completion. - .write(|write| unsafe { write.bits(transmit.as_ptr() as u32) }); + // SAFETY: begin copied into and reserved the permanent TX slot; + // no pointer to caller or movable driver storage reaches hardware. + .write(|write| unsafe { write.bits(transmit as u32) }); } fn rx_stream(&self) -> &stm32f722::dma1::ST { diff --git a/firmware/platforms/stm32f7/src/f722/peripherals/spi_dma.rs b/firmware/platforms/stm32f7/src/f722/peripherals/spi_dma.rs new file mode 100644 index 0000000..c8c0ca9 --- /dev/null +++ b/firmware/platforms/stm32f7/src/f722/peripherals/spi_dma.rs @@ -0,0 +1,53 @@ +use kaptan_spi::{CompletionError, SpiCompletion}; +use stm32f7::stm32f722::{dma2, spi1}; + +const QUIESCE_POLLS: usize = 1024; + +pub(super) fn bus_finished(spi: &spi1::RegisterBlock) -> bool { + let status = spi.sr().read(); + status.ftlvl().is_empty() && status.bsy().is_not_busy() +} + +pub(super) fn complete_transfer( + spi: &spi1::RegisterBlock, + rx: &dma2::ST, + tx: &dma2::ST, + completion: SpiCompletion, + has_fault: impl FnOnce() -> bool, +) -> Result { + spi.cr2() + .modify(|_, write| write.rxdmaen().clear_bit().txdmaen().clear_bit()); + rx.cr().modify(|_, write| write.en().clear_bit()); + tx.cr().modify(|_, write| write.en().clear_bit()); + // Leave SPI enabled while queued frames drain. Writing EN=0 does not + // establish that DMA stopped or that the final SPI clock completed. + let quiescent = (0..QUIESCE_POLLS).any(|_| { + rx.cr().read().en().is_disabled() && tx.cr().read().en().is_disabled() && bus_finished(spi) + }); + spi.cr1().modify(|_, write| write.spe().clear_bit()); + cortex_m::asm::dsb(); + if !quiescent { + return Err(CompletionError::Quarantined); + } + let complete = completion == SpiCompletion::Complete + && rx.ndtr().read().bits() == 0 + && tx.ndtr().read().bits() == 0 + && spi.sr().read().frlvl().is_empty() + && !has_fault(); + // Cancellation can leave old frames in the four-byte RX FIFO. Check + // faults before draining: the DR/SR reads can clear an overrun. + for _ in 0..4 { + if spi.sr().read().frlvl().is_empty() { + break; + } + let _ = spi.dr().read().bits(); + } + if !spi.sr().read().frlvl().is_empty() { + return Err(CompletionError::Quarantined); + } + Ok(if complete { + SpiCompletion::Complete + } else { + SpiCompletion::TransferFault + }) +} diff --git a/firmware/platforms/stm32f7/src/f722/peripherals/tim2.rs b/firmware/platforms/stm32f7/src/f722/peripherals/tim2.rs index bc388b6..0ad3bb3 100644 --- a/firmware/platforms/stm32f7/src/f722/peripherals/tim2.rs +++ b/firmware/platforms/stm32f7/src/f722/peripherals/tim2.rs @@ -1,56 +1,123 @@ -//! STM32F722 TIM2 periodic interrupt configuration. +//! One owned TIM2 counter for deadlines and 100 Hz CC1 service interrupts. +use cortex_m::peripheral::NVIC; use stm32f7::stm32f722; -/// Precomputed TIM2 periodic-update configuration. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct Tim2PeriodicConfig { - /// Timer prescaler register value. - pub prescaler: u16, - /// Timer auto-reload register value. - pub auto_reload: u32, +use super::time::{SERVICE_PERIOD_US, SharedTimer, TimerBackend, TimerClock}; + +static TIMER: SharedTimer = SharedTimer::new(); + +/// Service-IRQ handle for the same counter and extension as every TimerClock. +pub struct Tim2Periodic { + shared: &'static SharedTimer, } -/// TIM2 configured to issue periodic update interrupts. -pub struct Tim2Periodic { - _peripheral: stm32f722::TIM2, +pub(super) struct Tim2Counter { + peripheral: stm32f722::TIM2, } impl Tim2Periodic { - /// Enables TIM2 with the supplied platform-derived period configuration. - pub fn new( + /// Starts the fixed 1 MHz clock, leaving its IRQ disabled until RTIC init + /// has assembled every resource. Ownership moves into permanent storage. + pub(crate) fn new( rcc: &stm32f722::RCC, peripheral: stm32f722::TIM2, - config: Tim2PeriodicConfig, - ) -> Self { + dbgmcu: &stm32f722::DBGMCU, + ) -> Option<(TimerClock, Self)> { + if !TIMER.initialize(|| Tim2Counter::new(rcc, peripheral, dbgmcu)) { + return None; + } + Some((TimerClock { shared: &TIMER }, Self { shared: &TIMER })) + } + + /// Called last in successful RTIC init, with PRIMASK set. The init tail + /// restores TIM2's NVIC enable before RTIC publishes resources and releases + /// PRIMASK. Partial initialization never enables this peripheral interrupt. + pub fn enable_interrupt(&mut self) { + self.shared.enable_interrupt(); + } + + /// Returns true only for CC1IF. Compare rearming and epoch extension finish + /// before the caller may notify workers or invoke any callbacks. + pub fn on_interrupt(&mut self) -> bool { + self.shared.on_interrupt() + } +} + +impl Tim2Counter { + fn new(rcc: &stm32f722::RCC, peripheral: stm32f722::TIM2, dbgmcu: &stm32f722::DBGMCU) -> Self { + NVIC::mask(stm32f722::Interrupt::TIM2); + rcc.apb1enr().modify(|_, write| write.tim2en().set_bit()); + let _ = rcc.apb1enr().read(); rcc.apb1rstr().modify(|_, write| write.tim2rst().set_bit()); rcc.apb1rstr() .modify(|_, write| write.tim2rst().clear_bit()); - rcc.apb1enr().modify(|_, write| write.tim2en().set_bit()); - peripheral.cr1().modify(|_, write| write.cen().clear_bit()); - peripheral.psc().write(|write| { - // SAFETY: `prescaler` is exactly the 16-bit PSC register width. - unsafe { write.psc().bits(config.prescaler) } - }); - peripheral.arr().write(|write| { - // SAFETY: TIM2 ARR is a 32-bit register and accepts `auto_reload`. - unsafe { write.arr().bits(config.auto_reload) } - }); + rcc.apb1lpenr() + .modify(|_, write| write.tim2lpen().set_bit()); + dbgmcu + .apb1_fz() + .modify(|_, write| write.dbg_tim2_stop().clear_bit()); + + // Reset values select internal clock, continuous edge-aligned upcount, + // UIFREMAP=0, frozen output compare, no preload and no GPIO output. + peripheral.cr1().reset(); + peripheral.cr2().reset(); + peripheral.smcr().reset(); + peripheral.ccmr1_output().reset(); + peripheral.ccer().reset(); + peripheral.dier().reset(); + // TIMPRE=0, HCLK=216 MHz, APB1=/4: TIM2 input is 108 MHz. + peripheral.psc().write(|write| write.psc().set(107)); + peripheral.arr().write(|write| write.arr().set(u32::MAX)); + // Latch PSC while stopped, then establish the shared zero epoch and + // first compare before publishing handles or enabling any interrupt. peripheral.egr().write(|write| write.ug().set_bit()); - // TIM2 status flags are write-zero-to-clear. Use the PAC's explicit - // W0C operation rather than a read-modify-write after `EGR.UG`. - peripheral.sr().write(|write| write.uif().clear()); - peripheral.dier().modify(|_, write| write.uie().set_bit()); + peripheral.cnt().write(|write| write.cnt().set(0)); + peripheral + .ccr1() + .write(|write| write.ccr().set(SERVICE_PERIOD_US)); + peripheral.sr().reset(); + NVIC::unpend(stm32f722::Interrupt::TIM2); peripheral.cr1().modify(|_, write| write.cen().set_bit()); + Self { peripheral } + } +} + +impl TimerBackend for Tim2Counter { + fn counter(&mut self) -> u32 { + self.peripheral.cnt().read().cnt().bits() + } + + fn compare_pending(&mut self) -> bool { + self.peripheral.sr().read().cc1if().bit_is_set() + } + + fn acknowledge_compare(&mut self) { + // PAC 0.16 SR::ZERO_TO_MODIFY_FIELDS_BITMAP=0x1e5f makes write() + // preserve all other W0C flags. No SR read-modify-write and no later + // blanket clear that could erase a newly raised compare flag. + self.peripheral.sr().write(|write| write.cc1if().clear()); + cortex_m::asm::dsb(); + } + + fn set_compare(&mut self, counter: u32) { + self.peripheral + .ccr1() + .write(|write| write.ccr().set(counter)); + // Complete the APB write before the shared mechanism verifies CNT. + cortex_m::asm::dsb(); + } - Self { - _peripheral: peripheral, - } + fn enable_compare_interrupt(&mut self) { + // CC1IE only. UIE and every DMA request remain disabled. + self.peripheral + .dier() + .write(|write| write.cc1ie().set_bit()); + cortex_m::asm::dsb(); } - /// Acknowledges a TIM2 update interrupt. - pub fn acknowledge_update(&mut self) { - // `UIF` is write-zero-to-clear (W0C). - self._peripheral.sr().write(|write| write.uif().clear()); + fn pend_interrupt(&mut self) { + NVIC::pend(stm32f722::Interrupt::TIM2); + cortex_m::asm::dsb(); } } diff --git a/firmware/platforms/stm32f7/src/f722/peripherals/time.rs b/firmware/platforms/stm32f7/src/f722/peripherals/time.rs index b132243..1ce533d 100644 --- a/firmware/platforms/stm32f7/src/f722/peripherals/time.rs +++ b/firmware/platforms/stm32f7/src/f722/peripherals/time.rs @@ -1,106 +1,166 @@ -//! DWT-backed monotonic microsecond clock for STM32F722. +//! Shared TIM2 microsecond epoch and compare-service mechanism. -use cortex_m::peripheral::{DCB, DWT}; -use kaptan_err_reportable::KaptanError; +use core::cell::RefCell; +use critical_section::Mutex; use kaptan_time::{MonotonicClock, Timestamp}; -/// A DWT cycle counter extended into monotonic microseconds. +use super::tim2::Tim2Counter; + +pub(super) const SERVICE_PERIOD_US: u32 = 10_000; +// A missed compare must not turn into a wait for the next 32-bit wrap. +const REARM_ATTEMPTS: usize = 2; + +/// A handle to the platform's single TIM2-backed microsecond epoch. /// -/// `now` must be called at least once during every 32-bit cycle-counter wrap -/// interval. At the Kakute reset clock this is approximately 20 seconds; the -/// platform periodic service calls it much more frequently. -pub struct DwtMicrosecondClock { - cycles_per_microsecond: u32, - previous_cycles: u32, - elapsed_cycles: u64, +/// Copies share both the counter and its 64-bit extension. Every CNT read and +/// extension runs inside the existing single-core PRIMASK critical section. +/// Normal CC1 service observes the counter every 10 ms, even for silent readers. +/// All observations combined must be less than 2^32 microseconds apart +/// (approximately 71.58 minutes). NMI and fault handlers must not use this clock. +/// +/// Ordinary WFI sleep is supported by the fixed platform clock configuration. +/// Deep sleep, counter resets, clock changes and debugger halts spanning a full +/// counter wrap are unsupported and require a platform reset. CNT alone cannot +/// detect these violations. The manual's architecture chapter describes the +/// monotonic-time and service-IRQ contract. +#[derive(Clone, Copy)] +pub struct TimerClock { + pub(super) shared: &'static SharedTimer, } -/// Invalid fixed clock configuration for the DWT timestamp source. -#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)] -pub enum DwtClockStartError { - /// A zero-megahertz core clock cannot produce microsecond timestamps. - #[kaptan_error(id = 11101)] - ZeroFrequency, -} - -impl DwtMicrosecondClock { - /// Enables the DWT cycle counter for a fixed integer-MHz core clock. - /// - /// `MEGAHERTZ` must match the platform's core clock. This function is - /// called once during platform initialization before interrupts are enabled. - pub fn start( - dcb: &mut DCB, - dwt: &mut DWT, - ) -> Result { - if MEGAHERTZ == 0 { - return Err(DwtClockStartError::ZeroFrequency); - } - - dcb.enable_trace(); - DWT::unlock(); - dwt.set_cycle_count(0); - dwt.enable_cycle_counter(); - - Ok(Self { - cycles_per_microsecond: MEGAHERTZ, - previous_cycles: 0, - elapsed_cycles: 0, - }) - } - - /// Captures the current monotonic microsecond timestamp. +impl TimerClock { + /// Captures a timestamp in the one shared microsecond epoch. pub fn now(&mut self) -> Timestamp { - self.advance_to(DWT::cycle_count()) - } - - /// Creates another independent reader for the same already-running DWT - /// counter. Each reader must be serviced often enough to observe wraps. - pub const fn fork(&self) -> Self { - Self { - cycles_per_microsecond: self.cycles_per_microsecond, - previous_cycles: self.previous_cycles, - elapsed_cycles: self.elapsed_cycles, - } - } - - fn advance_to(&mut self, current_cycles: u32) -> Timestamp { - let elapsed = current_cycles.wrapping_sub(self.previous_cycles); - self.previous_cycles = current_cycles; - self.elapsed_cycles = self.elapsed_cycles.saturating_add(u64::from(elapsed)); - - Timestamp::from_microseconds(self.elapsed_cycles / u64::from(self.cycles_per_microsecond)) + self.shared.now() } } -impl MonotonicClock for DwtMicrosecondClock { +impl MonotonicClock for TimerClock { fn now(&mut self) -> Timestamp { Self::now(self) } } -#[cfg(test)] -mod tests { - use super::DwtMicrosecondClock; +/// The production PAC owner and injected test counter use this same mechanism. +pub(super) trait TimerBackend { + fn counter(&mut self) -> u32; + fn compare_pending(&mut self) -> bool; + fn acknowledge_compare(&mut self); + fn set_compare(&mut self, counter: u32); + fn enable_compare_interrupt(&mut self); + fn pend_interrupt(&mut self); +} - fn clock(previous_cycles: u32) -> DwtMicrosecondClock { - DwtMicrosecondClock { - cycles_per_microsecond: 16, - previous_cycles, - elapsed_cycles: 0, +pub(super) struct SharedTimer { + state: Mutex>>>, +} + +struct TimerState { + backend: B, + prev_cnt: u32, + elapsed_u64: u64, + rearm_pending: bool, +} + +impl SharedTimer { + pub(super) const fn new() -> Self { + Self { + state: Mutex::new(RefCell::new(None)), } } - #[test] - fn converts_cycles_to_microseconds() { - let mut clock = clock(0); - - assert_eq!(clock.advance_to(160).as_microseconds(), 10); + /// Publishes the owned backend once. A second call does not touch hardware. + /// The factory starts CNT at zero with compare interrupts still disabled. + pub(super) fn initialize(&self, make_backend: impl FnOnce() -> B) -> bool { + critical_section::with(|cs| { + let mut slot = self.state.borrow(cs).borrow_mut(); + if slot.is_some() { + return false; + } + *slot = Some(TimerState { + backend: make_backend(), + prev_cnt: 0, + elapsed_u64: 0, + rearm_pending: false, + }); + true + }) } - #[test] - fn extends_a_single_cycle_counter_wrap() { - let mut clock = clock(u32::MAX - 15); + fn with(&self, f: impl FnOnce(&mut TimerState) -> R) -> R { + critical_section::with(|cs| { + f(self + .state + .borrow(cs) + .borrow_mut() + .as_mut() + .expect("timer handles are published only after initialization")) + }) + } - assert_eq!(clock.advance_to(16).as_microseconds(), 2); + pub(super) fn now(&self) -> Timestamp { + // Read CNT here, after exclusion. Sampling before this lock would allow + // an IRQ to advance prev_cnt past the sample and fabricate a full wrap. + self.with(|state| Timestamp::from_microseconds(state.sample())) + } + + pub(super) fn enable_interrupt(&self) { + self.with(|state| { + state.backend.acknowledge_compare(); + state.rearm(); + state.backend.enable_compare_interrupt(); + }); + } + + /// Rearms before returning permission to notify service consumers. + pub(super) fn on_interrupt(&self) -> bool { + self.with(|state| { + let event = state.backend.compare_pending(); + if event { + // Acknowledge only the old CC1IF, before any CCR write. Flags + // raised during rearming remain pending for the next IRQ. + state.backend.acknowledge_compare(); + } else if !state.rearm_pending { + return false; + } + state.rearm(); + // A software recovery pend or stale NVIC pend is not a timer event. + event + }) } } + +impl TimerState { + fn sample(&mut self) -> u64 { + let cnt = self.backend.counter(); + self.elapsed_u64 += u64::from(cnt.wrapping_sub(self.prev_cnt)); + self.prev_cnt = cnt; + self.elapsed_u64 + } + + fn rearm(&mut self) { + let mut now = self.sample(); + for _ in 0..REARM_ATTEMPTS { + // First strictly future point on the epoch's 100 Hz grid. This + // skips any number of missed periods in O(1), without replay. + let period = u64::from(SERVICE_PERIOD_US); + let next = now + (period - now % period); + // CNT and the epoch started at zero. CCR uses the low 32 bits. + self.backend.set_compare(next as u32); + now = self.sample(); + if now < next { + self.rearm_pending = false; + return; + } + } + // The target passed during both writes (for example under a debugger). + // Retry on a software-pended IRQ even if neither compare raised CC1IF. + // Each entry is bounded and recovery alone never notifies consumers. + self.rearm_pending = true; + self.backend.pend_interrupt(); + } +} + +#[cfg(test)] +#[path = "../../../tests/support/timer_clock.rs"] +mod tests; diff --git a/firmware/platforms/stm32f7/src/f722/peripherals/usart.rs b/firmware/platforms/stm32f7/src/f722/peripherals/usart.rs index 6718030..8f26d79 100644 --- a/firmware/platforms/stm32f7/src/f722/peripherals/usart.rs +++ b/firmware/platforms/stm32f7/src/f722/peripherals/usart.rs @@ -9,7 +9,7 @@ pub enum BaudRateError { /// Zero is not a valid baud rate. #[kaptan_error(id = 11201)] Zero, - /// The requested divisor cannot fit USART BRR. + /// The requested divisor is outside the USART BRR range 16..=65535. #[kaptan_error(id = 11202)] OutOfRange, } @@ -51,57 +51,18 @@ impl Usart2Tx { ) -> Result { let brr = calculate_brr(peripheral_clock_hz, baud)?; - // Do not inherit a bootloader's USART2 kernel-clock selection. The - // F722 flight clock profile supplies `peripheral_clock_hz` from PCLK1, - // so select PCLK1 before programming BRR. + // Select the PCLK1 source used to calculate BRR. rcc.dckcfgr2().modify(|_, write| write.usart2sel().apb1()); rcc.apb1rstr() .modify(|_, write| write.usart2rst().set_bit()); rcc.apb1rstr() .modify(|_, write| write.usart2rst().clear_bit()); rcc.apb1enr().modify(|_, write| write.usart2en().set_bit()); - peripheral.brr().write(|write| { - // SAFETY: `brr` is validated to fit the complete BRR register. - unsafe { write.bits(u32::from(brr)) } - }); - peripheral.cr1().write(|write| { - write - .te() - .set_bit() - .re() - .set_bit() - .rxneie() - .set_bit() - .ue() - .set_bit() - }); - peripheral.cr2().write(|write| write.stop().stop1()); + let _ = rcc.apb1enr().read(); + configure_usart(&peripheral, brr); Ok(Self { peripheral }) } - - pub(crate) fn enable_dma_transmit(&self) { - self.peripheral.cr3().write(|write| write.dmat().set_bit()); - } - - pub(crate) fn disable_dma_transmit_from_interrupt() { - // SAFETY: the fixed USART2 DMA interrupt route is the sole TX-DMA - // owner. Receive interrupts never modify CR3. - let peripheral = unsafe { &*stm32f722::USART2::ptr() }; - peripheral.cr3().modify(|_, write| write.dmat().clear_bit()); - } - - pub(crate) fn transmit_data_register_address(&self) -> u32 { - self.peripheral.tdr().as_ptr() as u32 - } - - /// Acknowledges one USART2 receive interrupt without accessing TX state. - pub fn acknowledge_receive_interrupt() -> ReceiveInterrupt { - // SAFETY: the platform binds USART2 exactly once and this function touches - // only its receive/error registers, never the TX DMA configuration. - let peripheral = unsafe { &*stm32f722::USART2::ptr() }; - acknowledge_receive_interrupt(peripheral) - } } impl Usart1Tx { @@ -114,69 +75,51 @@ impl Usart1Tx { ) -> Result { let brr = calculate_brr(peripheral_clock_hz, baud)?; - // Do not inherit a bootloader or earlier image's USART1 kernel-clock - // selection. The F722 platform passes PCLK2 when calculating BRR, so - // select it explicitly before programming the divisor. + // Select the PCLK2 source used to calculate BRR. rcc.dckcfgr2().modify(|_, write| write.usart1sel().apb2()); rcc.apb2rstr() .modify(|_, write| write.usart1rst().set_bit()); rcc.apb2rstr() .modify(|_, write| write.usart1rst().clear_bit()); rcc.apb2enr().modify(|_, write| write.usart1en().set_bit()); - peripheral.brr().write(|write| { - // SAFETY: `brr` is validated to fit the complete BRR register. - unsafe { write.bits(u32::from(brr)) } - }); - peripheral.cr1().write(|write| { - write - .te() - .set_bit() - .re() - .set_bit() - .rxneie() - .set_bit() - .ue() - .set_bit() - }); - peripheral.cr2().write(|write| write.stop().stop1()); + let _ = rcc.apb2enr().read(); + configure_usart(&peripheral, brr); Ok(Self { peripheral }) } +} - pub(crate) fn enable_dma_transmit(&self) { - self.peripheral.cr3().write(|write| write.dmat().set_bit()); - } +/// Owns configured full-duplex USART3 hardware. +pub struct Usart3 { + peripheral: stm32f722::USART3, +} - pub(crate) fn disable_dma_transmit_from_interrupt() { - // SAFETY: the fixed USART1 DMA interrupt route is the sole TX-DMA - // owner. Receive interrupts never modify CR3. - let peripheral = unsafe { &*stm32f722::USART1::ptr() }; - peripheral.cr3().modify(|_, write| write.dmat().clear_bit()); - } - - pub(crate) fn transmit_data_register_address(&self) -> u32 { - self.peripheral.tdr().as_ptr() as u32 - } - - /// Acknowledges one USART1 receive interrupt without accessing TX state. - /// - /// The platform invokes this only from the `USART1` interrupt. USART1 TX DMA - /// owns its disjoint data register and DMA stream, so this static route may - /// safely acknowledge RX while that transfer is active. - pub fn acknowledge_receive_interrupt() -> ReceiveInterrupt { - // SAFETY: the platform binds USART1 exactly once and this function touches - // only its receive/error registers, never the TX DMA configuration. - let peripheral = unsafe { &*stm32f722::USART1::ptr() }; - acknowledge_receive_interrupt(peripheral) +impl Usart3 { + /// Enables USART3 with 8N1 framing after platform pin setup. + pub fn new( + rcc: &stm32f722::RCC, + peripheral: stm32f722::USART3, + peripheral_clock_hz: u32, + baud: u32, + ) -> Result { + let brr = calculate_brr(peripheral_clock_hz, baud)?; + rcc.dckcfgr2().modify(|_, write| write.usart3sel().apb1()); + rcc.apb1rstr() + .modify(|_, write| write.usart3rst().set_bit()); + rcc.apb1rstr() + .modify(|_, write| write.usart3rst().clear_bit()); + rcc.apb1enr().modify(|_, write| write.usart3en().set_bit()); + let _ = rcc.apb1enr().read(); + configure_usart(&peripheral, brr); + Ok(Self { peripheral }) } } /// Owns configured UART4 hardware. /// -/// This route is intended for peripherals that continuously stream data, such -/// as the MTF-01. It does not enable DMA state. +/// Uses interrupt-driven receive and transmit without DMA. pub struct Uart4 { - _peripheral: stm32f722::UART4, + peripheral: stm32f722::UART4, } impl Uart4 { @@ -189,38 +132,103 @@ impl Uart4 { ) -> Result { let brr = calculate_brr(peripheral_clock_hz, baud)?; + rcc.dckcfgr2().modify(|_, write| write.uart4sel().apb1()); rcc.apb1rstr().modify(|_, write| write.uart4rst().set_bit()); rcc.apb1rstr() .modify(|_, write| write.uart4rst().clear_bit()); rcc.apb1enr().modify(|_, write| write.uart4en().set_bit()); - peripheral.brr().write(|write| { - // SAFETY: `brr` is validated to fit the complete BRR register. - unsafe { write.bits(u32::from(brr)) } - }); - peripheral.cr1().write(|write| { - write - .te() - .set_bit() - .re() - .set_bit() - .rxneie() - .set_bit() - .ue() - .set_bit() - }); - peripheral.cr2().write(|write| write.stop().stop1()); + let _ = rcc.apb1enr().read(); + configure_usart(&peripheral, brr); - Ok(Self { - _peripheral: peripheral, - }) + Ok(Self { peripheral }) } +} - /// Acknowledges one UART4 receive interrupt. - pub fn acknowledge_receive_interrupt() -> ReceiveInterrupt { - // SAFETY: the platform binds UART4 exactly once and this function - // accesses only its receive and error registers. - let peripheral = unsafe { &*stm32f722::UART4::ptr() }; - acknowledge_receive_interrupt(peripheral) +/// Owns configured full-duplex USART6 hardware. +pub struct Usart6 { + peripheral: stm32f722::USART6, +} + +impl Usart6 { + /// Enables USART6 with 8N1 framing after platform pin setup. + pub fn new( + rcc: &stm32f722::RCC, + peripheral: stm32f722::USART6, + peripheral_clock_hz: u32, + baud: u32, + ) -> Result { + let brr = calculate_brr(peripheral_clock_hz, baud)?; + rcc.dckcfgr2().modify(|_, write| write.usart6sel().apb2()); + rcc.apb2rstr() + .modify(|_, write| write.usart6rst().set_bit()); + rcc.apb2rstr() + .modify(|_, write| write.usart6rst().clear_bit()); + rcc.apb2enr().modify(|_, write| write.usart6en().set_bit()); + let _ = rcc.apb2enr().read(); + configure_usart(&peripheral, brr); + Ok(Self { peripheral }) + } +} + +fn configure_usart(peripheral: &stm32f722::usart1::RegisterBlock, brr: u16) { + peripheral.brr().write(|write| { + // SAFETY: `brr` is validated to fit the complete BRR register. + unsafe { write.bits(u32::from(brr)) } + }); + peripheral.cr2().write(|write| write.stop().stop1()); + peripheral.cr1().write(|write| { + write + .te() + .set_bit() + .re() + .set_bit() + .rxneie() + .set_bit() + .ue() + .set_bit() + }); +} + +/// Exclusive PAC owners for the configurable serial path. No DMA controller, +/// static register alias or borrowed transmit buffer crosses this interface. +pub(crate) enum OwnedUart { + Uart1(Usart1Tx), + Uart2(Usart2Tx), + Uart3(Usart3), + Uart4(Uart4), + Uart6(Usart6), +} +impl OwnedUart { + fn registers(&mut self) -> &stm32f722::usart1::RegisterBlock { + match self { + Self::Uart1(uart) => &uart.peripheral, + Self::Uart2(uart) => &uart.peripheral, + Self::Uart3(uart) => &uart.peripheral, + Self::Uart4(uart) => &uart.peripheral, + Self::Uart6(uart) => &uart.peripheral, + } + } + pub(crate) fn receive(&mut self) -> ReceiveInterrupt { + acknowledge_receive_interrupt(self.registers()) + } +} +impl super::super::serial::UartIo for OwnedUart { + fn tx_ready(&mut self) -> bool { + self.registers().isr().read().txe().bit_is_set() + } + fn tx_complete(&mut self) -> bool { + self.registers().isr().read().tc().bit_is_set() + } + fn write_byte(&mut self, byte: u8) { + // SAFETY: every u8 fits the configured eight-bit transmit data field. + self.registers() + .tdr() + .write(|write| unsafe { write.tdr().bits(u16::from(byte)) }); + } + fn interrupts(&mut self, ready: bool, complete: bool) { + self.registers() + .cr1() + .modify(|_, write| write.txeie().bit(ready).tcie().bit(complete)); } } @@ -263,16 +271,16 @@ fn clear_receive_errors(peripheral: &stm32f722::usart1::RegisterBlock) { }); } -const fn calculate_brr(peripheral_clock_hz: u32, baud: u32) -> Result { +fn calculate_brr(peripheral_clock_hz: u32, baud: u32) -> Result { if baud == 0 { return Err(BaudRateError::Zero); } - let divisor = (peripheral_clock_hz + (baud / 2)) / baud; - if divisor == 0 || divisor > u16::MAX as u32 { + let divisor = (u64::from(peripheral_clock_hz) + u64::from(baud) / 2) / u64::from(baud); + if divisor < 16 { Err(BaudRateError::OutOfRange) } else { - Ok(divisor as u16) + u16::try_from(divisor).map_err(|_| BaudRateError::OutOfRange) } } @@ -289,4 +297,12 @@ mod tests { fn rejects_zero_baud() { assert_eq!(calculate_brr(16_000_000, 0), Err(BaudRateError::Zero)); } + + #[test] + fn brr_rounding_does_not_overflow() { + assert_eq!(calculate_brr(u32::MAX, 115_200), Ok(37_283)); + assert_eq!(calculate_brr(u32::MAX, 1), Err(BaudRateError::OutOfRange)); + assert_eq!(calculate_brr(15_000, 1_000), Err(BaudRateError::OutOfRange)); + assert_eq!(calculate_brr(16_000, 1_000), Ok(16)); + } } diff --git a/firmware/platforms/stm32f7/src/f722/peripherals/usart1_tx_dma2_stream7.rs b/firmware/platforms/stm32f7/src/f722/peripherals/usart1_tx_dma2_stream7.rs deleted file mode 100644 index 829ea80..0000000 --- a/firmware/platforms/stm32f7/src/f722/peripherals/usart1_tx_dma2_stream7.rs +++ /dev/null @@ -1,222 +0,0 @@ -//! Fixed STM32F722 DMA route for USART1 transmit. -//! -//! USART1 TX uses DMA2 stream 7, channel 4 on STM32F722. The route is a type, -//! rather than platform-provided integer parameters, so invalid request mappings -//! cannot be selected at construction. - -use kaptan_dma::{DmaReadableBuffer, DmaTxEngine, TxCompletion}; -use kaptan_err_reportable::KaptanError; -use stm32f7::stm32f722; - -use super::Usart1Tx; - -// The F7 DMA controller can expose TCIF a few bus cycles before CR.EN reads -// low. This bounded IRQ-side settling observation never spins indefinitely; -// a later service turn can retry a start if the stream is not yet available. -const DISABLE_POLL_LIMIT: u8 = 32; - -/// Owns the fixed STM32F722 USART1 TX DMA route. -pub struct Usart1TxDma { - usart: Usart1Tx, -} - -/// Failure to start one DMA frame transfer. -#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)] -pub enum StartError { - /// DMA cannot transfer an empty buffer or one exceeding the NDTR limit. - #[kaptan_error(id = 11301)] - InvalidBufferLength, - /// DMA2 stream 7 still owns an earlier transfer. - #[kaptan_error(id = 11302)] - Busy, -} - -impl Usart1TxDma { - /// Enables DMA2 for USART1's fixed transmit route. - pub fn new(rcc: &stm32f722::RCC, usart: Usart1Tx) -> Self { - rcc.ahb1enr().modify(|_, write| write.dma2en().set_bit()); - Self { usart } - } - - /// Starts one memory-to-USART DMA transfer. - /// - /// # Ownership - /// - /// The caller must retain `buffer`'s backing storage until - /// [`Self::acknowledge_interrupt`] returns a terminal result. - /// - The platform must bind `DMA2_STREAM7`, call `acknowledge_interrupt` from - /// that interrupt, and defer lifecycle handling to its single owner. - pub fn start(&mut self, buffer: DmaReadableBuffer<'_>) -> Result<(), StartError> { - let bytes = buffer.bytes(); - if bytes.is_empty() || bytes.len() > u16::MAX as usize { - return Err(StartError::InvalidBufferLength); - } - if self.is_active() { - return Err(StartError::Busy); - } - - self.clear_interrupt_flags(); - self.configure_stream(bytes); - self.stream().cr().modify(|_, write| write.en().enabled()); - // AN4031 section 4.3 requires the stream to be enabled before the - // peripheral request. Reversing this order can raise a spurious FEIF. - self.usart.enable_dma_transmit(); - - Ok(()) - } - - /// Acknowledges a DMA2 Stream7 terminal IRQ without accessing engine state. - pub fn acknowledge_interrupt() -> TxCompletion { - // SAFETY: this handler accesses only the fixed DMA2 Stream7 route. - let controller = unsafe { &*stm32f722::DMA2::ptr() }; - let status = controller.hisr().read(); - let completion = DmaTerminalStatus { - transfer_complete: status.tcif7().is_complete(), - transfer_error: status.teif7().is_error(), - direct_mode_error: status.dmeif7().is_error(), - } - .classify(); - if completion == TxCompletion::NotPending { - return completion; - } - - // A normal-mode STM32 DMA stream clears EN at terminal completion. - // Request disable and observe that transition for a small fixed bound; - // no unbounded wait is permitted in an IRQ. If the bound expires before - // EN becomes visible as low, a later service turn observes `Busy` and - // retries without treating this documented hardware latency as a fault. - controller - .st(7) - .cr() - .modify(|_, write| write.en().disabled()); - for _ in 0..DISABLE_POLL_LIMIT { - if !controller.st(7).cr().read().en().is_enabled() { - break; - } - } - Usart1Tx::disable_dma_transmit_from_interrupt(); - controller.hifcr().write(|write| { - write - .ctcif7() - .clear() - .chtif7() - .clear() - .cteif7() - .clear() - .cdmeif7() - .clear() - .cfeif7() - .clear() - }); - completion - } - - /// Returns whether DMA2 stream 7 owns a transfer. - pub fn is_active(&self) -> bool { - self.stream().cr().read().en().is_enabled() - } - - fn configure_stream(&mut self, buffer: &[u8]) { - self.stream() - .fcr() - .write(|write| write.dmdis().enabled().feie().disabled()); - self.stream().cr().write(|write| { - // SAFETY: Channel 4 and these transfer fields are the documented - // USART1 TX mapping for DMA2 stream 7 on STM32F722. - unsafe { - write - .chsel() - .bits(4) - .dir() - .memory_to_peripheral() - .pl() - .very_high() - .minc() - .incremented() - .pinc() - .fixed() - .msize() - .bits8() - .psize() - .bits8() - .circ() - .disabled() - .tcie() - .enabled() - .teie() - .enabled() - .dmeie() - .enabled() - } - }); - self.stream().ndtr().write(|write| { - // SAFETY: `start` checked the slice length against the NDTR limit. - unsafe { write.bits(buffer.len() as u32) } - }); - self.stream().par().write(|write| { - // SAFETY: This is the memory-mapped USART1 transmit-data register. - unsafe { write.bits(self.usart.transmit_data_register_address()) } - }); - self.stream().m0ar().write(|write| { - // SAFETY: The caller of `start` upholds the buffer lifetime contract. - unsafe { write.bits(buffer.as_ptr() as u32) } - }); - } - - fn stream(&self) -> &stm32f722::dma2::ST { - self.controller().st(7) - } - - fn clear_interrupt_flags(&self) { - self.controller().hifcr().write(|write| { - write - .ctcif7() - .clear() - .chtif7() - .clear() - .cteif7() - .clear() - .cdmeif7() - .clear() - .cfeif7() - .clear() - }); - } - - fn controller(&self) -> &stm32f722::dma2::RegisterBlock { - // SAFETY: USART1 TX permanently owns this disjoint Stream7 route and - // accesses no other DMA2 stream. - unsafe { &*stm32f722::DMA2::ptr() } - } -} - -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -struct DmaTerminalStatus { - transfer_complete: bool, - transfer_error: bool, - direct_mode_error: bool, -} - -impl DmaTerminalStatus { - const fn classify(self) -> TxCompletion { - // This route uses direct mode. FEIF can be raised when a peripheral - // request arrives before DMA can provide data; it does not terminate - // the stream and is cleared with the other status flags. TEIF and - // DMEIF remain genuine terminal faults. - if self.transfer_error || self.direct_mode_error { - TxCompletion::TransferFault - } else if self.transfer_complete { - TxCompletion::Complete - } else { - TxCompletion::NotPending - } - } -} - -impl DmaTxEngine for Usart1TxDma { - type StartError = StartError; - - fn start(&mut self, buffer: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> { - Self::start(self, buffer) - } -} diff --git a/firmware/platforms/stm32f7/src/f722/peripherals/usart2_tx_dma1_stream6.rs b/firmware/platforms/stm32f7/src/f722/peripherals/usart2_tx_dma1_stream6.rs deleted file mode 100644 index 01d6c96..0000000 --- a/firmware/platforms/stm32f7/src/f722/peripherals/usart2_tx_dma1_stream6.rs +++ /dev/null @@ -1,211 +0,0 @@ -//! Fixed STM32F722 DMA route for USART2 transmit. -//! -//! USART2 TX uses DMA1 Stream6, channel 4 on STM32F722. The route is a type, -//! rather than platform-provided integer parameters, so invalid request mappings -//! cannot be selected at construction. - -use kaptan_dma::{DmaReadableBuffer, DmaTxEngine, TxCompletion}; -use kaptan_err_reportable::KaptanError; -use stm32f7::stm32f722; - -use super::Usart2Tx; - -// Match the bounded terminal observation used by the USART1 route. A later -// service turn handles the documented short interval before EN reads low. -const DISABLE_POLL_LIMIT: u8 = 32; - -/// Owns the fixed USART2 TX DMA1 Stream6 route. -pub struct Usart2TxDma { - usart: Usart2Tx, -} - -/// Failure to start one DMA frame transfer on USART2. -#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)] -pub enum Usart2TxDmaStartError { - /// DMA cannot transfer an empty buffer or one exceeding the NDTR limit. - #[kaptan_error(id = 11401)] - InvalidBufferLength, - /// DMA1 stream 6 still owns an earlier transfer. - #[kaptan_error(id = 11402)] - Busy, -} - -impl Usart2TxDma { - /// Enables DMA1 for USART2's fixed transmit route. - pub fn new(rcc: &stm32f722::RCC, usart: Usart2Tx) -> Self { - rcc.ahb1enr().modify(|_, write| write.dma1en().set_bit()); - Self { usart } - } - - /// Starts one memory-to-USART DMA transfer. - /// - /// The caller retains the backing storage until - /// [`Self::acknowledge_interrupt`] reports a terminal result. - pub fn start(&mut self, buffer: DmaReadableBuffer<'_>) -> Result<(), Usart2TxDmaStartError> { - let bytes = buffer.bytes(); - if bytes.is_empty() || bytes.len() > u16::MAX as usize { - return Err(Usart2TxDmaStartError::InvalidBufferLength); - } - if self.is_active() { - return Err(Usart2TxDmaStartError::Busy); - } - - self.clear_interrupt_flags(); - self.configure_stream(bytes); - self.stream().cr().modify(|_, write| write.en().enabled()); - // AN4031 section 4.3 requires the stream to be enabled before the - // peripheral request. Reversing this order can raise a spurious FEIF. - self.usart.enable_dma_transmit(); - Ok(()) - } - - /// Acknowledges a DMA1 Stream6 terminal IRQ without accessing engine state. - pub fn acknowledge_interrupt() -> TxCompletion { - // SAFETY: this handler accesses only the fixed DMA1 Stream6 route. - let controller = unsafe { &*stm32f722::DMA1::ptr() }; - let status = controller.hisr().read(); - let completion = DmaTerminalStatus { - transfer_complete: status.tcif6().is_complete(), - transfer_error: status.teif6().is_error(), - direct_mode_error: status.dmeif6().is_error(), - } - .classify(); - if completion == TxCompletion::NotPending { - return completion; - } - - controller - .st(6) - .cr() - .modify(|_, write| write.en().disabled()); - for _ in 0..DISABLE_POLL_LIMIT { - if !controller.st(6).cr().read().en().is_enabled() { - break; - } - } - Usart2Tx::disable_dma_transmit_from_interrupt(); - controller.hifcr().write(|write| { - write - .ctcif6() - .clear() - .chtif6() - .clear() - .cteif6() - .clear() - .cdmeif6() - .clear() - .cfeif6() - .clear() - }); - completion - } - - /// Returns whether DMA1 Stream6 owns a transfer. - pub fn is_active(&self) -> bool { - self.stream().cr().read().en().is_enabled() - } - - fn configure_stream(&mut self, buffer: &[u8]) { - self.stream() - .fcr() - .write(|write| write.dmdis().enabled().feie().disabled()); - self.stream().cr().write(|write| { - // SAFETY: Channel 4 and these transfer fields are the documented - // USART2 TX mapping for DMA1 Stream6 on STM32F722. - unsafe { - write - .chsel() - .bits(4) - .dir() - .memory_to_peripheral() - .pl() - .very_high() - .minc() - .incremented() - .pinc() - .fixed() - .msize() - .bits8() - .psize() - .bits8() - .circ() - .disabled() - .tcie() - .enabled() - .teie() - .enabled() - .dmeie() - .enabled() - } - }); - self.stream().ndtr().write(|write| { - // SAFETY: `start` checked the slice length against the NDTR limit. - unsafe { write.bits(buffer.len() as u32) } - }); - self.stream().par().write(|write| { - // SAFETY: This is the memory-mapped USART2 transmit-data register. - unsafe { write.bits(self.usart.transmit_data_register_address()) } - }); - self.stream().m0ar().write(|write| { - // SAFETY: The caller of `start` upholds the buffer lifetime contract. - unsafe { write.bits(buffer.as_ptr() as u32) } - }); - } - - fn stream(&self) -> &stm32f722::dma2::ST { - self.controller().st(6) - } - - fn clear_interrupt_flags(&self) { - self.controller().hifcr().write(|write| { - write - .ctcif6() - .clear() - .chtif6() - .clear() - .cteif6() - .clear() - .cdmeif6() - .clear() - .cfeif6() - .clear() - }); - } - - fn controller(&self) -> &stm32f722::dma2::RegisterBlock { - // SAFETY: SPI3 owns DMA1 initialization; USART2 TX is permanently - // assigned to disjoint Stream6 and accesses no other stream. - unsafe { &*stm32f722::DMA1::ptr() } - } -} - -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -struct DmaTerminalStatus { - transfer_complete: bool, - transfer_error: bool, - direct_mode_error: bool, -} - -impl DmaTerminalStatus { - const fn classify(self) -> TxCompletion { - // This route uses direct mode. FEIF can be raised when a peripheral - // request arrives before DMA can provide data; it does not terminate - // the stream and is cleared with the other status flags. TEIF and - // DMEIF remain genuine terminal faults. - if self.transfer_error || self.direct_mode_error { - TxCompletion::TransferFault - } else if self.transfer_complete { - TxCompletion::Complete - } else { - TxCompletion::NotPending - } - } -} - -impl DmaTxEngine for Usart2TxDma { - type StartError = Usart2TxDmaStartError; - - fn start(&mut self, buffer: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> { - Self::start(self, buffer) - } -} diff --git a/firmware/platforms/stm32f7/src/f722/peripherals/usb.rs b/firmware/platforms/stm32f7/src/f722/peripherals/usb.rs index 2f77472..64a90f2 100644 --- a/firmware/platforms/stm32f7/src/f722/peripherals/usb.rs +++ b/firmware/platforms/stm32f7/src/f722/peripherals/usb.rs @@ -8,8 +8,11 @@ use usb_device::{ }; use usbd_serial::CdcAcmClass; +use super::time::TimerClock; + use super::super::{ clock::CORE_FREQUENCY_HZ, + data_channel::RxFifoInterruptGate, usb::{USB_PACKET_BYTES, UsbMavlink}, }; @@ -29,10 +32,12 @@ pub(crate) struct F722UsbPeripheral { _global: stm32f722::OTG_FS_GLOBAL, _device: stm32f722::OTG_FS_DEVICE, _power_clock: stm32f722::OTG_FS_PWRCLK, + clock: TimerClock, } // SAFETY: the token is constructed once from PAC-owned peripherals and moved -// into one UsbBus. Runtime register access is synchronized by that bus. +// into one UsbBus. Its register access, the delegated RXFLVLM gate and the +// read-only reset witnesses share the USB I/O owner's RTIC resource lock. unsafe impl Sync for F722UsbPeripheral {} // SAFETY: the F722 OTG FS register layout, FIFO depth, and endpoint count are @@ -42,6 +47,9 @@ unsafe impl UsbPeripheral for F722UsbPeripheral { const HIGH_SPEED: bool = false; const FIFO_DEPTH_WORDS: usize = 320; const ENDPOINT_COUNT: usize = 6; + // Per-wait fault containment policy, not a qualified hardware latency. + const WAIT_TIMEOUT_US: u32 = 5_000; + const WAIT_POLL_LIMIT: u32 = CORE_FREQUENCY_HZ / 200; fn enable() { cortex_m::interrupt::free(|_| { @@ -59,6 +67,24 @@ unsafe impl UsbPeripheral for F722UsbPeripheral { fn ahb_frequency_hz(&self) -> u32 { CORE_FREQUENCY_HZ } + + fn now_us(&self) -> u64 { + let mut clock = self.clock; + clock.now().as_microseconds() + } + + fn abort(&self) { + cortex_m::peripheral::NVIC::mask(stm32f722::Interrupt::OTG_FS); + cortex_m::interrupt::free(|_| { + // SAFETY: this capability owns OTG FS. Exclusion serializes the RCC + // RMW with other peripheral owners. Reset remains asserted, with + // the clock on, until the next whole-controller restart. + let rcc = unsafe { &*stm32f722::RCC::ptr() }; + rcc.ahb2rstr().modify(|_, write| write.otgfsrst().set_bit()); + }); + cortex_m::asm::dsb(); + cortex_m::peripheral::NVIC::unpend(stm32f722::Interrupt::OTG_FS); + } } type F722UsbBus = UsbBus; @@ -87,12 +113,16 @@ pub(crate) fn usb_mavlink( global: stm32f722::OTG_FS_GLOBAL, device: stm32f722::OTG_FS_DEVICE, power_clock: stm32f722::OTG_FS_PWRCLK, + clock: TimerClock, product: &'static str, -) -> Result { +) -> Result<(F722UsbMavlink, RxFifoInterruptGate), ()> { + // Delegate one narrow capability while consuming the controller's PAC owners. + let mut rx_interrupt = RxFifoInterruptGate::new(&global); let peripheral = F722UsbPeripheral { _global: global, _device: device, _power_clock: power_clock, + clock, }; let endpoint_memory = cortex_m::singleton!( USB_ENDPOINT_MEMORY: [u32; ENDPOINT_MEMORY_WORDS] = [0; ENDPOINT_MEMORY_WORDS] @@ -117,5 +147,10 @@ pub(crate) fn usb_mavlink( .max_packet_size_0(USB_PACKET_BYTES as u8) .map_err(|_| ())? .build(); - Ok(UsbMavlink::new(device, cdc)) + // build() calls UsbDevice::new -> allocator.freeze -> UsbBus::enable, which + // initializes GINTMSK. Apply the empty application-RX state only afterwards. + if !device.bus().is_offline() { + rx_interrupt.set_enabled(true); + } + Ok((UsbMavlink::new(device, cdc), rx_interrupt)) } diff --git a/firmware/platforms/stm32f7/src/f722/ports.rs b/firmware/platforms/stm32f7/src/f722/ports.rs new file mode 100644 index 0000000..e18da5a --- /dev/null +++ b/firmware/platforms/stm32f7/src/f722/ports.rs @@ -0,0 +1,160 @@ +//! UART pin routes and configuration validation. +use crate::SerialInventory; +use kaptan_config::{Config, ConfigValue, ConfigViolation, FieldId, ValidationRule}; +use kaptan_ports::{PortDescriptor, PortId, PortPlan, PortSettings, Role}; + +// Resource IDs: PAx = x, PBx = 16+x, PCx = 32+x; peripheral IDs >= 100. +// USB, both supported IMUs and inhibited outputs are disjoint from these routes. +pub const UART1: PortDescriptor = PortDescriptor { + id: PortId::new(1), + role_field: FieldId::new(201), + baud_field: FieldId::new(202), + role_key: "ports.uart1.role", + baud_key: "ports.uart1.baud", + supported_roles: Role::CONFIG_ENUM_OPTIONS, + allowed_bauds: &[115_200, 230_400, 460_800], + resources: &[9, 10, 101], +}; +pub const UART2: PortDescriptor = PortDescriptor { + id: PortId::new(2), + role_field: FieldId::new(203), + baud_field: FieldId::new(204), + role_key: "ports.uart2.role", + baud_key: "ports.uart2.baud", + supported_roles: Role::CONFIG_ENUM_OPTIONS, + allowed_bauds: &[115_200, 230_400, 460_800], + resources: &[2, 3, 102], +}; +pub const UART3: PortDescriptor = PortDescriptor { + id: PortId::new(3), + role_field: FieldId::new(205), + baud_field: FieldId::new(206), + role_key: "ports.uart3.role", + baud_key: "ports.uart3.baud", + supported_roles: Role::CONFIG_ENUM_OPTIONS, + allowed_bauds: &[115_200, 230_400, 460_800], + resources: &[26, 27, 103], +}; +pub const UART4: PortDescriptor = PortDescriptor { + id: PortId::new(4), + role_field: FieldId::new(207), + baud_field: FieldId::new(208), + role_key: "ports.uart4.role", + baud_key: "ports.uart4.baud", + supported_roles: Role::CONFIG_ENUM_OPTIONS, + allowed_bauds: &[115_200, 230_400, 460_800], + resources: &[0, 1, 104], +}; +pub const UART6: PortDescriptor = PortDescriptor { + id: PortId::new(6), + role_field: FieldId::new(211), + baud_field: FieldId::new(212), + role_key: "ports.uart6.role", + baud_key: "ports.uart6.baud", + supported_roles: Role::CONFIG_ENUM_OPTIONS, + allowed_bauds: &[115_200, 230_400, 460_800], + resources: &[38, 39, 106], +}; + +const MCU_ROUTES: [PortDescriptor; 5] = [UART1, UART2, UART3, UART4, UART6]; +const RESERVED_RESOURCES: &[u16] = &[ + 4, 5, 6, 7, 18, 15, 35, 42, 43, 44, 16, 17, 20, 21, 13, 50, 24, 25, +]; + +pub(crate) fn project_ports( + candidate: &C, + inventory: SerialInventory, + wire_bytes: u32, +) -> Result, ConfigViolation> { + let descriptors: &[PortDescriptor; N] = inventory + .descriptors() + .try_into() + .map_err(|_| ConfigViolation::global(ValidationRule::Inconsistent))?; + // A board selects canonical routes, not replacement pin or capability data. + for descriptor in descriptors { + if !MCU_ROUTES.contains(descriptor) { + return Err(ConfigViolation::field( + descriptor.role_field, + ValidationRule::UnsupportedCapability, + )); + } + for expected in descriptor.fields() { + let mut fields = (0..C::FIELD_COUNT) + .filter_map(C::descriptor) + .filter(|field| field.id == expected.id); + if fields.next() != Some(&expected) || fields.next().is_some() { + return Err(ConfigViolation::field( + expected.id, + ValidationRule::Inconsistent, + )); + } + } + } + // Reject even disabled extra fields. Otherwise the root would advertise + // settings that boot never consumes. Other schema children remain independent. + for field in (0..C::FIELD_COUNT).filter_map(C::descriptor) { + if (field.display_key.starts_with("ports.") + || MCU_ROUTES + .iter() + .any(|port| field.id == port.role_field || field.id == port.baud_field)) + && !descriptors + .iter() + .any(|port| field.id == port.role_field || field.id == port.baud_field) + { + return Err(ConfigViolation::field( + field.id, + ValidationRule::Inconsistent, + )); + } + } + let mut settings = [PortSettings { + role: Role::Disabled, + baud: 115_200, + }; N]; + let mut mtf01 = None; + for (slot, descriptor) in settings.iter_mut().zip(descriptors) { + let Some(ConfigValue::U8(role)) = candidate.field_value(descriptor.role_field) else { + return Err(ConfigViolation::field( + descriptor.role_field, + ValidationRule::Inconsistent, + )); + }; + slot.role = Role::try_from_wire(role).ok_or(ConfigViolation::field( + descriptor.role_field, + ValidationRule::InvalidEnumValue, + ))?; + let Some(ConfigValue::U32(baud)) = candidate.field_value(descriptor.baud_field) else { + return Err(ConfigViolation::field( + descriptor.baud_field, + ValidationRule::Inconsistent, + )); + }; + slot.baud = baud; + if slot.role == Role::Mtf01 { + // F722 has one range/flow mailbox pair and uses the factory baud. + if let Some(previous) = mtf01 { + return Err(ConfigViolation::related( + descriptor.role_field, + previous, + ValidationRule::Inconsistent, + )); + } + mtf01 = Some(descriptor.role_field); + if baud != 115_200 { + return Err(ConfigViolation::field( + descriptor.baud_field, + ValidationRule::UnsupportedCapability, + )); + } + } + } + PortPlan::validate(descriptors, settings, RESERVED_RESOURCES, wire_bytes, 0) +} + +pub(crate) fn disable_ports(candidate: &mut C, inventory: SerialInventory) { + for descriptor in inventory.descriptors() { + candidate + .set_field(descriptor.role_field, ConfigValue::U8(0)) + .expect("factory port role accepts Disabled"); + } +} diff --git a/firmware/platforms/stm32f7/src/f722/routes.rs b/firmware/platforms/stm32f7/src/f722/routes.rs index 44040b9..a6f2810 100644 --- a/firmware/platforms/stm32f7/src/f722/routes.rs +++ b/firmware/platforms/stm32f7/src/f722/routes.rs @@ -1,9 +1,63 @@ //! Platform-owned STM32F722 electrical route implementations. -use super::peripherals::{Spi1, Spi1Error, Spi3, Spi3Error}; -use kaptan_spi::{SpiCompletion, SpiDevice}; +use super::{ + interrupts::F722Interrupts, + peripherals::{Spi1, Spi1Error, Spi3, Spi3Error, TimerClock}, + sensors::ImuService, +}; +use crate::{Icm42688Spi3, Mpu6000Spi1, sealed}; +use kaptan_spi::{CompletionError, SpiCompletion, SpiDevice}; use stm32f7::stm32f722; +/// Selects matching IMU initialization and interrupt handlers. +pub trait F722Route: Copy + sealed::Route + F722Interrupts { + type Imu: ImuService; + + fn initialize(self, resources: ImuResources<'_>, clock: TimerClock) -> Self::Imu; +} + +/// Peripheral tokens consumed by the selected IMU route. +#[doc(hidden)] +pub struct ImuResources<'a> { + pub(crate) rcc: &'a stm32f722::RCC, + pub(crate) gpioa: &'a stm32f722::GPIOA, + pub(crate) gpioc: &'a stm32f722::GPIOC, + pub(crate) gpiob: stm32f722::GPIOB, + pub(crate) syscfg: stm32f722::SYSCFG, + pub(crate) exti: stm32f722::EXTI, + pub(crate) spi1: stm32f722::SPI1, + pub(crate) spi3: stm32f722::SPI3, + pub(crate) dma1: stm32f722::DMA1, + pub(crate) dma2: stm32f722::DMA2, +} + +impl F722Route for Mpu6000Spi1 { + type Imu = super::sensors::Mpu6000Imu; + fn initialize(self, r: ImuResources<'_>, clock: TimerClock) -> Self::Imu { + super::peripherals::configure_spi1_pins_pa5_pa6_pa7(r.rcc, r.gpioa); + super::peripherals::configure_exti4_pin_pa4(r.gpioa); + super::board::configure_exti4(r.rcc, r.syscfg, r.exti); + Self::Imu::new( + r.rcc, + r.spi1, + r.dma2, + r.gpiob, + clock, + self.mount, + self.config, + ) + } +} +impl F722Route for Icm42688Spi3 { + type Imu = super::sensors::Icm42688Imu; + fn initialize(self, r: ImuResources<'_>, clock: TimerClock) -> Self::Imu { + super::peripherals::configure_spi3_pins_pc10_pc11_pc12(r.rcc, r.gpioc); + super::peripherals::configure_exti3_pin_pc3(r.gpioc); + super::board::configure_exti3(r.rcc, r.syscfg, r.exti); + Self::Imu::new(r.rcc, r.spi3, r.dma1, r.gpioa, clock, self.mount) + } +} + /// Owns the MPU6000 SPI1 transaction path and its PB2 chip select. pub(crate) struct Mpu6000Spi1Route { spi: Spi1, @@ -25,9 +79,8 @@ impl Mpu6000Spi1Route { .pupdr() .modify(|_, write| write.pupdr2().floating()); chip_select.bsrr().write(|write| write.bs2().set_bit()); - // Drop the full GPIOB owner after initialization. Runtime access is - // deliberately limited to PB2's atomic BSRR bits below, leaving no - // route-owned PAC token that could reconfigure the former motor pins. + // Release GPIOB after setup. Runtime writes only PB2's atomic BSRR + // bits and cannot reconfigure motor pins. Self { spi } } @@ -54,27 +107,33 @@ impl Mpu6000Spi1Route { impl SpiDevice for Mpu6000Spi1Route { type StartError = Spi1Error; - fn start_transfer( - &mut self, - transmit: &[u8], - receive: &mut [u8], - ) -> Result<(), Self::StartError> { + fn is_busy(error: &Self::StartError) -> bool { + *error == Spi1Error::Busy + } + + fn start_transfer(&mut self, transmit: &[u8]) -> Result<(), Self::StartError> { + self.spi.check_start(transmit.len())?; self.select(); - if let Err(error) = self.spi.start_transfer(transmit, receive) { + if let Err(error) = self.spi.start_transfer(transmit) { self.deselect(); return Err(error); } Ok(()) } - fn complete_transfer(&mut self, completion: SpiCompletion) { - self.spi.complete_transfer(completion); + fn complete_transfer( + &mut self, + completion: SpiCompletion, + receive: &mut [u8], + ) -> Result<(), CompletionError> { + let result = self.spi.complete_transfer(completion, receive); if matches!( completion, SpiCompletion::Complete | SpiCompletion::TransferFault ) { self.deselect(); } + result } } @@ -130,26 +189,32 @@ impl Icm42688Spi3Route { impl SpiDevice for Icm42688Spi3Route { type StartError = Spi3Error; - fn start_transfer( - &mut self, - transmit: &[u8], - receive: &mut [u8], - ) -> Result<(), Self::StartError> { + fn is_busy(error: &Self::StartError) -> bool { + *error == Spi3Error::Busy + } + + fn start_transfer(&mut self, transmit: &[u8]) -> Result<(), Self::StartError> { + self.spi.check_start(transmit.len())?; self.select(); - if let Err(error) = self.spi.start_transfer(transmit, receive) { + if let Err(error) = self.spi.start_transfer(transmit) { self.deselect(); return Err(error); } Ok(()) } - fn complete_transfer(&mut self, completion: SpiCompletion) { - self.spi.complete_transfer(completion); + fn complete_transfer( + &mut self, + completion: SpiCompletion, + receive: &mut [u8], + ) -> Result<(), CompletionError> { + let result = self.spi.complete_transfer(completion, receive); if matches!( completion, SpiCompletion::Complete | SpiCompletion::TransferFault ) { self.deselect(); } + result } } diff --git a/firmware/platforms/stm32f7/src/f722/sensors.rs b/firmware/platforms/stm32f7/src/f722/sensors.rs index 01046f7..dc19673 100644 --- a/firmware/platforms/stm32f7/src/f722/sensors.rs +++ b/firmware/platforms/stm32f7/src/f722/sensors.rs @@ -1,9 +1,9 @@ //! Platform-owned F722 sensor-service assembly. -use super::peripherals::{DwtMicrosecondClock, Spi1, Spi1Error, Spi3, Spi3Error}; +use super::peripherals::{Spi1, Spi1Error, Spi3, Spi3Error, TimerClock}; use kaptan_drivers::{ - icm42688::{Error as Icm42688Error, Icm42688, Icm42688Service, Icm42688ServiceEvent}, - mpu6000::{Error as Mpu6000Error, Mpu6000, Mpu6000Config, Mpu6000Service, Mpu6000ServiceEvent}, + icm42688::{Icm42688, Icm42688Service, Icm42688ServiceEvent}, + mpu6000::{Mpu6000, Mpu6000Config, Mpu6000Service, Mpu6000ServiceEvent}, }; use kaptan_err_reportable::{ErrorId, ReportableError}; use kaptan_sensor::imu::ImuSnapshot; @@ -16,115 +16,127 @@ use super::routes::{Icm42688Spi3Route, Mpu6000Spi1Route}; /// A profile-neutral IMU worker result. Device protocol errors are consumed /// here because flight policy only needs the terminal healthy/fault outcome. -pub(crate) enum ImuEvent { +pub enum ImuEvent { Idle, Initialized, Sample(ImuSnapshot), Fault(ErrorId), } -/// Opaque owner of the selected F722 IMU service. -/// -/// RTIC places local resource types in generated public metadata, so this -/// wrapper is public. The device services and routes remain private to the -/// platform, preserving the board-facing abstraction boundary. -pub struct F722Imu { - inner: F722ImuKind, +/// Operations used by the deferred IMU worker, statically dispatched to the +/// route's concrete service. No peripheral ownership crosses this interface. +pub trait ImuService: Send + 'static { + fn begin(&mut self) -> ImuEvent; + fn on_periodic_tick(&mut self) -> ImuEvent; + fn on_data_ready(&mut self) -> ImuEvent; + fn on_transfer_complete(&mut self, completion: SpiCompletion) -> ImuEvent; +} + +/// RTIC metadata needs a public owner; its concrete driver and pins stay private. +pub struct Mpu6000Imu { + inner: Mpu6000Service, mount: ImuMount, } -/// The two concrete IMU services supported by the F722 platform. -enum F722ImuKind { - Mpu6000(Mpu6000Service), - Icm42688(Icm42688Service), -} - -impl F722Imu { - pub(crate) fn mpu6000( +impl Mpu6000Imu { + pub(crate) fn new( rcc: &stm32f722::RCC, spi: stm32f722::SPI1, dma: stm32f722::DMA2, chip_select: stm32f722::GPIOB, - clock: DwtMicrosecondClock, + clock: TimerClock, mount: ImuMount, config: Mpu6000Config, ) -> Self { Self { - inner: F722ImuKind::Mpu6000(Mpu6000Service::new( + inner: Mpu6000Service::new( clock, Mpu6000::new( Mpu6000Spi1Route::new(rcc, Spi1::new(rcc, spi, dma), chip_select), config, ), - )), + ), mount, } } +} - pub(crate) fn icm42688( +impl ImuService for Mpu6000Imu { + fn begin(&mut self) -> ImuEvent { + match self.inner.begin() { + Ok(()) => ImuEvent::Idle, + Err(error) => ImuEvent::Fault(error.error_id()), + } + } + + fn on_periodic_tick(&mut self) -> ImuEvent { + mpu_event(self.inner.on_periodic_tick(), self.mount) + } + + fn on_data_ready(&mut self) -> ImuEvent { + mpu_event(self.inner.on_data_ready(), self.mount) + } + + fn on_transfer_complete(&mut self, completion: SpiCompletion) -> ImuEvent { + mpu_event(self.inner.on_transfer_complete(completion), self.mount) + } +} + +/// Concrete ICM owner, including its board-declared mounting transform. +pub struct Icm42688Imu { + inner: Icm42688Service, + mount: ImuMount, +} + +impl Icm42688Imu { + pub(crate) fn new( rcc: &stm32f722::RCC, spi: stm32f722::SPI3, dma: stm32f722::DMA1, chip_select: &stm32f722::GPIOA, - clock: DwtMicrosecondClock, + clock: TimerClock, mount: ImuMount, ) -> Self { Self { - inner: F722ImuKind::Icm42688(Icm42688Service::new( + inner: Icm42688Service::new( clock, Icm42688::new(Icm42688Spi3Route::new( rcc, Spi3::new(rcc, spi, dma), chip_select, )), - )), + ), mount, } } - - pub(crate) fn begin(&mut self) -> ImuEvent { - match &mut self.inner { - F722ImuKind::Mpu6000(imu) => match imu.begin() { - Ok(()) => ImuEvent::Idle, - Err(error) => ImuEvent::Fault(error.error_id()), - }, - F722ImuKind::Icm42688(imu) => match imu.begin() { - Ok(()) => ImuEvent::Idle, - Err(error) => ImuEvent::Fault(error.error_id()), - }, - } - } - - pub(crate) fn on_periodic_tick(&mut self) -> ImuEvent { - match &mut self.inner { - F722ImuKind::Mpu6000(imu) => mpu_event(imu.on_periodic_tick()), - F722ImuKind::Icm42688(imu) => icm_event(imu.on_periodic_tick(), self.mount), - } - } - - pub(crate) fn on_data_ready(&mut self) -> ImuEvent { - match &mut self.inner { - F722ImuKind::Mpu6000(imu) => mpu_event(imu.on_data_ready()), - F722ImuKind::Icm42688(imu) => icm_event(imu.on_data_ready(), self.mount), - } - } - - pub(crate) fn on_transfer_complete(&mut self, completion: SpiCompletion) -> ImuEvent { - match &mut self.inner { - F722ImuKind::Mpu6000(imu) => mpu_event(imu.on_transfer_complete(completion)), - F722ImuKind::Icm42688(imu) => { - icm_event(imu.on_transfer_complete(completion), self.mount) - } - } - } } -fn mpu_event(event: Mpu6000ServiceEvent) -> ImuEvent { +impl ImuService for Icm42688Imu { + fn begin(&mut self) -> ImuEvent { + match self.inner.begin() { + Ok(()) => ImuEvent::Idle, + Err(error) => ImuEvent::Fault(error.error_id()), + } + } + + fn on_periodic_tick(&mut self) -> ImuEvent { + icm_event(self.inner.on_periodic_tick(), self.mount) + } + + fn on_data_ready(&mut self) -> ImuEvent { + icm_event(self.inner.on_data_ready(), self.mount) + } + + fn on_transfer_complete(&mut self, completion: SpiCompletion) -> ImuEvent { + icm_event(self.inner.on_transfer_complete(completion), self.mount) + } +} + +fn mpu_event(event: Mpu6000ServiceEvent, mount: ImuMount) -> ImuEvent { match event { Mpu6000ServiceEvent::Idle => ImuEvent::Idle, Mpu6000ServiceEvent::Initialized => ImuEvent::Initialized, - Mpu6000ServiceEvent::Sample(sample) => ImuEvent::Sample(sample), - Mpu6000ServiceEvent::Fault(Mpu6000Error::Start(Spi1Error::Busy)) => ImuEvent::Idle, + Mpu6000ServiceEvent::Sample(sample) => ImuEvent::Sample(align_mount(sample, mount)), Mpu6000ServiceEvent::Fault(error) => ImuEvent::Fault(error.error_id()), } } @@ -134,15 +146,13 @@ fn icm_event(event: Icm42688ServiceEvent, mount: ImuMount) -> ImuEven Icm42688ServiceEvent::Idle => ImuEvent::Idle, Icm42688ServiceEvent::Initialized => ImuEvent::Initialized, Icm42688ServiceEvent::Sample(sample) => ImuEvent::Sample(align_mount(sample, mount)), - Icm42688ServiceEvent::Fault(Icm42688Error::Start(Spi3Error::Busy)) => ImuEvent::Idle, Icm42688ServiceEvent::Fault(error) => ImuEvent::Fault(error.error_id()), } } /// Applies the board-declared fixed IMU-to-body transform. /// -/// This is immutable board metadata validated against the selected electrical -/// route before initialization. It is not an airframe calibration setting. +/// This is immutable board metadata, not an airframe calibration setting. fn align_mount(mut sample: ImuSnapshot, mount: ImuMount) -> ImuSnapshot { match mount { ImuMount::BodyAligned => sample, @@ -157,6 +167,13 @@ fn align_mount(mut sample: ImuSnapshot, mount: ImuMount) -> ImuSnapshot { sample.angular_rate_milliradians_per_second[1].saturating_neg(); sample } + ImuMount::Yaw270 => { + let [x, y, z] = sample.acceleration_milligravity; + sample.acceleration_milligravity = [y.saturating_neg(), x, z]; + let [x, y, z] = sample.angular_rate_milliradians_per_second; + sample.angular_rate_milliradians_per_second = [y.saturating_neg(), x, z]; + sample + } } } @@ -178,35 +195,69 @@ mod tests { ImuSnapshot::new( Timestamp::from_microseconds(0), [10, 20, 30], - [40, 50, 60], + [-34_871, 34_870, -34_871], 0, ), ImuMount::Yaw180, ); assert_eq!(sample.acceleration_milligravity, [-10, -20, 30]); - assert_eq!(sample.angular_rate_milliradians_per_second, [-40, -50, 60]); + assert_eq!( + sample.angular_rate_milliradians_per_second, + [34_871, -34_870, -34_871] + ); } #[test] - fn transient_spi_busy_is_retry_not_a_typed_imu_fault() { + fn mpu_samples_apply_declared_mount_without_changing_timestamp_or_temperature() { + let sample = ImuSnapshot::new( + Timestamp::from_microseconds(123), + [10, 20, 30], + [40, 50, 60], + 25, + ); + for (mount, acceleration, angular_rate) in [ + (ImuMount::BodyAligned, [10, 20, 30], [40, 50, 60]), + (ImuMount::Yaw180, [-10, -20, 30], [-40, -50, 60]), + (ImuMount::Yaw270, [-20, 10, 30], [-50, 40, 60]), + ] { + let ImuEvent::Sample(aligned) = mpu_event(Mpu6000ServiceEvent::Sample(sample), mount) + else { + panic!("sample event must stay a sample"); + }; + assert_eq!( + aligned, + ImuSnapshot::new( + Timestamp::from_microseconds(123), + acceleration, + angular_rate, + 25, + ) + ); + } + } + + #[test] + fn declared_driver_faults_are_never_silently_discarded() { assert!(matches!( - mpu_event(Mpu6000ServiceEvent::Fault(Mpu6000Error::Start( - Spi1Error::Busy - ))), - ImuEvent::Idle + mpu_event( + Mpu6000ServiceEvent::Fault(Mpu6000Error::Start(Spi1Error::Busy)), + ImuMount::BodyAligned + ), + ImuEvent::Fault(_) )); assert!(matches!( icm_event( Icm42688ServiceEvent::Fault(Icm42688Error::Start(Spi3Error::Busy)), ImuMount::BodyAligned, ), - ImuEvent::Idle + ImuEvent::Fault(_) )); assert!(matches!( - mpu_event(Mpu6000ServiceEvent::Fault(Mpu6000Error::Start( - Spi1Error::InvalidLength - ))), + mpu_event( + Mpu6000ServiceEvent::Fault(Mpu6000Error::Start(Spi1Error::InvalidLength)), + ImuMount::BodyAligned + ), ImuEvent::Fault(_) )); assert!(matches!( diff --git a/firmware/platforms/stm32f7/src/f722/serial.rs b/firmware/platforms/stm32f7/src/f722/serial.rs new file mode 100644 index 0000000..624413e --- /dev/null +++ b/firmware/platforms/stm32f7/src/f722/serial.rs @@ -0,0 +1,241 @@ +//! Owned bounded IRQ TX: moving/dropping a queue cannot leave a DMA reader. +use kaptan_mavlink::MavlinkFrame; +use kaptan_time::Timestamp; +use kaptan_tx::{PendingFrame, TransmitQueue}; + +pub(crate) const RX_CHUNK_BYTES: usize = 16; +const RX_CAPACITY: usize = 512; + +#[derive(Clone, Copy)] +pub(crate) enum ReceiveEvent { + Byte(u8), + Fault, + Idle, +} +pub(crate) trait SerialDevice: UartIo { + fn receive(&mut self) -> ReceiveEvent; +} + +/// A short-lock handoff, with the IRQ receipt time of every byte retained. +pub struct RxChunk { + pub(crate) bytes: [u8; RX_CHUNK_BYTES], + pub(crate) timestamps: [Timestamp; RX_CHUNK_BYTES], + pub(crate) length: usize, + pub(crate) discontinuity: bool, + pub(crate) pending: bool, + pub(crate) completed: bool, +} +impl Default for RxChunk { + fn default() -> Self { + Self { + bytes: [0; RX_CHUNK_BYTES], + timestamps: [Timestamp::default(); RX_CHUNK_BYTES], + length: 0, + discontinuity: false, + pending: false, + completed: false, + } + } +} + +/// PAC + byte queues only. This owner never decodes protocols or calls storage. +/// 512 bytes bound backlog to ~44 ms at 115200 / ~11 ms at 460800 baud. +/// Overflow discards the damaged stream and orders a decoder reset before any +/// subsequently admitted bytes, rather than splicing across a lost byte. +pub(crate) struct SerialIo { + device: Option, + tx: Option, + bytes: [u8; RX_CAPACITY], + timestamps: [Timestamp; RX_CAPACITY], + head: usize, + length: usize, + discontinuity: bool, + completed: bool, +} + +impl SerialIo { + pub(crate) fn new(device: Option, transmit: bool) -> Self { + Self { + device, + tx: transmit.then(IrqTransmit::new), + bytes: [0; RX_CAPACITY], + timestamps: [Timestamp::default(); RX_CAPACITY], + head: 0, + length: 0, + discontinuity: false, + completed: false, + } + } + /// Timestamp is captured by the canonical clock immediately before this + /// bounded acknowledgement, not later when the protocol worker drains RX. + pub fn on_interrupt(&mut self, now: Timestamp) -> bool { + let Some(device) = self.device.as_mut() else { + return false; + }; + let event = device.receive(); + let completed = self.tx.as_mut().is_some_and(|tx| tx.on_interrupt(device)); + self.completed |= completed; + if matches!(event, ReceiveEvent::Fault) + || (matches!(event, ReceiveEvent::Byte(_)) && self.length == RX_CAPACITY) + { + self.head = 0; + self.length = 0; + self.discontinuity = true; + } + if let ReceiveEvent::Byte(byte) = event { + let index = (self.head + self.length) % RX_CAPACITY; + self.bytes[index] = byte; + self.timestamps[index] = now; + self.length += 1; + } + !matches!(event, ReceiveEvent::Idle) || completed + } + pub(crate) fn take_chunk(&mut self) -> RxChunk { + let mut chunk = RxChunk { + discontinuity: core::mem::take(&mut self.discontinuity), + completed: core::mem::take(&mut self.completed), + length: self.length.min(RX_CHUNK_BYTES), + ..RxChunk::default() + }; + for index in 0..chunk.length { + chunk.bytes[index] = self.bytes[self.head]; + chunk.timestamps[index] = self.timestamps[self.head]; + self.head = (self.head + 1) % RX_CAPACITY; + } + self.length -= chunk.length; + chunk.pending = self.length != 0; + chunk + } + pub(crate) fn send(&mut self, frame: PendingFrame) -> bool { + match (&mut self.device, &mut self.tx) { + (Some(device), Some(tx)) => tx.send(device, frame), + _ => false, + } + } +} + +pub(crate) trait UartIo { + fn tx_ready(&mut self) -> bool; + fn tx_complete(&mut self) -> bool; + fn write_byte(&mut self, byte: u8); + fn interrupts(&mut self, ready: bool, complete: bool); +} + +pub(crate) struct IrqTransmit { + queue: TransmitQueue, + offset: usize, + waiting_complete: bool, +} +impl IrqTransmit { + pub(crate) fn new() -> Self { + Self { + queue: TransmitQueue::new(), + offset: 0, + waiting_complete: false, + } + } + pub(crate) fn send(&mut self, io: &mut impl UartIo, frame: PendingFrame) -> bool { + let accepted = self.queue.enqueue(frame).is_retained(); + self.arm(io); + accepted + } + fn arm(&mut self, io: &mut impl UartIo) { + let _ = self + .queue + .start_with(|_| Ok::<_, core::convert::Infallible>(())); + io.interrupts( + self.queue.is_active() && !self.waiting_complete, + self.waiting_complete, + ); + } + /// At most one byte copied to TDR and one frame completion per interrupt. + pub(crate) fn on_interrupt(&mut self, io: &mut impl UartIo) -> bool { + let mut completed = false; + if self.waiting_complete && io.tx_complete() { + self.queue.complete(); + self.offset = 0; + self.waiting_complete = false; + completed = true; + } + self.arm(io); + if !self.waiting_complete + && io.tx_ready() + && let Some(active) = self.queue.active() + { + let bytes = active.frame.raw_bytes(); + io.write_byte(bytes[self.offset]); + self.offset += 1; + self.waiting_complete = self.offset == bytes.len(); + self.arm(io); + } + completed + } +} + +#[cfg(test)] +mod receive_tests { + use super::*; + struct Input(ReceiveEvent); + impl UartIo for Input { + fn tx_ready(&mut self) -> bool { + false + } + fn tx_complete(&mut self) -> bool { + false + } + fn write_byte(&mut self, _: u8) { + panic!("receive-only endpoint"); + } + fn interrupts(&mut self, _: bool, _: bool) { + panic!("receive-only endpoint"); + } + } + impl SerialDevice for Input { + fn receive(&mut self) -> ReceiveEvent { + self.0 + } + } + #[test] + fn overflow_and_hardware_fault_order_reset_before_later_bytes() { + let mut io = SerialIo::new(Some(Input(ReceiveEvent::Byte(42))), false); + for t in 0..=RX_CAPACITY { + assert!(io.on_interrupt(Timestamp::from_microseconds(t as u64))); + } + let chunk = io.take_chunk(); + assert!(chunk.discontinuity); + assert_eq!(chunk.length, 1); + assert_eq!(chunk.timestamps[0].as_microseconds(), RX_CAPACITY as u64); + io.device.as_mut().unwrap().0 = ReceiveEvent::Fault; + io.on_interrupt(Timestamp::from_microseconds(1000)); + io.device.as_mut().unwrap().0 = ReceiveEvent::Byte(7); + io.on_interrupt(Timestamp::from_microseconds(1001)); + let chunk = io.take_chunk(); + assert!(chunk.discontinuity); + assert_eq!(&chunk.bytes[..chunk.length], &[7]); + assert_eq!(chunk.timestamps[0].as_microseconds(), 1001); + } + #[test] + fn drains_are_bounded_and_disabled_endpoints_never_touch_registers() { + let mut io = SerialIo::::new(None, false); + assert!(!io.on_interrupt(Timestamp::default())); + assert_eq!(io.take_chunk().length, 0); + let mut io = SerialIo::new(Some(Input(ReceiveEvent::Byte(1))), false); + for t in 0..35 { + io.on_interrupt(Timestamp::from_microseconds(t)); + } + for (length, pending) in [(16, true), (16, true), (3, false)] { + let chunk = io.take_chunk(); + assert_eq!(chunk.length, length); + assert_eq!(chunk.pending, pending); + assert!(!chunk.completed, "RX must not manufacture a TX completion"); + } + io.device.as_mut().unwrap().0 = ReceiveEvent::Idle; + assert!(!io.on_interrupt(Timestamp::default())); + let frame = kaptan_mavlink::encoder::MavlinkEncoder::new(1, 1) + .encode(&kaptan_mavlink::generated::kaptan::HEARTBEAT_DATA::default()); + assert!(!io.send(PendingFrame { + class: kaptan_tx::FrameClass::Vital, + frame + })); + } +} diff --git a/firmware/platforms/stm32f7/src/f722/usb.rs b/firmware/platforms/stm32f7/src/f722/usb.rs index 653f7e4..31b7180 100644 --- a/firmware/platforms/stm32f7/src/f722/usb.rs +++ b/firmware/platforms/stm32f7/src/f722/usb.rs @@ -16,7 +16,21 @@ use usbd_serial::CdcAcmClass; pub(crate) const USB_PACKET_BYTES: usize = 64; -pub(crate) struct UsbMavlink<'a, B: UsbBus> { +pub(crate) trait UsbBusHealth: UsbBus { + fn is_offline(&self) -> bool; + fn disable(&self); +} + +impl UsbBusHealth for synopsys_usb_otg::UsbBus

{ + fn is_offline(&self) -> bool { + self.is_offline() + } + fn disable(&self) { + self.disable(); + } +} + +pub(crate) struct UsbMavlink<'a, B: UsbBusHealth> { device: UsbDevice<'a, B>, cdc: CdcAcmClass<'a, B>, transmit: TransmitQueue, @@ -26,7 +40,7 @@ pub(crate) struct UsbMavlink<'a, B: UsbBus> { rx_remaining: Range, } -impl<'a, B: UsbBus> UsbMavlink<'a, B> { +impl<'a, B: UsbBusHealth> UsbMavlink<'a, B> { pub(crate) fn new(device: UsbDevice<'a, B>, cdc: CdcAcmClass<'a, B>) -> Self { Self { device, @@ -40,7 +54,26 @@ impl<'a, B: UsbBus> UsbMavlink<'a, B> { } pub(crate) fn is_open(&self) -> bool { - self.device.state() == UsbDeviceState::Configured && self.cdc.dtr() + !self.device.bus().is_offline() + && self.device.state() == UsbDeviceState::Configured + && self.cdc.dtr() + } + + /// Gates RX FIFO interrupts while packet bytes remain unconsumed. + /// The caller must hold the USB lock through the operation and gate update. + /// A partial MAVLink frame alone must not mask RX. + pub(crate) fn with_rx_interrupt_gate( + &mut self, + operation: impl FnOnce(&mut Self) -> R, + set_rx_interrupt_enabled: impl FnOnce(bool), + ) -> R { + let result = operation(self); + // A terminal bus fault holds the peripheral in reset. Do not access its + // interrupt registers again, including through the delegated RX gate. + if !self.device.bus().is_offline() { + set_rx_interrupt_enabled(self.rx_remaining.is_empty()); + } + result } /// Returns whether the frame was retained, not whether the host received it. @@ -50,34 +83,45 @@ impl<'a, B: UsbBus> UsbMavlink<'a, B> { } let retained = self.transmit.enqueue(pending).is_retained(); self.service_transmit(); - retained + retained && !self.device.bus().is_offline() } /// Services USB and buffers at most one packet for the protocol worker. pub(crate) fn poll(&mut self) -> bool { + if self.device.bus().is_offline() { + self.close(); + return false; + } let previous_state = self.device.state(); let previous_dtr = self.cdc.dtr(); self.device.poll(&mut [&mut self.cdc]); + if self.device.bus().is_offline() { + self.close(); + return false; + } - if previous_state != UsbDeviceState::Default - && self.device.state() == UsbDeviceState::Default - { - // Only bus reset flushes endpoint FIFOs. DTR close, suspend, and - // deconfiguration pause TX without invalidating its byte cursor. + let bus_reset = previous_state != UsbDeviceState::Default + && self.device.state() == UsbDeviceState::Default; + if bus_reset { self.transmit = TransmitQueue::new(); self.tx_offset = 0; self.reset_receive(); - } else if previous_dtr && !self.cdc.dtr() { + self.discard_endpoint_receive(); + } + if previous_dtr && !self.cdc.dtr() { self.reset_receive(); } - if self.rx_remaining.is_empty() && self.device.state() == UsbDeviceState::Configured { + if !self.device.bus().is_offline() + && self.rx_remaining.is_empty() + && self.device.state() == UsbDeviceState::Configured + { // A previous poll may have left a packet in the bus while our // buffer was occupied. Reading must not depend on poll's result. match self.cdc.read_packet(&mut self.rx_packet) { Ok(length) if self.is_open() => self.rx_remaining = 0..length, Ok(_) | Err(UsbError::WouldBlock) => {} - Err(error) => panic!("CDC OUT endpoint invariant: {error:?}"), + Err(_) => self.close(), } } self.service_transmit(); @@ -86,6 +130,10 @@ impl<'a, B: UsbBus> UsbMavlink<'a, B> { /// Parses buffered bytes in the worker, without a second receive queue. pub(crate) fn take_received(&mut self) -> Option { + if self.device.bus().is_offline() { + self.close(); + return None; + } for index in self.rx_remaining.by_ref() { if let Some(frame) = self.framer.receive_byte(self.rx_packet[index]) { return Some(frame); @@ -99,6 +147,23 @@ impl<'a, B: UsbBus> UsbMavlink<'a, B> { self.rx_remaining = 0..0; } + fn close(&mut self) { + self.device.bus().disable(); + self.transmit = TransmitQueue::new(); + self.tx_offset = 0; + self.reset_receive(); + } + + fn discard_endpoint_receive(&mut self) { + // The pinned driver's deconfigure/configure paths flush hardware FIFOs, + // but do not clear EndpointBuffer::has_data. Read/discard its single + // CDC OUT software packet as cleanup, without admitting it. + match self.cdc.read_packet(&mut self.rx_packet) { + Ok(_) | Err(UsbError::WouldBlock) => {} + Err(_) => self.close(), + } + } + fn service_transmit(&mut self) { if !self.is_open() { return; @@ -111,7 +176,9 @@ impl<'a, B: UsbBus> UsbMavlink<'a, B> { let end = (*offset + USB_PACKET_BYTES).min(bytes.len()); let packet = &bytes[*offset..end]; let written = cdc.write_packet(packet)?; - assert_eq!(written, packet.len(), "CDC IN endpoint short write"); + if written != packet.len() { + return Err(UsbError::InvalidState); + } *offset = end; // An exact multiple of 64 needs a ZLP: retain offset == len until // the next empty packet is accepted. The endpoint copies bytes, @@ -125,11 +192,17 @@ impl<'a, B: UsbBus> UsbMavlink<'a, B> { }; match result { Ok(()) if complete => { - self.transmit.complete(); + self.transmit + .complete() + .expect("completed active USB frame"); self.tx_offset = 0; } Ok(()) | Err(UsbError::WouldBlock) => {} - Err(error) => panic!("CDC IN endpoint invariant: {error:?}"), + Err(_) => self.close(), } } } + +#[cfg(test)] +#[path = "../../tests/support/usb_bus.rs"] +pub(crate) mod test_bus; diff --git a/firmware/platforms/stm32f7/src/lib.rs b/firmware/platforms/stm32f7/src/lib.rs index 4d42168..23d840a 100644 --- a/firmware/platforms/stm32f7/src/lib.rs +++ b/firmware/platforms/stm32f7/src/lib.rs @@ -1,20 +1,24 @@ -//! STM32F7 execution platform for declarative Kaptan boards. +//! STM32F7 drivers and RTIC runtime. //! -//! A board crate selects the safe route descriptors below and invokes -//! [`flight_firmware!`]. The macro specializes one platform-defined RTIC task -//! graph for that board's configuration aggregate. RTIC, PAC ownership, -//! interrupt acknowledgement, DMA, and atomics remain platform implementation -//! details rather than board-package code. +//! Board crates supply wiring and configuration to [`flight_firmware!`]. #![no_std] +#[cfg(test)] +extern crate self as platform_stm32f7; + use kaptan_config::Config; use kaptan_err_reportable::KaptanError; +pub use f722::F722UserConfig; +pub use f722::ports::{UART1, UART2, UART3, UART4, UART6}; pub use kaptan_drivers::mpu6000::{ AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000Config, }; pub use kaptan_log::LevelFilter; +pub use kaptan_ports; + +use f722::F722Route; /// Fixed sensor-to-body mounting transform. #[derive(Clone, Copy, Debug, Eq, PartialEq)] @@ -23,6 +27,8 @@ pub enum ImuMount { BodyAligned, /// Sensor is rotated 180 degrees around the body Z axis. Yaw180, + /// Board mounting maps sensor axes (X, Y, Z) to (-Y, X, Z). + Yaw270, } #[doc(hidden)] @@ -32,7 +38,6 @@ pub enum ImuMount { )] pub mod f722; -/// Computes the exact bounded snapshot storage selected by a board aggregate. #[doc(hidden)] pub const fn config_snapshot_length() -> usize { assert!( @@ -42,14 +47,12 @@ pub const fn config_snapshot_length() -> usize { kaptan_config::scalar_snapshot_length(C::FIELD_COUNT as u16, C::SNAPSHOT_VALUE_BYTES) } -/// Dependencies named only by the platform's exported RTIC macro expansion. -/// Board source never depends on these implementation details directly. +/// Dependencies used by the exported RTIC macro. #[doc(hidden)] #[cfg_attr(not(target_os = "none"), allow(unused_imports))] pub mod __platform_internal { pub use cortex_m as cm; pub use cortex_m_rt as cmrt; - pub use kaptan_dma as dma; pub use kaptan_log as logging; #[cfg(target_os = "none")] pub use panic_halt as panic; @@ -62,16 +65,10 @@ pub mod __platform_internal { /// Construction or validation failure for a declarative F722 board. #[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)] pub enum BoardError { - /// The immutable vertical route and its electrical parameters disagree. - #[kaptan_error(id = 11901)] - VerticalSensorCapabilityMismatch, /// The declaration requests a service-timer frequency not implemented by /// the selected F722 platform profile. #[kaptan_error(id = 11902)] UnsupportedServiceFrequency, - /// The selected baud cannot carry the deterministic telemetry schedule. - #[kaptan_error(id = 11903)] - TelemetryLinkOvercommitted, /// The platform could not establish a clock, peripheral, or DMA owner. #[kaptan_error(id = 11904)] Initialization, @@ -80,472 +77,152 @@ pub enum BoardError { Configuration, } -/// MPU6000 component selector. +/// MPU6000 on SPI1 (PA5/PA6/PA7, PB2 chip select, PA4 data-ready). +/// +/// Driver settings must be supplied explicitly: +/// +/// ```compile_fail,E0063 +/// use platform_stm32f7::{ImuMount, Mpu6000Spi1}; +/// let _ = Mpu6000Spi1 { mount: ImuMount::BodyAligned }; +/// ``` #[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct Mpu6000; -/// MPU6000 SPI1 route awaiting a data-ready pin. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct Mpu6000Spi1; -/// MPU6000 SPI1/PA4 route awaiting its physical mount. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct Mpu6000Spi1Exti4; -/// Mounted MPU6000 route awaiting its driver configuration. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct MountedMpu6000 { - mount: ImuMount, -} -/// Complete MPU6000 declaration. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct ConfiguredMpu6000 { - mount: ImuMount, - config: Mpu6000Config, +pub struct Mpu6000Spi1 { + pub mount: ImuMount, + pub config: Mpu6000Config, } -impl Mpu6000 { - pub const fn on_spi1() -> Mpu6000Spi1 { - Mpu6000Spi1 +/// ICM-42688-P on SPI3 (PC10/PC11/PC12, PA15 chip select, PC3 data-ready). +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct Icm42688Spi3 { + pub mount: ImuMount, +} + +/// Optional vertical sensing supported by either electrical route. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum VerticalSensor { + None, + /// LIDAR-Lite V3 range on I2C1 (PB8/PB9). + LidarLiteV3I2c1, +} + +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum StatusLed { + ActiveLowPd2, + ActiveHighPa13, +} + +/// Documented motor pins driven low before fallible initialization. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum MotorPins { + /// PB0, PB1, PB4 and PB5. + Four, + /// PB0, PB1, PB4, PB5, PB6 and PB7. + Six, +} + +/// Board serial ports in schema order. Boot validates each descriptor against +/// the supported MCU routes. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct SerialInventory(&'static [kaptan_ports::PortDescriptor]); +impl SerialInventory { + pub const fn new(descriptors: &'static [kaptan_ports::PortDescriptor]) -> Self { + Self(descriptors) + } + pub const fn len(self) -> usize { + self.0.len() + } + pub const fn is_empty(self) -> bool { + self.0.is_empty() + } + pub const fn descriptors(self) -> &'static [kaptan_ports::PortDescriptor] { + self.0 + } + /// Resolves a stable IRQ/wire identity to the board's compact storage slot. + pub fn index(self, id: kaptan_ports::PortId) -> Option { + self.0.iter().position(|descriptor| descriptor.id == id) } } -impl Mpu6000Spi1 { - pub const fn with_drdy(self, _: Pa4) -> Mpu6000Spi1Exti4 { - Mpu6000Spi1Exti4 - } +/// Fixed board wiring. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct F722Hardware { + route: R, + serial: SerialInventory, + led: StatusLed, + vertical: VerticalSensor, + motors: MotorPins, } -impl Mpu6000Spi1Exti4 { - pub const fn mounted(self, mount: ImuMount) -> MountedMpu6000 { - MountedMpu6000 { mount } - } -} - -impl MountedMpu6000 { - pub const fn configured(self, config: Mpu6000Config) -> ConfiguredMpu6000 { - ConfiguredMpu6000 { - mount: self.mount, - config, - } - } -} - -/// ICM-42688-P component selector. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct Icm42688; -/// ICM-42688-P SPI3 route awaiting a data-ready pin. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct Icm42688Spi3; -/// ICM-42688-P SPI3/PC3 route awaiting its physical mount. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct Icm42688Spi3Exti3; -/// Complete ICM-42688-P declaration. The driver currently exposes no tuning -/// object, so the manifest does not pretend one exists. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct MountedIcm42688 { - mount: ImuMount, -} - -impl Icm42688 { - pub const fn on_spi3() -> Icm42688Spi3 { - Icm42688Spi3 - } -} - -impl Icm42688Spi3 { - pub const fn with_drdy(self, _: Pc3) -> Icm42688Spi3Exti3 { - Icm42688Spi3Exti3 - } -} - -impl Icm42688Spi3Exti3 { - pub const fn mounted(self, mount: ImuMount) -> MountedIcm42688 { - MountedIcm42688 { mount } - } -} - -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct Pa4; -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct Pc3; - -/// MAVLink component selector. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct Mavlink; -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct UnconfiguredMavlinkUsart1; -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct UnconfiguredMavlinkUsart2; -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct MavlinkUsart1 { - baud: u32, -} -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct MavlinkUsart2 { - baud: u32, -} - -impl Mavlink { - pub const fn on_usart1() -> UnconfiguredMavlinkUsart1 { - UnconfiguredMavlinkUsart1 - } - - pub const fn on_usart2() -> UnconfiguredMavlinkUsart2 { - UnconfiguredMavlinkUsart2 - } -} - -impl UnconfiguredMavlinkUsart1 { - pub const fn baud(self, baud: u32) -> MavlinkUsart1 { - MavlinkUsart1 { baud } - } -} - -impl UnconfiguredMavlinkUsart2 { - pub const fn baud(self, baud: u32) -> MavlinkUsart2 { - MavlinkUsart2 { baud } - } -} - -/// Vertical-sensor component selector. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct VerticalSensor; -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct NoVerticalSensor; -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct UnconfiguredMtf01Uart4; -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct Mtf01Uart4 { - baud: u32, -} -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct LidarLiteV3I2c1; - -impl VerticalSensor { - pub const fn none() -> NoVerticalSensor { - NoVerticalSensor - } - - pub const fn mtf01_on_uart4() -> UnconfiguredMtf01Uart4 { - UnconfiguredMtf01Uart4 - } - - pub const fn lidar_lite_v3_on_i2c1() -> LidarLiteV3I2c1 { - LidarLiteV3I2c1 - } -} - -impl UnconfiguredMtf01Uart4 { - pub const fn baud(self, baud: u32) -> Mtf01Uart4 { - Mtf01Uart4 { baud } - } -} - -/// Required boot-time output inhibition route. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct Pb0Pb1Pb4Pb5Low; -/// Kakute status LED on PD2, active low. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct Pd2ActiveLow; -/// GEPRC status LED on PA13, active high. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct Pa13ActiveHigh; - -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct Missing; - -/// Complete, non-executing hardware manifest stored in [`F722ImmutableConfig`]. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct F722Hardware { - profile: F722Profile, - vertical: f722::VerticalRoute, - imu_mount: ImuMount, - mpu6000_config: Option, - mavlink_baud: u32, - vertical_sensor_baud: Option, - status_led: F722StatusLedRoute, -} - -impl F722Hardware { - /// Starts an immutable hardware declaration. Only complete, compatible - /// declarations expose [`F722HardwareBuilder::telemetry_only`]. - #[allow(clippy::new_ret_no_self)] - pub const fn new() -> F722HardwareBuilder { - F722HardwareBuilder { - output: Missing, - imu: Missing, - mavlink: Missing, - vertical: Missing, - status_led: Missing, +impl F722Hardware { + pub const fn new( + route: R, + serial: SerialInventory, + led: StatusLed, + vertical: VerticalSensor, + motors: MotorPins, + ) -> Self { + Self { + route, + serial, + led, + vertical, + motors, } } - pub(crate) const fn profile(self) -> F722Profile { - self.profile - } - - pub(crate) const fn vertical(self) -> f722::VerticalRoute { + pub(crate) const fn vertical(self) -> VerticalSensor { self.vertical } - pub(crate) const fn imu_mount(self) -> ImuMount { - self.imu_mount - } - - pub(crate) const fn mpu6000_config(self) -> Option { - self.mpu6000_config - } - - pub(crate) const fn mavlink_baud(self) -> u32 { - self.mavlink_baud - } - - pub(crate) const fn vertical_sensor_baud(self) -> Option { - self.vertical_sensor_baud - } - - pub(crate) const fn status_led(self) -> F722StatusLedRoute { - self.status_led + pub const fn serial(self) -> SerialInventory { + self.serial } #[doc(hidden)] pub const fn has_range(self) -> bool { - matches!( - self.vertical, - f722::VerticalRoute::Mtf01 | f722::VerticalRoute::LidarLiteV3 - ) - } - - #[doc(hidden)] - pub const fn has_optical_flow(self) -> bool { - matches!(self.vertical, f722::VerticalRoute::Mtf01) - } -} - -/// Typestate used only while constructing [`F722Hardware`]. -pub struct F722HardwareBuilder< - Output = Missing, - Imu = Missing, - Link = Missing, - Vertical = Missing, - Led = Missing, -> { - output: Output, - imu: Imu, - mavlink: Link, - vertical: Vertical, - status_led: Led, -} - -impl F722HardwareBuilder { - pub const fn output_inhibit( - self, - output: Pb0Pb1Pb4Pb5Low, - ) -> F722HardwareBuilder { - F722HardwareBuilder { - output, - imu: self.imu, - mavlink: self.mavlink, - vertical: self.vertical, - status_led: self.status_led, - } - } -} - -impl F722HardwareBuilder { - pub const fn imu(self, imu: I) -> F722HardwareBuilder { - F722HardwareBuilder { - output: self.output, - imu, - mavlink: self.mavlink, - vertical: self.vertical, - status_led: self.status_led, - } - } -} - -impl F722HardwareBuilder { - pub const fn mavlink(self, mavlink: M) -> F722HardwareBuilder { - F722HardwareBuilder { - output: self.output, - imu: self.imu, - mavlink, - vertical: self.vertical, - status_led: self.status_led, - } - } -} - -impl F722HardwareBuilder { - pub const fn vertical_sensor(self, vertical: V) -> F722HardwareBuilder { - F722HardwareBuilder { - output: self.output, - imu: self.imu, - mavlink: self.mavlink, - vertical, - status_led: self.status_led, - } - } -} - -impl F722HardwareBuilder { - pub const fn status_led(self, status_led: L) -> F722HardwareBuilder { - F722HardwareBuilder { - output: self.output, - imu: self.imu, - mavlink: self.mavlink, - vertical: self.vertical, - status_led, - } + !matches!(self.vertical, VerticalSensor::None) } } +/// Board configuration with matching hardware and interrupt routes. +/// +/// ```compile_fail,E0308 +/// use platform_stm32f7::*; +/// fn wrong_route(hardware: F722Hardware) { +/// let _: F722ImmutableConfig = F722ImmutableConfig { +/// name: "mismatch", +/// service_hz: 100, +/// hardware, +/// }; +/// } +/// ``` #[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub(crate) enum F722Profile { - Mpu6000Spi1Usart1, - Icm42688Spi3Usart2, -} - -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub(crate) enum F722StatusLedRoute { - Pd2ActiveLow, - Pa13ActiveHigh, -} - -macro_rules! finish_mpu6000_hardware { - ($vertical:ty, $route:expr, no_serial_baud) => { - impl - F722HardwareBuilder< - Pb0Pb1Pb4Pb5Low, - ConfiguredMpu6000, - MavlinkUsart1, - $vertical, - Pd2ActiveLow, - > - { - pub const fn telemetry_only(self) -> F722Hardware { - F722Hardware { - profile: F722Profile::Mpu6000Spi1Usart1, - vertical: $route, - imu_mount: self.imu.mount, - mpu6000_config: Some(self.imu.config), - mavlink_baud: self.mavlink.baud, - vertical_sensor_baud: None, - status_led: F722StatusLedRoute::Pd2ActiveLow, - } - } - } - }; - ($vertical:ty, $route:expr, serial_baud) => { - impl - F722HardwareBuilder< - Pb0Pb1Pb4Pb5Low, - ConfiguredMpu6000, - MavlinkUsart1, - $vertical, - Pd2ActiveLow, - > - { - pub const fn telemetry_only(self) -> F722Hardware { - F722Hardware { - profile: F722Profile::Mpu6000Spi1Usart1, - vertical: $route, - imu_mount: self.imu.mount, - mpu6000_config: Some(self.imu.config), - mavlink_baud: self.mavlink.baud, - vertical_sensor_baud: Some(self.vertical.baud), - status_led: F722StatusLedRoute::Pd2ActiveLow, - } - } - } - }; -} - -finish_mpu6000_hardware!(NoVerticalSensor, f722::VerticalRoute::None, no_serial_baud); -finish_mpu6000_hardware!(Mtf01Uart4, f722::VerticalRoute::Mtf01, serial_baud); -finish_mpu6000_hardware!( - LidarLiteV3I2c1, - f722::VerticalRoute::LidarLiteV3, - no_serial_baud -); - -macro_rules! finish_icm42688_hardware { - ($vertical:ty, $route:expr, no_serial_baud) => { - impl - F722HardwareBuilder< - Pb0Pb1Pb4Pb5Low, - MountedIcm42688, - MavlinkUsart2, - $vertical, - Pa13ActiveHigh, - > - { - pub const fn telemetry_only(self) -> F722Hardware { - F722Hardware { - profile: F722Profile::Icm42688Spi3Usart2, - vertical: $route, - imu_mount: self.imu.mount, - mpu6000_config: None, - mavlink_baud: self.mavlink.baud, - vertical_sensor_baud: None, - status_led: F722StatusLedRoute::Pa13ActiveHigh, - } - } - } - }; - ($vertical:ty, $route:expr, serial_baud) => { - impl - F722HardwareBuilder< - Pb0Pb1Pb4Pb5Low, - MountedIcm42688, - MavlinkUsart2, - $vertical, - Pa13ActiveHigh, - > - { - pub const fn telemetry_only(self) -> F722Hardware { - F722Hardware { - profile: F722Profile::Icm42688Spi3Usart2, - vertical: $route, - imu_mount: self.imu.mount, - mpu6000_config: None, - mavlink_baud: self.mavlink.baud, - vertical_sensor_baud: Some(self.vertical.baud), - status_led: F722StatusLedRoute::Pa13ActiveHigh, - } - } - } - }; -} - -finish_icm42688_hardware!(NoVerticalSensor, f722::VerticalRoute::None, no_serial_baud); -finish_icm42688_hardware!(Mtf01Uart4, f722::VerticalRoute::Mtf01, serial_baud); -finish_icm42688_hardware!( - LidarLiteV3I2c1, - f722::VerticalRoute::LidarLiteV3, - no_serial_baud -); - -/// Literal immutable root embedded by each F722 board package. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct F722ImmutableConfig { +pub struct F722ImmutableConfig { pub name: &'static str, pub service_hz: usize, - pub hardware: F722Hardware, + pub hardware: F722Hardware, } mod sealed { + pub trait Route {} + impl Route for super::Mpu6000Spi1 {} + impl Route for super::Icm42688Spi3 {} + pub trait ImmutableConfig {} - impl ImmutableConfig for super::F722ImmutableConfig {} + impl ImmutableConfig for super::F722ImmutableConfig {} } -/// Sealed associated-type bound preserving the visible two-root board -/// contract while requiring the concrete platform manifest above. +/// Restricts board declarations to the platform's configuration types. #[doc(hidden)] pub trait F7ImmutableConfig: Copy + sealed::ImmutableConfig { - fn f722(&self) -> &F722ImmutableConfig; + type Route: F722Route; + fn f722(&self) -> &F722ImmutableConfig; } -impl F7ImmutableConfig for F722ImmutableConfig { - fn f722(&self) -> &F722ImmutableConfig { +impl F7ImmutableConfig for F722ImmutableConfig { + type Route = R; + + fn f722(&self) -> &F722ImmutableConfig { self } } @@ -559,22 +236,13 @@ pub trait F7Board { const IMMUTABLE_CONFIG: Self::ImmutableConfig; } -/// Installs the one platform-owned RTIC application for a declarative board. +/// Installs the F722 RTIC application for a board. /// -/// Board crates can only select one of the reviewed physical-sensor telemetry -/// route descriptors; RTIC ownership stays platform-private. -/// -/// The application binds the finite superset of interrupt roles needed by the -/// currently supported F722 profiles. The route declaration selects which -/// roles are initialized; inactive bindings acknowledge nothing. Adding an -/// interrupt role is therefore a platform-only change, not a board task graph. +/// The board's route selects hardware initialization and interrupt handlers. +/// Inactive bindings do not access peripherals. #[macro_export] macro_rules! flight_firmware { ($board:ty) => { $crate::__f722_flight_application!($board); }; } - -/// Link anchor for the platform-owned F722 board-I/O application. -#[doc(hidden)] -pub fn firmware_anchor() {} diff --git a/firmware/platforms/stm32f7/tests/platform_safety_boundary.rs b/firmware/platforms/stm32f7/tests/platform_safety_boundary.rs index 2569575..8f4ee08 100644 --- a/firmware/platforms/stm32f7/tests/platform_safety_boundary.rs +++ b/firmware/platforms/stm32f7/tests/platform_safety_boundary.rs @@ -1,415 +1,132 @@ +//! Source checks only for hardware boundaries that host execution cannot probe. use std::{fs, path::PathBuf}; - -use platform_stm32f7::{ - AccelerometerRange, DigitalLowPassFilter, F722Hardware, F722ImmutableConfig, GyroscopeRange, - ImuMount, Mavlink, Mpu6000, Mpu6000Config, Pa4, Pb0Pb1Pb4Pb5Low, Pd2ActiveLow, VerticalSensor, -}; - -fn platform_source(relative: &str) -> String { - let root = PathBuf::from(env!("CARGO_MANIFEST_DIR")); - fs::read_to_string(root.join(relative)).expect("platform source must exist") +fn source(relative: &str) -> String { + fs::read_to_string(PathBuf::from(env!("CARGO_MANIFEST_DIR")).join(relative)).unwrap() } - #[test] -fn former_motor_pins_are_inhibited_before_all_other_initialization() { - let module = platform_source("src/f722/mod.rs"); - let interlock = module - .find("motor_inhibit::inhibit_pb0_pb1_pb4_pb5") - .expect("F722 initialization must claim the former motor pins first"); - let validation = module - .find("validate_board_config") - .expect("F722 initialization must validate its board configuration"); - let board_initialization = module - .find("board::initialize") - .expect("F722 initialization must subsequently claim telemetry I/O"); - - assert!(interlock < validation); - assert!(validation < board_initialization); - - let interlock = platform_source("src/f722/motor_inhibit.rs"); - let reset = interlock - .find("port.bsrr()") - .expect("interlock must reset the former motor-pin output latches"); - let clock_delay = interlock - .find("rcc.ahb1enr().read()") - .expect("GPIO access must follow the documented RCC synchronization delay"); - let output_mode = interlock - .find("port.moder()") - .expect("interlock must configure the former motor pins as GPIO outputs"); +fn motor_pins_are_inhibited_before_initialization() { + let module = source("src/f722/mod.rs"); + let interlock = module.find("motor_inhibit::inhibit(").unwrap(); + let validation = module.find("board::validate_board_config").unwrap(); + let initialize = module.find("board::initialize").unwrap(); + assert!(interlock < validation && validation < initialize); + let interlock = source("src/f722/motor_inhibit.rs"); assert!( - clock_delay < reset && reset < output_mode, - "output latches must be low before GPIO output mode is selected" + interlock.find("rcc.ahb1enr().read()").unwrap() < interlock.find("port.bsrr()").unwrap() ); - for pin in ["br0", "br1", "br4", "br5"] { - assert!(interlock.contains(pin), "interlock must reset PB pin {pin}"); - } - for pin in ["moder0", "moder1", "moder4", "moder5"] { - assert!( - interlock.contains(pin), - "interlock must configure PB pin {pin}" - ); - } - for pin in ["ot0", "ot1", "ot4", "ot5"] { - assert!( - interlock.contains(pin), - "interlock must set PB pin {pin} push-pull" - ); - } - for pin in ["ospeedr0", "ospeedr1", "ospeedr4", "ospeedr5"] { - assert!( - interlock.contains(pin), - "interlock must set PB pin {pin} low speed" - ); - } + assert!(interlock.find("port.bsrr()").unwrap() < interlock.find("port.moder()").unwrap()); } #[test] -fn immutable_board_policy_is_one_complete_literal_manifest() { - const IMMUTABLE: F722ImmutableConfig = F722ImmutableConfig { - name: "test", - service_hz: 100, - hardware: F722Hardware::new() - .output_inhibit(Pb0Pb1Pb4Pb5Low) - .imu( - Mpu6000::on_spi1() - .with_drdy(Pa4) - .mounted(ImuMount::BodyAligned) - .configured(Mpu6000Config { - accelerometer: AccelerometerRange::PlusMinus4G, - gyroscope: GyroscopeRange::PlusMinus500DegreesPerSecond, - digital_low_pass_filter: DigitalLowPassFilter::Bandwidth42Hz, - sample_rate_divider: 9, - }), - ) - .mavlink(Mavlink::on_usart1().baud(115_200)) - .vertical_sensor(VerticalSensor::mtf01_on_uart4().baud(115_200)) - .status_led(Pd2ActiveLow) - .telemetry_only(), - }; - - assert_eq!(IMMUTABLE.name, "test"); - assert_eq!(IMMUTABLE.service_hz, 100); -} - -#[test] -fn telemetry_platform_has_no_physical_actuator_peripheral_path() { +fn board_images_have_no_actuator_route() { let sources = [ - platform_source("src/lib.rs"), - platform_source("src/f722/mod.rs"), - platform_source("src/f722/board.rs"), - platform_source("src/f722/firmware.rs"), - platform_source("src/f722/interrupts.rs"), + "src/lib.rs", + "src/f722/mod.rs", + "src/f722/board.rs", + "src/f722/firmware.rs", + "src/f722/interrupts.rs", ] + .map(source) .join("\n"); - for forbidden in [ "mod actuator", "MotorOutput", "MotorRoute", - "MotorOutputs", "TIM3", "TIM5", "Dshot", "Pwm", - "device.TIM3", - "device.TIM5", ] { - assert!( - !sources.contains(forbidden), - "telemetry platform must not expose actuator path {forbidden}" - ); + assert!(!sources.contains(forbidden), "{forbidden}"); } - - let manifest = platform_source("Cargo.toml"); - assert!( - !manifest.contains("kaptan-actuator"), - "telemetry platform must not link the physical actuator crate" - ); + assert!(!source("Cargo.toml").contains("kaptan-actuator")); } - #[test] -fn ordinary_f722_image_has_direct_configuration_after_output_inhibition() { - let board = platform_source("src/f722/board.rs"); - let firmware = platform_source("src/f722/firmware.rs"); - let routes = platform_source("src/f722/routes.rs"); - let manifest = platform_source("Cargo.toml"); - - assert!( - !manifest.contains("development-config") - && !manifest.contains("development-authority") - && !manifest.contains("development-unsigned"), - "ordinary board images must not hide configuration behind a development feature" - ); - for required in [ - "config_file: F722ConfigFile", - "file_service: FileService", - "uart_receiver: IntegrityMavlinkReceiver<1>", - "file_lease_ticks: u16", - "TRANSFER_REPLAY_LEASE_SECONDS", - "let Some(message) = frame.decode()", - "wire::decode_request", - ".dispatch(&mut self.config_file", - "send_file_response", - "let class = kaptan_mavlink::transmitter::FrameClass::Critical", - "control.status != FileStatus::RequestConflict", +fn uart_runtime_has_no_dma_or_static_register_aliases() { + for path in [ + "src/f722/serial.rs", + "src/f722/peripherals/usart.rs", + "src/f722/data_channel.rs", ] { - assert!( - board.contains(required), - "ordinary image must retain configuration composition: {required}" - ); - } - assert!(!board.contains("DirectConfigAdmission")); - assert!( - routes.contains("pub(crate) struct Mpu6000Spi1Route {\n spi: Spi1,\n}") - && routes.contains("stm32f722::GPIOB::ptr()") - && routes.contains("write.br2().set_bit()") - && routes.contains("write.bs2().set_bit()"), - "the MPU runtime may drive only PB2 atomically, not retain GPIOB ownership" - ); - assert!(!board.contains("development-config")); - assert!( - !firmware.contains("shared = [mavlink, motors]"), - "the MAVLink worker must not share a motor resource" - ); - assert!( - firmware.contains("const MAVLINK_EVENT_RECEIVED") - && firmware.contains("mavlink.receive_mavlink_interrupt(event)") - && !firmware.contains("development-config"), - "ordinary images must always dispatch received MAVLink frames" - ); -} - -#[test] -fn flash_busy_deadline_resets_entirely_from_sram() { - let config = platform_source("src/f722/config.rs"); - assert!(config.contains("link_section = \".data.kaptan_flash_operation\"")); - assert!(config.contains("fn execute(&self, operation: F722FlashOperation)")); - assert!(config.contains("\"mrs {primask}, PRIMASK\"")); - assert!(config.contains("\"cpsid i\"")); - assert!(config.contains("\"cpsie i\"")); - assert!(config.contains("fn reset_after_flash_timeout() -> !")); - assert!(config.contains("SYSRESETREQ")); - assert!(config.contains("core::arch::asm!(\"dsb sy\"")); - assert!(!config.contains("BusyTimeout")); -} - -#[test] -fn usart_tx_dma_arms_each_stream_before_its_request() { - let usart = platform_source("src/f722/peripherals/usart.rs"); - assert!(usart.contains("disable_dma_transmit_from_interrupt")); - - for (name, path) in [ - ("USART1", "src/f722/peripherals/usart1_tx_dma2_stream7.rs"), - ("USART2", "src/f722/peripherals/usart2_tx_dma1_stream6.rs"), - ] { - let route = platform_source(path); - let constructor_start = route.find("pub fn new").expect("DMA constructor"); - let transfer_start = route.find("pub fn start").expect("DMA start operation"); - let acknowledge_start = route - .find("pub fn acknowledge_interrupt") - .expect("DMA interrupt acknowledgement"); - - assert!( - !route[constructor_start..transfer_start].contains("enable_dma_transmit"), - "{name} must not leave its DMA request armed between transfers" - ); - - assert!( - route.contains("write.dmdis().enabled().feie().disabled()"), - "{name} must explicitly select direct mode without FE interrupts" - ); - let start = &route[transfer_start..acknowledge_start]; - let stream_enable = start - .find("write.en().enabled()") - .expect("DMA stream must be enabled"); - let request_enable = start - .find("self.usart.enable_dma_transmit()") - .expect("USART DMA request must be enabled"); - assert!( - stream_enable < request_enable, - "{name} must enable its stream before the peripheral request" - ); - - let acknowledge = &route[acknowledge_start..]; - let stream_disable = acknowledge - .find("write.en().disabled()") - .expect("DMA stream must be disabled"); - let request_disable = acknowledge - .find("disable_dma_transmit_from_interrupt()") - .expect("USART DMA request must be disabled"); - assert!( - stream_disable < request_disable, - "{name} must disable its stream before the peripheral request" - ); - - let classifier = acknowledge - .find("impl DmaTerminalStatus") - .map(|index| &acknowledge[index..]) - .expect("DMA terminal classifier"); - assert!(classifier.contains("self.transfer_error || self.direct_mode_error")); - assert!(classifier.contains("TxCompletion::Complete")); - assert!( - !classifier.contains("fifo_error"), - "{name} direct-mode FEIF must not manufacture a terminal fault" - ); - } -} - -#[test] -fn configuration_schema_is_generated_without_board_forwarding_code() { - let platform = platform_source("src/lib.rs"); - let platform_config = platform_source("src/f722/config.rs"); - let platform_firmware = platform_source("src/f722/firmware.rs"); - let kakute = platform_source("../../boards/kakute-f7-mini-v3/src/board.rs"); - let geprc = platform_source("../../boards/geprc-f722-v2/src/board.rs"); - - assert!( - platform.contains("pub struct F722ImmutableConfig") - && platform.contains("pub hardware: F722Hardware") - && platform.contains("type UserConfig: Clone + Config") - && platform_config.contains("struct UserConfigModel") - && platform_config.contains("fn project_runtime") - && platform_config.contains("candidate.field_value(field)"), - "the platform must retain exactly one immutable manifest and one generic user adapter" - ); - for semantic_id in [ - "MAVLINK_SYSTEM_ID", - "MAVLINK_COMPONENT_ID", - "IMU_HZ", - "HEARTBEAT_HZ", - "MAVLINK_LOG_LEVEL_FILTER", - "RANGE_HZ", - "OPTICAL_FLOW_HZ", - ] { - assert!( - platform_config.contains(&format!("const {semantic_id}: FieldId")), - "missing stable platform-consumed semantic ID {semantic_id}" - ); - } - for (name, board) in [("Kakute", kakute), ("GEPRC", geprc)] { - assert!( - board.contains("type UserConfig = UserConfig;") - && board.contains("type ImmutableConfig = ImmutableConfig;") - && board.contains("const FACTORY_USER_CONFIG: UserConfig = UserConfig {") - && board.contains("const IMMUTABLE_CONFIG: ImmutableConfig = ImmutableConfig {") - && board.contains("hardware: F722Hardware::new()") - && board.contains(".telemetry_only()"), - "{name} must visibly declare the two literal roots" - ); - for redundant in [ - "impl F7ImmutableConfig for", - "impl F7UserConfig for", - "impl_f722_user_config!", + let code = source(path); + for forbidden in [ + "DMA1::ptr", + "DMA2::ptr", + "USART1::ptr", + "USART2::ptr", + "USART3::ptr", + "UART4::ptr", + "USART6::ptr", + "DmaReadableBuffer", + "MavlinkTransmitter", + "dmat()", ] { - assert!(!board.contains(redundant), "{name} retains {redundant}"); + assert!(!code.contains(forbidden), "{path}: {forbidden}"); } } - assert!( - platform_firmware.contains("<$board as $crate::F7Board>::UserConfig") - && platform_firmware.contains("<$board as $crate::F7Board>::ImmutableConfig") - && platform_firmware.contains("config_snapshot_length::<"), - "the common RTIC graph must specialize storage for the two board roots" - ); + let firmware = source("src/f722/firmware.rs"); + assert!(!firmware.contains("DMA2_STREAM7")); + assert!(!firmware.contains("DMA1_STREAM6")); + assert_eq!(firmware.matches("imu_worker::spawn()").count(), 1); + assert_eq!(firmware.matches("mavlink_worker::spawn()").count(), 1); +} +#[test] +fn source_keeps_atomic_chip_select_without_gpio_owner() { + let routes = source("src/f722/routes.rs"); + let compact: String = routes.split_whitespace().collect(); + assert!(compact.contains("pub(crate)structMpu6000Spi1Route{spi:Spi1,}")); + for bit in ["br2", "bs2", "br15", "bs15"] { + assert!(routes.contains(&format!("write.{bit}().set_bit()"))); + } +} +#[test] +fn source_keeps_flash_irq_mask_and_reset() { + let config = source("src/f722/config.rs"); + for required in [ + "link_section = \".data.kaptan_flash_operation\"", + "\"mrs {primask}, PRIMASK\"", + "\"cpsid i\"", + "\"cpsie i\"", + "fn reset_after_flash_timeout() -> !", + "SYSRESETREQ", + "core::arch::asm!(\"dsb sy\"", + ] { + assert!(config.contains(required), "{required}"); + } + assert!(!config.contains("BusyTimeout")); +} +#[test] +fn normal_images_keep_usb_configuration_and_shared_file_service() { + let manifest = source("Cargo.toml"); + assert!(!manifest.contains("development-config")); + let board = source("src/f722/board.rs"); + assert_eq!(board.matches("FileService::new").count(), 1); + assert!(board.contains("usb_mavlink(")); + assert!(board.contains("IngressId(id)")); + assert!(board.contains("result.refresh_lease")); + assert!(!board.contains("file_lease_ticks")); } #[test] -fn f722_link_budget_reserves_only_deterministic_telemetry() { - let board = platform_source("src/f722/board.rs"); - assert!(board.contains("SCHEDULED_TELEMETRY_BYTES_PER_SECOND")); - assert!(board.contains("SERVICE_HZ * SCHEDULED_FRAME_MAXIMUM_BYTES")); - assert!(board.contains("self.drain_one_mavlink_diagnostic();")); - assert!(board.contains("let mavlink_log_filter = runtime.mavlink_log_level_filter;")); - assert!(board.contains("LogSinkQueue::new(mavlink_log_filter)")); - assert!(!board.contains("mavlink_logging_enabled")); - assert!(!board.contains("LogFilter")); - for periodic_diagnostic_policy in [ - "ERROR_RECORDS_PER_SECOND", - "LOG_RECORDS_PER_SECOND", - "WORST_CASE_OBSERVABILITY_BYTES_PER_SECOND", - "ERROR_FRAME_MAXIMUM_BYTES", - "LOG_FRAME_MAXIMUM_BYTES", - "budget_ticks", - "error_budget", - "log_budget", - ] { - assert!( - !board.contains(periodic_diagnostic_policy), - "diagnostics must not have a periodic policy: {periodic_diagnostic_policy}" - ); +fn source_keeps_protocol_local_and_irq_priorities() { + let firmware = source("src/f722/firmware.rs"); + let shared = + &firmware[firmware.find("struct Shared").unwrap()..firmware.find("#[local]").unwrap()]; + assert!(!shared.contains("mavlink:")); + for owner in ["uarts:", "usb:", "serial_clock:"] { + assert!(shared.contains(owner)); } -} - -#[test] -fn mavlink_transport_faults_do_not_recurse_or_retain_dead_accounting() { - let board = platform_source("src/f722/board.rs"); - let transport = platform_source("src/f722/data_channel.rs"); - let portable_channel = platform_source("../../libs/kaptan-data-channel/src/lib.rs"); - - assert!( - !board.contains("mavlink transmit dma fault"), - "the MAVLink sink must not report its own transmit failure through itself" - ); - - for recursive_path in [ - "drain_transport_faults", - "take_transport_fault", - "TransmitMemory", - "TransmitEngine", - "TransmitTerminal", - "pending_faults", - ] { - assert!( - !board.contains(recursive_path) - && !transport.contains(recursive_path) - && !portable_channel.contains(recursive_path), - "MAVLink transport must not feed {recursive_path} back into itself" - ); + assert!(firmware.contains("context.local.mavlink.process_worker_events")); + assert!(!firmware.contains(".lock(|mavlink|")); + assert!(firmware.contains("priority = 7, local = [mavlink]")); + for vector in ["USART1", "USART2", "USART3", "UART4", "USART6"] { + assert!(firmware.contains(&format!("binds = {vector}, priority = 14"))); } - for dead_counter in [ - "dma_fault_count", - "memory_fault_count", - "engine_fault_count", - "terminal_fault_count", - ] { - assert!( - !transport.contains(dead_counter), - "unused transport accounting must stay absent: {dead_counter}" - ); - } -} - -#[test] -fn f722_producers_keep_logs_and_reportable_errors_separate() { - let board = platform_source("src/f722/board.rs"); - let config = platform_source("src/f722/config.rs"); - let logger = platform_source("../../libs/kaptan-log/src/facade.rs"); - let errors = platform_source("../../libs/kaptan-err-reportable/src/ingress.rs"); - - for macro_name in [ - "kaptan_log::debug!", - "kaptan_log::info!", - "kaptan_log::warn!", - "kaptan_log::error!", - ] { - assert!( - board.contains(macro_name), - "f722 does not exercise {macro_name}" - ); - } - assert!(board.contains("#[kaptan_err_reportable::propagate]")); - assert!( - config.contains("enum PersistConfigurationError {") - && config.contains("#[kaptan_error(id = 2210)]") - && config.contains("#[kaptan_error(id = 2209)]") - && config.contains("impl From>") - && config.contains("fn persist_candidate(") - && config.contains("#[kaptan_err_reportable::propagate]") - ); - assert!(!board.contains("report_error")); - assert!(!board.contains("report_static_error")); - assert!(logger.contains("GLOBAL_INGRESS_CAPACITY")); - assert!(logger.contains("compare_exchange")); - assert!(errors.contains("AtomicU8")); - assert!(errors.contains("UnsafeCell>")); - assert!(errors.contains("compare_exchange")); + assert!(firmware.contains("binds = OTG_FS, priority = 8, shared = [usb]")); + assert!(firmware.contains("binds = TIM2, priority = 10")); + assert!(firmware.contains("local = [timer], shared = [lidar]")); + let timer_irq = &firmware + [firmware.find("fn tim2(").unwrap()..firmware.find("async fn imu_worker").unwrap()]; + assert!(timer_irq.contains("if !context.local.timer.on_interrupt()")); + assert!(timer_irq.find(".on_interrupt()").unwrap() < timer_irq.find(".lidar.lock").unwrap()); } diff --git a/firmware/platforms/stm32f7/tests/config_active_state.rs b/firmware/platforms/stm32f7/tests/support/config_active_state.rs similarity index 72% rename from firmware/platforms/stm32f7/tests/config_active_state.rs rename to firmware/platforms/stm32f7/tests/support/config_active_state.rs index 64611de..d80d1d9 100644 --- a/firmware/platforms/stm32f7/tests/config_active_state.rs +++ b/firmware/platforms/stm32f7/tests/support/config_active_state.rs @@ -1,15 +1,11 @@ -#![allow(dead_code)] - use kaptan_config::{Config, ConfigManager, ConfigModel, ConfigStore, ConfigValue, FieldId}; use platform_stm32f7::{ - F7ImmutableConfig, F722Hardware, F722ImmutableConfig, ImuMount, LevelFilter, Mavlink, Mpu6000, - Mpu6000Config, Pa4, Pb0Pb1Pb4Pb5Low, Pd2ActiveLow, VerticalSensor, + F722Hardware, ImuMount, LevelFilter, MotorPins, Mpu6000Config, Mpu6000Spi1, SerialInventory, + StatusLed, VerticalSensor, }; -// Include the implementation as a child module so this platform-internal test -// can exercise the crate-private user-candidate adapter without flash hardware. -#[path = "../src/f722/config.rs"] -mod target_config; +type F722ImmutableConfig = platform_stm32f7::F722ImmutableConfig; +use crate::f722::config as target_config; use target_config::UserConfigModel; @@ -67,23 +63,21 @@ struct TestUserConfig { const IMMUTABLE: F722ImmutableConfig = F722ImmutableConfig { name: "active-candidate-test", service_hz: 100, - hardware: F722Hardware::new() - .output_inhibit(Pb0Pb1Pb4Pb5Low) - .imu( - Mpu6000::on_spi1() - .with_drdy(Pa4) - .mounted(ImuMount::BodyAligned) - .configured(Mpu6000Config { - accelerometer: platform_stm32f7::AccelerometerRange::PlusMinus4G, - gyroscope: platform_stm32f7::GyroscopeRange::PlusMinus500DegreesPerSecond, - digital_low_pass_filter: platform_stm32f7::DigitalLowPassFilter::Bandwidth42Hz, - sample_rate_divider: 9, - }), - ) - .mavlink(Mavlink::on_usart1().baud(115_200)) - .vertical_sensor(VerticalSensor::none()) - .status_led(Pd2ActiveLow) - .telemetry_only(), + hardware: F722Hardware::new( + Mpu6000Spi1 { + mount: ImuMount::BodyAligned, + config: Mpu6000Config { + accelerometer: platform_stm32f7::AccelerometerRange::PlusMinus4G, + gyroscope: platform_stm32f7::GyroscopeRange::PlusMinus500DegreesPerSecond, + digital_low_pass_filter: platform_stm32f7::DigitalLowPassFilter::Bandwidth42Hz, + sample_rate_divider: 9, + }, + }, + SerialInventory::new(&[]), + StatusLed::ActiveLowPd2, + VerticalSensor::None, + MotorPins::Four, + ), }; struct OneRecord(Option); @@ -158,13 +152,13 @@ fn mavlink_logging_policy_is_projected_and_validated_independently() { 9, "runtime projection must retain narrow board-native scalar widths" ); - let mut candidate = UserConfigModel::::new(factory()); + let mut candidate = UserConfigModel::::new(factory()); candidate .set_field(FieldId::new(105), ConfigValue::U8(0)) .unwrap(); let active = candidate.validate(&IMMUTABLE).unwrap(); - let runtime = target_config::project_runtime(&active, &IMMUTABLE).unwrap(); + let runtime = target_config::project_runtime(&active.candidate, &IMMUTABLE).unwrap(); assert_eq!(runtime.mavlink_log_level_filter, LevelFilter::Off); } @@ -188,43 +182,39 @@ fn runtime_projection_rejects_missing_and_wrong_kind_values() { } #[test] -fn runtime_construction_reads_the_persisted_board_owned_user_root() { - let factory = UserConfigModel::::new(factory()); +fn active_projection_retains_the_persisted_board_owned_user_root() { + let factory = UserConfigModel::::new(factory()); let mut persisted = factory.clone(); persisted .set_field(FieldId::new(1000), ConfigValue::U8(42)) .expect("driver field must accept a valid persisted value"); - let manager = ConfigManager::recover_compatible_or_factory( - factory, - IMMUTABLE, - OneRecord(Some(persisted)), - ) - .expect("synthetic persisted user configuration must recover"); + let manager = ConfigManager::recover(factory, IMMUTABLE, OneRecord(Some(persisted))) + .expect("synthetic persisted user configuration must recover"); assert_eq!( - manager.active().field_value(FieldId::new(1000)), + manager.active().candidate.field_value(FieldId::new(1000)), Some(ConfigValue::U8(42)), - "runtime construction must receive the validated persisted field" + "the active projection must retain the validated persisted field" ); } #[test] fn committing_a_user_edit_does_not_mutate_the_boot_active_view() { - let factory = UserConfigModel::::new(factory()); + let factory = UserConfigModel::::new(factory()); let mut manager = ConfigManager::recover(factory, IMMUTABLE, OneRecord(None)).unwrap(); - let mut replacement = manager.committed().clone(); + let mut replacement = manager.committed().unwrap().clone(); replacement .set_field(FieldId::new(1000), ConfigValue::U8(42)) .unwrap(); manager.replace(replacement).unwrap(); assert_eq!( - manager.active().field_value(FieldId::new(1000)), + manager.active().candidate.field_value(FieldId::new(1000)), Some(ConfigValue::U8(7)) ); assert_eq!( - manager.committed().field_value(FieldId::new(1000)), + manager.committed().unwrap().field_value(FieldId::new(1000)), Some(ConfigValue::U8(42)) ); } diff --git a/firmware/platforms/stm32f7/tests/support/config_files.rs b/firmware/platforms/stm32f7/tests/support/config_files.rs new file mode 100644 index 0000000..bfddd3f --- /dev/null +++ b/firmware/platforms/stm32f7/tests/support/config_files.rs @@ -0,0 +1,775 @@ +use kaptan_config::{ + ConfigManager, ConfigModel, ConfigStore, ConfigValue, DualSlotJournal, DualSlotNor, FieldId, + KcfgExporter, KcfgImporter, KcfgResultDetail, ReplaceError, decode_kcfg, encode_edited_kcfg, +}; +use kaptan_file_mavlink::{ + Content, ControlRequest, ControlResponse, DATA_BYTES, DataRequest, DispatchResult, + FileOperation, FileProvider, FileRequest, FileResponse, FileService, FileStatus, IngressId, + Principal, Target, TransferPeer, crc32c, wire, +}; +use kaptan_ports::{PortId, Role}; +use platform_stm32f7::{F7Board, F7ImmutableConfig}; +extern crate std; +use crate::f722::{config as target_config, geprc, kakute}; +use std::{cell::RefCell, rc::Rc, vec, vec::Vec}; +use target_config::{ + BootDiagnostic, F722ConfigFile, F722FlashError, UserConfigModel, recover_configuration, +}; + +type Board = kakute::KakuteF7MiniV3; +type C = ::UserConfig; +type I = ::ImmutableConfig; +const PORT_COUNT: usize = Board::IMMUTABLE_CONFIG.hardware.serial().len(); +type Model = UserConfigModel; +const N: usize = platform_stm32f7::config_snapshot_length::(); +type Store = DualSlotJournal; +type Manager = ConfigManager; +type Provider = F722ConfigFile; + +struct FlashState { + bytes: [[u8; 2048]; 2], + operations: usize, + bytes_read: usize, + fail_after: Option, + fail_after_commit: bool, + fail_reads: bool, +} +impl Default for FlashState { + fn default() -> Self { + Self { + bytes: [[0xff; 2048]; 2], + operations: 0, + bytes_read: 0, + fail_after: None, + fail_after_commit: false, + fail_reads: false, + } + } +} +#[derive(Clone, Default)] +struct RamNor(Rc>); +impl RamNor { + fn operation(state: &mut FlashState) -> Result<(), F722FlashError> { + if state.fail_after == Some(state.operations) { + return Err(F722FlashError::Controller); + } + state.operations += 1; + Ok(()) + } +} +impl DualSlotNor for RamNor { + type Error = F722FlashError; + fn slot_size(&self) -> usize { + 2048 + } + fn read(&mut self, slot: usize, offset: usize, output: &mut [u8]) -> Result<(), Self::Error> { + let mut state = self.0.borrow_mut(); + if state.fail_reads { + return Err(F722FlashError::Controller); + } + state.bytes_read += output.len(); + output.copy_from_slice(&state.bytes[slot][offset..offset + output.len()]); + Ok(()) + } + fn erase_slot(&mut self, slot: usize) -> Result<(), Self::Error> { + let mut state = self.0.borrow_mut(); + Self::operation(&mut state)?; + state.bytes[slot].fill(0xff); + Ok(()) + } + fn program_word(&mut self, slot: usize, offset: usize, word: u32) -> Result<(), Self::Error> { + let mut state = self.0.borrow_mut(); + Self::operation(&mut state)?; + for (byte, value) in state.bytes[slot][offset..offset + 4] + .iter_mut() + .zip(word.to_le_bytes()) + { + assert_eq!(*byte & value, value); + *byte = value; + } + // Fault after the durable marker, before journal verification. + if state.fail_after_commit && word == u32::from_le_bytes(*b"DONE") { + return Err(F722FlashError::Verification); + } + Ok(()) + } +} +fn factory() -> Model { + Model::new(Board::FACTORY_USER_CONFIG) +} +fn recover(flash: &RamNor) -> Manager { + recover_configuration( + factory(), + Board::IMMUTABLE_CONFIG, + Store::new(flash.clone()), + ) + .unwrap() +} +fn artifact(candidate: &Model) -> Vec { + let factory = factory(); + let export = KcfgExporter::new(candidate, &factory, false).unwrap(); + let mut bytes = vec![0; export.length()]; + assert_eq!(export.read(0, &mut bytes).unwrap(), bytes.len()); + bytes +} +fn decode(bytes: &[u8]) -> Model { + let factory = factory(); + let mut importer = KcfgImporter::new(&factory).unwrap(); + for chunk in bytes.chunks(37) { + importer.push(&factory, chunk).unwrap(); + } + importer.finish().unwrap().candidate +} +const TARGET: Target = Target::new(1, 1); +const PEER: TransferPeer = TransferPeer { + principal: Principal::new(42, 7), + ingress: IngressId(0), +}; +fn control(operation: FileOperation, position: u32, path: &[u8], bytes: &[u8]) -> FileRequest { + FileRequest::Control(ControlRequest { + operation, + target: TARGET, + position, + file_size: bytes.len() as u32, + checksum_crc32c: if bytes.is_empty() { 0 } else { crc32c(bytes) }, + transfer_id: 9, + content: Content::try_new(path).unwrap(), + }) +} +fn exchange( + service: &mut FileService, + provider: &mut impl FileProvider, + peer: TransferPeer, + request: FileRequest, +) -> DispatchResult { + let request = wire::decode_request(&wire::encode_request(request)).unwrap(); + let result = service.dispatch(provider, peer, request).unwrap(); + assert_eq!( + wire::decode_response(&wire::encode_response(result.response)).unwrap(), + result.response + ); + result +} +fn accepted(result: DispatchResult) -> Result { + let FileResponse::Control(response) = result.response else { + panic!("control response expected"); + }; + if response.status != FileStatus::Ok { + return Err(response.status); + } + assert!(result.refresh_lease); + Ok(response) +} +fn read_result(provider: &mut impl FileProvider, path: &str) -> Result, FileStatus> { + let mut service = FileService::new(TARGET); + let metadata = accepted(exchange( + &mut service, + provider, + PEER, + control(FileOperation::OpenRead, 0, path.as_bytes(), &[]), + ))?; + let mut bytes = vec![0; metadata.file_size as usize]; + for (i, chunk) in bytes.chunks_mut(DATA_BYTES).enumerate() { + let result = exchange( + &mut service, + provider, + PEER, + control(FileOperation::Read, (i * DATA_BYTES) as u32, &[], &[]), + ); + assert!(result.refresh_lease); + let FileResponse::Data(response) = result.response else { + panic!("data response expected"); + }; + chunk.copy_from_slice(&response.data[..chunk.len()]); + } + assert_eq!(crc32c(&bytes), metadata.checksum_crc32c); + accepted(exchange( + &mut service, + provider, + PEER, + control(FileOperation::Close, bytes.len() as u32, &[], &[]), + ))?; + Ok(bytes) +} +fn read(provider: &mut impl FileProvider, path: &str) -> Vec { + read_result(provider, path).unwrap() +} +fn write_to(provider: &mut Provider, path: &[u8], candidate: &Model) -> Result<(), FileStatus> { + let base = read_result(provider, "/config/user.kcfg") + .or_else(|_| read_result(provider, "/config/active.kcfg"))?; + upload(provider, path, &edit(&base, candidate)).map(|_| ()) +} +fn edit(base: &[u8], candidate: &Model) -> Vec { + let source = decode_kcfg(base, &mut [0; kaptan_config::KCFG_FIELD_ID_SCRATCH_BYTES]).unwrap(); + let mut edited = vec![0; base.len()]; + encode_edited_kcfg(&source, false, &mut edited, |field| { + candidate.field_value(field.id()).unwrap() + }) + .unwrap(); + edited +} +fn upload(provider: &mut Provider, path: &[u8], bytes: &[u8]) -> Result { + let mut service = FileService::new(TARGET); + accepted(exchange( + &mut service, + provider, + PEER, + control(FileOperation::OpenWrite, 0, path, bytes), + ))?; + for (i, chunk) in bytes.chunks(DATA_BYTES).enumerate() { + let mut data = [0; DATA_BYTES]; + data[..chunk.len()].copy_from_slice(chunk); + let request = FileRequest::Data(DataRequest { + target: TARGET, + transfer_id: 9, + position: (i * DATA_BYTES) as u32, + data, + }); + for id in [1, 2, 4] { + let foreign = exchange( + &mut service, + provider, + TransferPeer { + ingress: IngressId(id), + ..PEER + }, + request, + ); + assert!(!foreign.refresh_lease); + assert_eq!(accepted(foreign), Err(FileStatus::InvalidTransfer)); + } + let result = exchange(&mut service, provider, PEER, request); + accepted(result)?; + assert_eq!( + exchange(&mut service, provider, PEER, request), + result, + "retry must not append twice" + ); + } + let result = exchange( + &mut service, + provider, + PEER, + control(FileOperation::Close, bytes.len() as u32, &[], &[]), + ); + if let FileResponse::Control(response) = result.response + && response.status == FileStatus::RequestConflict + { + let (_, text) = KcfgResultDetail::decode(response.content.as_bytes()).unwrap(); + assert!( + text.contains("reload"), + "conflict must carry a readable KCFG prefix" + ); + } + accepted(result).map(|response| response.content) +} +fn write(provider: &mut Provider, candidate: &Model) -> Result<(), FileStatus> { + write_to(provider, b"/config/user.kcfg", candidate) +} + +#[test] +fn both_actual_providers_export_only_their_declared_ports() { + fn check() { + let factory = UserConfigModel::::new( + B::FACTORY_USER_CONFIG, + ); + let manager = recover_configuration( + factory.clone(), + B::IMMUTABLE_CONFIG, + DualSlotJournal::<_, BYTES>::new(RamNor::default()), + ) + .unwrap(); + assert_eq!(manager.active().ports.iter().len(), PORTS); + let mut provider = F722ConfigFile::<_, _, PORTS, BYTES, _>::new(manager); + for path in ["/config/user.kcfg", "/config/active.kcfg"] { + let bytes = read(&mut provider, path); + let mut importer = KcfgImporter::new(&factory).unwrap(); + importer.push(&factory, &bytes).unwrap(); + let exported = importer.finish().unwrap().candidate; + for port in [ + platform_stm32f7::UART1, + platform_stm32f7::UART2, + platform_stm32f7::UART3, + platform_stm32f7::UART4, + platform_stm32f7::UART6, + ] { + let available = B::IMMUTABLE_CONFIG + .f722() + .hardware + .serial() + .index(port.id) + .is_some(); + assert_eq!(exported.field_value(port.role_field).is_some(), available); + assert_eq!(exported.field_value(port.baud_field).is_some(), available); + } + } + } + check::(); + check::< + geprc::GeprcF722V2, + { geprc::GeprcF722V2::IMMUTABLE_CONFIG.hardware.serial().len() }, + { + platform_stm32f7::config_snapshot_length::<::UserConfig>( + ) + }, + >(); +} + +#[test] +fn actual_board_kcfg_provider_round_trips_and_active_is_read_only_restart_only() { + let flash = RamNor::default(); + let mut provider = Provider::new(recover(&flash)); + let _initial = read(&mut provider, "/config/active.kcfg"); + let mut next = decode(&read(&mut provider, "/config/user.kcfg")); + next.set_field(FieldId::new(201), ConfigValue::U8(2)) + .unwrap(); + next.set_field(FieldId::new(207), ConfigValue::U8(1)) + .unwrap(); + write(&mut provider, &next).unwrap(); + let active = decode(&read(&mut provider, "/config/active.kcfg")); + let committed = decode(&read(&mut provider, "/config/user.kcfg")); + assert_eq!( + active.field_value(FieldId::new(201)), + Some(ConfigValue::U8(0)) + ); + assert_eq!( + active.field_value(FieldId::new(207)), + Some(ConfigValue::U8(0)) + ); + assert_eq!( + committed.field_value(FieldId::new(201)), + Some(ConfigValue::U8(2)) + ); + assert_eq!( + committed.field_value(FieldId::new(207)), + Some(ConfigValue::U8(1)) + ); + assert_eq!( + write_to(&mut provider, b"/config/active.kcfg", &next), + Err(FileStatus::NotFound) + ); + let reboot = recover(&flash); + assert_eq!( + reboot.active().ports.get(PortId::new(4)).unwrap().role, + Role::Mavlink + ); + assert_eq!(reboot.diagnostic(), BootDiagnostic::Healthy); +} + +#[test] +fn latest_readable_invalid_candidate_is_retained_for_usb_repair_without_enabling_uarts() { + let flash = RamNor::default(); + let mut store = Store::new(flash.clone()); + store.persist(&factory()).unwrap(); + let mut invalid = factory(); + invalid + .set_field(FieldId::new(201), ConfigValue::U8(2)) + .unwrap(); + invalid + .set_field(FieldId::new(207), ConfigValue::U8(2)) + .unwrap(); + let error = invalid.validate(&Board::IMMUTABLE_CONFIG).err().unwrap(); + assert_eq!(error.primary(), Some(FieldId::new(207))); + assert_eq!(error.related_field(), Some(FieldId::new(201))); + store.persist(&invalid).unwrap(); + let manager = recover(&flash); + assert!(matches!( + manager.diagnostic(), + BootDiagnostic::InvalidPersisted(_) + )); + assert!( + manager + .active() + .ports + .iter() + .all(|(_, p)| p.role == Role::Disabled) + ); + assert_eq!( + manager.committed().unwrap().field_value(FieldId::new(201)), + Some(ConfigValue::U8(2)) + ); + let before = flash.0.borrow().operations; + let mut provider = Provider::new(manager); + assert_eq!( + decode(&read(&mut provider, "/config/user.kcfg")).field_value(FieldId::new(201)), + Some(ConfigValue::U8(2)) + ); + assert_eq!( + flash.0.borrow().operations, + before, + "boot and inspection never rewrite the candidate" + ); + assert_eq!( + write(&mut provider, &invalid), + Err(FileStatus::ContentRejected) + ); + assert_eq!(flash.0.borrow().operations, before); + let mut repaired = factory(); + repaired + .set_field(FieldId::new(201), ConfigValue::U8(1)) + .unwrap(); + write(&mut provider, &repaired).unwrap(); + let active = decode(&read(&mut provider, "/config/active.kcfg")); + for id in [201, 207] { + assert_eq!( + active.field_value(FieldId::new(id)), + Some(ConfigValue::U8(0)) + ); + } + assert_eq!( + recover(&flash) + .active() + .ports + .get(PortId::new(1)) + .unwrap() + .role, + Role::Mavlink + ); +} + +#[test] +fn failed_persist_keeps_active_and_invalidates_committed() { + let probe = RamNor::default(); + let mut manager = recover(&probe); + let mut next = factory(); + next.set_field(FieldId::new(201), ConfigValue::U8(1)) + .unwrap(); + manager.replace(next.clone()).unwrap(); + let operations = probe.0.borrow().operations; + for fail_after in 0..operations { + let flash = RamNor::default(); + let mut store = Store::new(flash.clone()); + store.persist(&factory()).unwrap(); + let mut manager = recover(&flash); + { + let mut state = flash.0.borrow_mut(); + state.operations = 0; + state.fail_after = Some(fail_after); + } + assert!(matches!( + manager.replace(next.clone()), + Err(ReplaceError::Store(_)) + )); + assert!(manager.committed().is_none()); + assert_eq!( + manager.active().ports.get(PortId::new(1)).unwrap().role, + Role::Disabled + ); + assert!(matches!( + manager.replace(factory()), + Err(ReplaceError::DurableStateUnknown) + )); + let mut provider = Provider::new(manager); + assert_eq!( + read_result(&mut provider, "/config/user.kcfg"), + Err(FileStatus::StorageFailure) + ); + assert_eq!( + decode(&read(&mut provider, "/config/active.kcfg")).field_value(FieldId::new(201)), + Some(ConfigValue::U8(0)) + ); + flash.0.borrow_mut().fail_after = None; + assert_eq!( + recover(&flash) + .active() + .ports + .get(PortId::new(1)) + .unwrap() + .role, + Role::Disabled + ); + } + let flash = RamNor::default(); + let mut manager = recover(&flash); + flash.0.borrow_mut().fail_after_commit = true; + assert!(matches!(manager.replace(next), Err(ReplaceError::Store(_)))); + assert!(manager.committed().is_none()); + assert_eq!( + manager.active().ports.get(PortId::new(1)).unwrap().role, + Role::Disabled + ); + flash.0.borrow_mut().fail_after_commit = false; + assert_eq!( + recover(&flash) + .active() + .ports + .get(PortId::new(1)) + .unwrap() + .role, + Role::Mavlink + ); +} + +#[test] +fn failed_provider_close_blocks_saves_but_keeps_active_export_available() { + let flash = RamNor::default(); + let mut provider = Provider::new(recover(&flash)); + let mut next = factory(); + next.set_field(FieldId::new(201), ConfigValue::U8(1)) + .unwrap(); + flash.0.borrow_mut().fail_after = Some(2); + assert_eq!(write(&mut provider, &next), Err(FileStatus::StorageFailure)); + flash.0.borrow_mut().fail_after = None; + assert_eq!( + write(&mut provider, &factory()), + Err(FileStatus::StorageFailure) + ); + assert_eq!( + decode(&read(&mut provider, "/config/active.kcfg")).field_value(FieldId::new(201)), + Some(ConfigValue::U8(0)) + ); + write_to(&mut provider, b"/config/reset.kcfg", &factory()).unwrap(); + write(&mut provider, &next).unwrap(); + assert_eq!( + decode(&read(&mut provider, "/config/active.kcfg")).field_value(FieldId::new(201)), + Some(ConfigValue::U8(0)) + ); +} + +#[test] +fn storage_read_failure_is_explicit_and_cannot_activate_factory_uart_defaults() { + let flash = RamNor::default(); + flash.0.borrow_mut().fail_reads = true; + let mut manager = recover(&flash); + assert_eq!(manager.diagnostic(), BootDiagnostic::StorageUnknown); + assert!( + manager + .active() + .ports + .iter() + .all(|(_, p)| p.role == Role::Disabled) + ); + assert!(manager.committed().is_none()); + assert!(matches!( + manager.replace(factory()), + Err(ReplaceError::DurableStateUnknown) + )); +} + +#[test] +fn occupied_but_undecodable_storage_is_not_a_healthy_factory_boot() { + let flash = RamNor::default(); + flash.0.borrow_mut().bytes[1][128] = 0; + let manager = recover(&flash); + assert_eq!(manager.diagnostic(), BootDiagnostic::StorageUnknown); + assert!(manager.committed().is_none()); + assert!( + manager + .active() + .ports + .iter() + .all(|(_, p)| p.role == Role::Disabled) + ); + assert_eq!(flash.0.borrow().operations, 0); + assert_eq!(flash.0.borrow().bytes[1][128], 0); +} + +#[test] +fn two_downloaded_drafts_conflict_before_noop_and_fresh_noop_skips_flash() { + let flash = RamNor::default(); + let mut provider = Provider::new(recover(&flash)); + let a = read(&mut provider, "/config/user.kcfg"); + let b = read(&mut provider, "/config/user.kcfg"); + let mut next = decode(&b); + next.set_field(FieldId::new(201), ConfigValue::U8(1)) + .unwrap(); + upload(&mut provider, b"/config/user.kcfg", &edit(&b, &next)).unwrap(); + let operations = flash.0.borrow().operations; + for stale in [a.clone(), edit(&a, &next)] { + assert_eq!( + upload(&mut provider, b"/config/user.kcfg", &stale), + Err(FileStatus::RequestConflict) + ); + assert_eq!(flash.0.borrow().operations, operations); + } + let fresh = read(&mut provider, "/config/user.kcfg"); + let result = upload(&mut provider, b"/config/user.kcfg", &fresh).unwrap(); + assert!( + KcfgResultDetail::decode(result.as_bytes()) + .unwrap() + .0 + .restart_required() + ); + assert_eq!(flash.0.borrow().operations, operations); + assert_eq!( + decode(&read(&mut provider, "/config/active.kcfg")).field_value(FieldId::new(201)), + Some(ConfigValue::U8(0)) + ); + assert_eq!( + recover(&flash) + .active() + .ports + .get(PortId::new(1)) + .unwrap() + .role, + Role::Mavlink + ); +} + +// Use the codec to produce an incompatible snapshot without duplicating its layout. +#[derive(Clone)] +struct OldSchema(Model); +impl ConfigModel for OldSchema { + type Active = ::Active; + type Context = I; + const SCHEMA_ID: [u8; 16] = *b"OLD-BOARD-SCHEMA"; + fn descriptor(index: u16) -> Option<&'static kaptan_config::FieldDescriptor> { + Model::descriptor(index) + } + fn field_count() -> u16 { + Model::field_count() + } + fn field_value(&self, id: FieldId) -> Option { + self.0.field_value(id) + } + fn active_field_value(active: &Self::Active, id: FieldId) -> Option { + Model::active_field_value(active, id) + } + fn set_field( + &mut self, + id: FieldId, + value: ConfigValue, + ) -> Result<(), kaptan_config::FieldWriteError> { + self.0.set_field(id, value) + } + fn validate(&self, context: &I) -> Result { + self.0.validate(context) + } +} + +#[test] +fn old_schema_can_only_be_reset_explicitly_to_exact_factory_from_active_artifact() { + let flash = RamNor::default(); + Store::new(flash.clone()) + .persist(&OldSchema(factory())) + .unwrap(); + let before = flash.0.borrow().operations; + assert!(matches!( + ConfigManager::recover( + factory(), + Board::IMMUTABLE_CONFIG, + Store::new(flash.clone()) + ), + Err(kaptan_config::StartupError::UnreadablePersisted) + )); + let manager = recover(&flash); + assert_eq!(manager.diagnostic(), BootDiagnostic::StorageUnknown); + let mut provider = Provider::new(manager); + assert_eq!(flash.0.borrow().operations, before); + assert_eq!( + read_result(&mut provider, "/config/user.kcfg"), + Err(FileStatus::StorageFailure) + ); + let active = read(&mut provider, "/config/active.kcfg"); + let mut nonfactory = factory(); + nonfactory + .set_field(FieldId::new(201), ConfigValue::U8(1)) + .unwrap(); + assert_eq!( + upload( + &mut provider, + b"/config/reset.kcfg", + &edit(&active, &nonfactory) + ), + Err(FileStatus::ContentRejected) + ); + assert_eq!(flash.0.borrow().operations, before); + let reset = edit(&active, &factory()); + assert_eq!( + upload(&mut provider, b"/config/user.kcfg", &reset), + Err(FileStatus::StorageFailure) + ); + let result = upload(&mut provider, b"/config/reset.kcfg", &reset).unwrap(); + assert!( + !KcfgResultDetail::decode(result.as_bytes()) + .unwrap() + .0 + .restart_required() + ); + assert!(flash.0.borrow().operations > before); + assert_eq!( + artifact(&decode(&read(&mut provider, "/config/active.kcfg"))), + artifact(&decode(&active)) + ); + assert_eq!( + decode(&read(&mut provider, "/config/user.kcfg")).field_value(FieldId::new(201)), + Some(ConfigValue::U8(0)) + ); + let reboot = recover(&flash); + assert_eq!(reboot.diagnostic(), BootDiagnostic::Healthy); + assert_eq!( + reboot.active().ports.get(PortId::new(1)).unwrap().role, + Role::Disabled + ); + assert!(!reboot.restart_required()); +} + +#[test] +fn reset_failures_remain_unknown_and_explicit_retry_can_recover_after_durable_marker() { + for after_commit in [false, true] { + let flash = RamNor::default(); + Store::new(flash.clone()) + .persist(&OldSchema(factory())) + .unwrap(); + let mut provider = Provider::new(recover(&flash)); + let active = read(&mut provider, "/config/active.kcfg"); + let reset = edit(&active, &factory()); + { + let mut state = flash.0.borrow_mut(); + state.fail_after_commit = after_commit; + if !after_commit { + state.fail_after = Some(state.operations + 2); + } + } + assert_eq!( + upload(&mut provider, b"/config/reset.kcfg", &reset), + Err(FileStatus::StorageFailure) + ); + assert_eq!( + read_result(&mut provider, "/config/user.kcfg"), + Err(FileStatus::StorageFailure) + ); + assert_eq!(read(&mut provider, "/config/active.kcfg"), active); + { + let mut state = flash.0.borrow_mut(); + state.fail_after_commit = false; + state.fail_after = None; + } + assert_eq!( + upload(&mut provider, b"/config/user.kcfg", &reset), + Err(FileStatus::StorageFailure) + ); + let result = upload(&mut provider, b"/config/reset.kcfg", &reset).unwrap(); + assert!( + !KcfgResultDetail::decode(result.as_bytes()) + .unwrap() + .0 + .restart_required() + ); + assert_eq!( + artifact(&decode(&read(&mut provider, "/config/active.kcfg"))), + artifact(&decode(&active)) + ); + assert_eq!( + recover(&flash) + .active() + .ports + .get(PortId::new(1)) + .unwrap() + .role, + Role::Disabled + ); + } +} + +#[test] +fn readable_journal_recovery_does_not_scan_entire_slots() { + let flash = RamNor::default(); + Store::new(flash.clone()).persist(&factory()).unwrap(); + flash.0.borrow_mut().bytes_read = 0; + assert_eq!(recover(&flash).diagnostic(), BootDiagnostic::Healthy); + assert!( + flash.0.borrow().bytes_read < 2048, + "readable records should reuse journal recovery" + ); +} diff --git a/firmware/platforms/stm32f7/tests/support/io_latency.rs b/firmware/platforms/stm32f7/tests/support/io_latency.rs new file mode 100644 index 0000000..a9749e7 --- /dev/null +++ b/firmware/platforms/stm32f7/tests/support/io_latency.rs @@ -0,0 +1,671 @@ +//! Exercises the actual protocol worker and physical UART owners. Only elapsed +//! time, PAC registers and provider latency are simulated; no protocol is copied. +extern crate std; +use super::*; +use crate::f722::serial::{ReceiveEvent, SerialDevice, SerialIo, UartIo}; +use crate::f722::{data_channel::exchange_usb, usb}; +use kaptan_file_mavlink::{ + Content, ControlRequest, FileFailure, FileMetadata, FileOperation, FilePath, FileRequest, + FileStatus, +}; +use kaptan_mavlink::{encoder::MavlinkEncoder, receiver::IntegrityMavlinkFramer}; +use std::{ + boxed::Box, + cell::{Cell, RefCell}, + collections::VecDeque, + rc::Rc, + vec::Vec, +}; +use usb::test_bus as usb_bus; + +const INVENTORY: SerialInventory = + SerialInventory::new(&[crate::UART1, crate::UART2, crate::UART4]); + +#[derive(Default)] +struct Registers { + rx: VecDeque, + tx: Vec, + tx_ready_irq: bool, + tx_complete_irq: bool, +} +struct MockUart(Rc>); +impl UartIo for MockUart { + fn tx_ready(&mut self) -> bool { + true + } + fn tx_complete(&mut self) -> bool { + true + } + fn write_byte(&mut self, byte: u8) { + self.0.borrow_mut().tx.push(byte); + } + fn interrupts(&mut self, ready: bool, complete: bool) { + let mut registers = self.0.borrow_mut(); + registers.tx_ready_irq = ready; + registers.tx_complete_irq = complete; + } +} +impl SerialDevice for MockUart { + fn receive(&mut self) -> ReceiveEvent { + self.0 + .borrow_mut() + .rx + .pop_front() + .map_or(ReceiveEvent::Idle, ReceiveEvent::Byte) + } +} + +struct Io { + uart: [SerialIo; 3], + registers: [Rc>; 3], + sent: Vec<(u8, kaptan::MavMessage)>, + admitted: [usize; 3], +} +impl Io { + fn new() -> Self { + let registers = core::array::from_fn(|_| Rc::new(RefCell::new(Registers::default()))); + Self { + uart: core::array::from_fn(|i| { + SerialIo::new(Some(MockUart(registers[i].clone())), i != 2) + }), + registers, + sent: Vec::new(), + admitted: [0; 3], + } + } + fn irq(&mut self, now: Timestamp) { + for i in 0..3 { + if !self.registers[i].borrow().rx.is_empty() { + self.admitted[i] += 1; + } + self.uart[i].on_interrupt(now); + } + } + fn finish_setup_transmit(&mut self, now: Timestamp) { + // The owned TX queue has four pending slots plus reserved/active + // storage. 2048 ready/complete IRQs cover even six maximum-sized frames. + for _ in 0..2048 { + let mut pending = false; + for i in 0..2 { + let enabled = { + let registers = self.registers[i].borrow(); + registers.tx_ready_irq || registers.tx_complete_irq + }; + if enabled { + pending = true; + self.uart[i].on_interrupt(now); + } + } + if !pending { + return; + } + } + panic!("setup UART transmission did not reach quiescence"); + } + fn exchange(&mut self, id: u8, frame: Option>) -> IoBatch { + if let Some(frame) = &frame { + self.sent.push((id, checked(frame.frame).decode().unwrap())); + } + if id == 0 { + return match frame { + Some(_) => IoBatch { + accepted: true, + ..IoBatch::default() + }, + None => IoBatch::default(), + }; + } + self.uart[INVENTORY.index(PortId::new(id)).unwrap()].exchange(frame) + } +} + +struct SlowProvider<'a> { + work: Option>, + opens: usize, +} +impl FileProvider for SlowProvider<'_> { + fn open_read(&mut self, _: &FilePath) -> Result { + self.opens += 1; + if let Some(work) = self.work.take() { + work(); + } + Ok(FileMetadata { + size: 0, + checksum_crc32c: 0, + }) + } + fn open_write(&mut self, _: &FilePath, _: FileMetadata) -> Result<(), FileFailure> { + unreachable!() + } + fn read(&mut self, _: u32, _: &mut [u8]) -> Result<(), FileFailure> { + unreachable!() + } + fn write(&mut self, _: u32, _: &[u8]) -> Result<(), FileFailure> { + unreachable!() + } + fn close(&mut self) -> Result { + Ok(Content::EMPTY) + } + fn abort(&mut self) {} +} +fn checked(frame: MavlinkFrame) -> IntegrityCheckedFrame { + let mut parser = IntegrityMavlinkFramer::new(); + frame + .raw_bytes() + .iter() + .find_map(|&byte| parser.receive_byte(byte)) + .unwrap() +} +fn checked_stream(bytes: &[u8]) -> Vec { + let mut parser = IntegrityMavlinkFramer::new(); + let frames: Vec<_> = bytes + .iter() + .filter_map(|&byte| parser.receive_byte(byte)) + .collect(); + assert_eq!( + frames + .iter() + .map(|frame| frame.raw().raw_bytes().len()) + .sum::(), + bytes.len(), + "all transmitted bytes must belong to complete CRC-valid frames" + ); + frames +} +fn open(system: u8) -> MavlinkFrame { + let request = FileRequest::Control(ControlRequest { + operation: FileOperation::OpenRead, + target: Target::new(1, 1), + position: 0, + file_size: 0, + checksum_crc32c: 0, + transfer_id: 9, + content: Content::try_new(b"/test").unwrap(), + }); + let kaptan::MavMessage::FILE_CONTROL(data) = wire::encode_request(request) else { + unreachable!() + }; + MavlinkEncoder::new(system, 7).encode(&data) +} +fn mtf() -> [u8; 27] { + let mut frame = [0; 27]; + frame[..6].copy_from_slice(&[0xef, 1, 1, 0x51, 0, 20]); + frame[10..14].copy_from_slice(&1234u32.to_le_bytes()); + frame[16] = 1; // range valid + frame[23] = 1; // flow valid + frame[26] = frame[..26] + .iter() + .fold(0u8, |sum, byte| sum.wrapping_add(*byte)); + frame +} +fn worker(provider: SlowProvider<'_>) -> ProtocolWorker, 3> { + let mailboxes = Box::leak(Box::new(( + LatestImuMailbox::new(), + LatestRangeMailbox::new(), + LatestOpticalFlowMailbox::new(), + ))); + let (_, imu) = mailboxes.0.split(); + let (range_publisher, range) = mailboxes.1.split(); + let (flow_publisher, optical_flow) = mailboxes.2.split(); + let rates = DataChannelRates { + imu_hz: 1, + heartbeat_hz: 1, + range_hz: Some(98), + optical_flow_hz: None, + }; + ProtocolWorker { + inventory: INVENTORY, + usb: Link::new(1, 1, rates).unwrap(), + serial: [ + Some(SerialEndpoint::Mavlink(Link::new(1, 1, rates).unwrap())), + Some(SerialEndpoint::Mavlink(Link::new(1, 1, rates).unwrap())), + Some(SerialEndpoint::Mtf01 { + service: Mtf01::new(), + range: range_publisher, + optical_flow: flow_publisher, + }), + ], + collector: DataCollector::new(DataSources { + imu, + range, + optical_flow, + }), + config_file: provider, + file_service: FileService::new(Target::new(1, 1)), + file_lease: FileLease::default(), + mavlink_errors: ErrorSink::new(), + mavlink_logs: LogSinkQueue::new(crate::LevelFilter::Off), + } +} + +#[test] +fn compact_worker_routes_by_stable_id_and_never_exchanges_missing_ports() { + let ProtocolWorker { + usb, + serial: [_, uart2, mtf01], + collector, + config_file, + file_service, + file_lease, + mavlink_errors, + mavlink_logs, + .. + } = worker(SlowProvider { + work: None, + opens: 0, + }); + // Non-numeric order makes any residual platform-wide slot mapping fail. + let mut worker = ProtocolWorker { + usb, + serial: [mtf01, uart2], + inventory: SerialInventory::new(&[crate::UART4, crate::UART2]), + collector, + config_file, + file_service, + file_lease, + mavlink_errors, + mavlink_logs, + }; + assert!(worker.link(1).is_none()); + assert!(worker.link(4).is_none()); + assert!(worker.link(2).is_some()); + let mut exchanges = Vec::new(); + worker.process_worker_events( + 2, + |id, frame| { + assert!(frame.is_none()); + exchanges.push(id); + IoBatch::default() + }, + Timestamp::default, + ); + assert_eq!(exchanges, [4, 2, 0]); + worker.dispatch_frame( + 2, + checked(open(42)), + &mut |id, frame| { + assert_eq!(id, 2); + assert!(matches!( + checked(frame.unwrap().frame).decode(), + Some(kaptan::MavMessage::FILE_CONTROL(_)) + )); + IoBatch::default() + }, + &mut Timestamp::default, + ); + assert_eq!(worker.config_file.opens, 1); +} + +#[test] +fn expensive_dispatch_keeps_uarts_and_usb_tx_serviceable_during_rx_backpressure() { + usb_bus::with_link(|host, usb| { + let usb = RefCell::new(usb); + let io = Rc::new(RefCell::new(Io::new())); + let time = Rc::new(Cell::new(0u64)); + let mut worker = worker(SlowProvider { + work: None, + opens: 0, + }); + let mut exchange = |id, frame| { + let batch = io.borrow_mut().exchange(id, frame); + if id == 0 { + exchange_usb(&mut usb.borrow_mut(), frame, |enabled| { + assert!( + usb.try_borrow_mut().is_err(), + "gate must finish inside the I/O borrow" + ); + host.set_rx_interrupt_enabled(enabled) + }) + } else { + batch + } + }; + // Finish an initial sensor report and advance the real schedule to RANGE. + io.borrow().registers[2].borrow_mut().rx.extend(mtf()); + for t in 1..=27 { + io.borrow_mut().irq(Timestamp::from_microseconds(t * 100)); + } + time.set(3000); + for _ in 0..2 { + worker.process_worker_events(1, &mut exchange, || { + Timestamp::from_microseconds(time.get()) + }); + } + assert_eq!( + worker + .collector + .snapshot() + .range + .unwrap() + .timestamp + .as_microseconds(), + 2700 + ); + // Warm-up ticks may route process-global errors raised by another test. + // Explicitly queue one diagnostic locally so the setup/backlog case is + // deterministic without changing or draining the global error ingress. + let mut diagnostic = kaptan::ERROR_DATA::DEFAULT; + diagnostic.error_id = 65001; + let raised_at = b"io_latency_setup:1"; + diagnostic.raised_at[..raised_at.len()].copy_from_slice(raised_at); + for id in [0, 1, 2] { + let frame = worker + .link(id) + .unwrap() + .channel + .encode_message(FrameClass::Critical, &diagnostic); + assert!(exchange(id, Some(frame)).accepted); + } + // Settle setup traffic on every transport before queuing workload + // markers. Vital traffic can overtake a pending critical diagnostic; + // without this UART drain, that diagnostic could follow the marker. + io.borrow_mut() + .finish_setup_transmit(Timestamp::from_microseconds(time.get())); + host.collect_packets(&mut usb.borrow_mut()); + let uart_tx_start: [usize; 2] = core::array::from_fn(|i| { + let registers = io.borrow().registers[i].clone(); + let registers = registers.borrow(); + let frames = checked_stream(®isters.tx); + assert!(frames.iter().any(|frame| matches!( + frame.decode(), Some(kaptan::MavMessage::ERROR(data)) if data.error_id == 65001 + ))); + registers.tx.len() + }); + io.borrow_mut().sent.clear(); + let heartbeat = MavlinkEncoder::new(1, 1).encode(&kaptan::HEARTBEAT_DATA::default()); + for id in [1, 2] { + assert!( + io.borrow_mut() + .exchange( + id, + Some(PendingFrame { + class: FrameClass::Vital, + frame: heartbeat, + }), + ) + .accepted + ); + } + let mut data = kaptan::FILE_DATA_DATA::DEFAULT; + data.data[..107].fill(0x5a); + let usb_frame = MavlinkEncoder::new(1, 1).encode(&data); + assert_eq!(usb_frame.raw_bytes().len(), 128); + exchange_usb( + &mut usb.borrow_mut(), + Some(PendingFrame { + class: FrameClass::Vital, + frame: usb_frame, + }), + |enabled| host.set_rx_interrupt_enabled(enabled), + ); + // Real MAVLink bytes on both UARTs and a second MTF report arrive during + // expensive provider work. Borrowing I/O here would panic if dispatch still + // ran under the caller's I/O lock. + let during = io.clone(); + let clock = time.clone(); + let physical_usb = &usb; + worker.config_file.work = Some(std::boxed::Box::new(move || { + for i in 0..2 { + during.borrow().registers[i] + .borrow_mut() + .rx + .extend(open(42).raw_bytes()); + } + during.borrow().registers[2].borrow_mut().rx.extend(mtf()); + let mut transmitted_usb = Vec::new(); + for t in 1..=3000 { + clock.set(3000 + t * 100); + during + .borrow_mut() + .irq(Timestamp::from_microseconds(clock.get())); + if t == 1 { + host.write_packet(open(42).raw_bytes()); + } + if host.enter_irq() { + physical_usb.borrow_mut().with_rx_interrupt_gate( + |usb| usb.poll(), + |enabled| host.set_rx_interrupt_enabled(enabled), + ); + } + if let Some(packet) = host.take_packet() { + transmitted_usb.push(packet); + } + } + for (i, start) in uart_tx_start.into_iter().enumerate() { + let registers = during.borrow().registers[i].clone(); + assert_eq!( + ®isters.borrow().tx[start..], + heartbeat.raw_bytes(), + "UART {i} must transmit the complete workload marker during provider work" + ); + } + assert_eq!( + transmitted_usb.concat(), + usb_frame.raw_bytes(), + "IN completion interrupts submit all remaining bytes during provider work" + ); + assert_eq!( + transmitted_usb.iter().map(Vec::len).collect::>(), + [64, 64, 0] + ); + assert!( + !host.rx_interrupt_enabled(), + "only the RX FIFO interrupt waits for the consumer" + ); + assert!(host.nvic_enabled()); + assert!( + host.events_empty(), + "TX completion was serviced without the worker" + ); + })); + host.write_packet(open(42).raw_bytes()); + worker.process_worker_events(1, &mut exchange, || { + Timestamp::from_microseconds(time.get()) + }); + assert_eq!(time.get(), 303000); + // The report that was fresh at worker entry became stale during dispatch. + assert!( + !io.borrow() + .sent + .iter() + .any(|(_, msg)| matches!(msg, kaptan::MavMessage::RANGE(_))) + ); + for _ in 0..32 { + worker.process_worker_events(2, &mut exchange, || { + Timestamp::from_microseconds(time.get()) + }); + } + assert_eq!( + worker + .collector + .snapshot() + .range + .unwrap() + .timestamp + .as_microseconds(), + 5700, + "queued MTF bytes retain receipt time, not drain time" + ); + assert_eq!(io.borrow().admitted[2], 54); + assert!( + host.rx_interrupt_enabled(), + "draining RX rearms its FIFO interrupt after provider work" + ); + for id in [0, 1, 2] { + assert!( + io.borrow().sent.iter().any(|(ingress, msg)| *ingress == id + && matches!(msg, kaptan::MavMessage::FILE_CONTROL(_))), + "each link receives its own response" + ); + } + assert_eq!( + worker.config_file.opens, 1, + "USB retry received during provider work is replayed" + ); + assert_eq!( + io.borrow() + .sent + .iter() + .filter(|(id, msg)| *id == 0 && matches!(msg, kaptan::MavMessage::FILE_CONTROL(_))) + .count(), + 2 + ); + // A long silent interval followed by a new byte uses the SAME canonical time + // source (serviced independently by TIM2), not a dormant per-sensor clock. + time.set(60_000_000); + io.borrow().registers[2].borrow_mut().rx.extend(mtf()); + for t in 1..=27 { + io.borrow_mut() + .irq(Timestamp::from_microseconds(time.get() + t)); + } + for _ in 0..2 { + worker.process_worker_events(2, &mut exchange, || { + Timestamp::from_microseconds(time.get() + 100) + }); + } + assert_eq!( + worker + .collector + .snapshot() + .range + .unwrap() + .timestamp + .as_microseconds(), + 60_000_027 + ); + }); +} + +#[test] +fn each_dispatch_rechecks_lease_and_foreign_requests_do_not_renew_it() { + let mut worker = worker(SlowProvider { + work: None, + opens: 0, + }); + let time = Cell::new(0u64); + let mut replies = Vec::new(); + worker.dispatch_frame( + 0, + checked(open(42)), + &mut |id, frame| { + replies.push((id, checked(frame.unwrap().frame).decode().unwrap())); + IoBatch::default() + }, + &mut || Timestamp::from_microseconds(time.get()), + ); + assert_eq!(worker.file_lease.deadline, Some(10_000_000)); + let mut usb = Some(checked(open(43))); + if let Some(SerialEndpoint::Mavlink(link)) = &mut worker.serial[0] { + for &byte in open(42).raw_bytes() { + link.receiver.receive_byte(byte); + } + } + time.set(9_000_000); + worker.process_worker_events( + 2, + |id, frame| { + if let Some(frame) = frame { + replies.push((id, checked(frame.frame).decode().unwrap())); + if id == 0 { + time.set(11_000_000); + } + IoBatch::default() + } else if id == 0 { + IoBatch { + usb: usb.take(), + ..IoBatch::default() + } + } else { + IoBatch::default() + } + }, + || Timestamp::from_microseconds(time.get()), + ); + let statuses: Vec<_> = replies + .iter() + .filter_map(|(id, msg)| match msg { + kaptan::MavMessage::FILE_CONTROL(data) => Some((*id, data.status as u8)), + _ => None, + }) + .collect(); + assert_eq!( + statuses, + [ + (0, FileStatus::Ok as u8), + (0, FileStatus::Busy as u8), + (1, FileStatus::Ok as u8) + ] + ); + assert_eq!(worker.config_file.opens, 2); + assert_eq!(worker.file_lease.deadline, Some(21_000_000)); +} + +#[test] +fn level_triggered_usb_and_continuous_uart_notifications_reach_the_actual_consumer() { + usb_bus::with_link(|host, usb| { + let mut io = Io::new(); + let mut worker = worker(SlowProvider { + work: None, + opens: 0, + }); + for _ in 0..3 { + host.write_packet(open(42).raw_bytes()); + } + let uart_frame = MavlinkEncoder::new(43, 7).heartbeat(); + let mut workers = 0; + let mut usb_replies = 0; + for tick in 0..128 { + // Each decision has a genuine priority-14 byte arrival on both UARTs. + // These notifications cannot let a repeated priority-8 USB level + // prevent the priority-7 protocol consumer from running. + for registers in &io.registers[..2] { + registers + .borrow_mut() + .rx + .push_back(uart_frame.raw_bytes()[tick % uart_frame.raw_bytes().len()]); + } + let now = Timestamp::from_microseconds(tick as u64 * 100); + io.irq(now); + if host.enter_irq() { + usb.with_rx_interrupt_gate( + |usb| usb.poll(), + |enabled| host.set_rx_interrupt_enabled(enabled), + ); + } else { + workers += 1; + worker.process_worker_events( + 2, + |id, frame| { + if id == 0 { + if frame.as_ref().is_some_and(|frame| { + matches!( + checked(frame.frame).decode(), + Some(kaptan::MavMessage::FILE_CONTROL(_)) + ) + }) { + usb_replies += 1; + } + exchange_usb(usb, frame, |enabled| { + host.set_rx_interrupt_enabled(enabled) + }) + } else { + io.exchange(id, frame) + } + }, + || now, + ); + } + host.take_packet(); // Host ACK, without a hidden device poll. + } + assert!(workers > 0); + assert_eq!(worker.config_file.opens, 1); + assert_eq!( + usb_replies, 3, + "the consumer decoded the request and replayed both retries" + ); + assert_eq!(io.admitted[..2], [128, 128]); + assert_eq!(host.receive_backlog(), (0, false)); + assert!(host.rx_interrupt_enabled()); + assert!(host.nvic_enabled()); + }); +} diff --git a/firmware/platforms/stm32f7/tests/support/route_variants.rs b/firmware/platforms/stm32f7/tests/support/route_variants.rs new file mode 100644 index 0000000..85d46e7 --- /dev/null +++ b/firmware/platforms/stm32f7/tests/support/route_variants.rs @@ -0,0 +1,239 @@ +extern crate std; +use std::vec::Vec; + +use kaptan_config::{Config, ConfigModel, ConfigValue, FieldId}; +use kaptan_ports::{PortCatalog, PortDescriptor, PortSettings, Ports, Role}; +use platform_stm32f7::{ + f722::{F722Route, Icm42688Imu, ImuService, Mpu6000Imu}, + *, +}; + +use crate::f722::{config as target_config, geprc, kakute, ports}; + +fn board_route() +where + B: F7Board, + B::ImmutableConfig: F7ImmutableConfig, + R: F722Route, + Imu: ImuService, +{ +} + +#[test] +fn board_defaults_disable_configurable_ports() { + board_route::(); + board_route::(); + fn check() { + let candidate = + target_config::UserConfigModel::<_, B::ImmutableConfig, 5>::new(B::FACTORY_USER_CONFIG); + let active = candidate.validate(&B::IMMUTABLE_CONFIG).unwrap(); + let inventory = B::IMMUTABLE_CONFIG.f722().hardware.serial(); + assert_eq!(inventory.len(), 5); + assert_eq!( + active + .ports + .iter() + .map(|(id, _)| id.get()) + .collect::>(), + [1, 2, 3, 4, 6] + ); + assert!( + active + .ports + .iter() + .all(|(_, port)| port.role == Role::Disabled) + ); + let fields: Vec<_> = (0..B::UserConfig::FIELD_COUNT) + .map(|index| B::UserConfig::descriptor(index).unwrap()) + .collect(); + assert_eq!(fields.len(), 7 + 2 * inventory.len()); + assert_eq!( + fields[..7] + .iter() + .map(|field| field.id.get()) + .collect::>(), + [101, 102, 105, 103, 104, 106, 107] + ); + for (port, fields) in inventory + .descriptors() + .iter() + .zip(fields[7..].chunks_exact(2)) + { + assert_eq!(fields, port.fields().iter().collect::>()); + assert_eq!( + candidate.field_value(port.baud_field), + Some(ConfigValue::U32(115_200)) + ); + } + assert_eq!(inventory.index(kaptan_ports::PortId::new(0)), None); + } + check::(); + check::(); +} + +struct Both; +impl PortCatalog<3> for Both { + const DESCRIPTORS: [PortDescriptor; 3] = [UART1, UART2, UART4]; +} + +#[test] +fn independent_imu_serial_and_led_facts_allow_cross_route_compositions() { + let mpu = Mpu6000Spi1 { + mount: ImuMount::BodyAligned, + config: Mpu6000Config { + accelerometer: AccelerometerRange::PlusMinus4G, + gyroscope: GyroscopeRange::PlusMinus500DegreesPerSecond, + digital_low_pass_filter: DigitalLowPassFilter::Bandwidth42Hz, + sample_rate_divider: 9, + }, + }; + let icm = Icm42688Spi3 { + mount: ImuMount::Yaw180, + }; + fn check(route: R) { + for led in [StatusLed::ActiveLowPd2, StatusLed::ActiveHighPa13] { + let inventory = SerialInventory::new(&Both::DESCRIPTORS); + let hardware = + F722Hardware::new(route, inventory, led, VerticalSensor::None, MotorPins::Four); + for sensor in [0, 1, 2] { + let mut settings = [PortSettings { + role: Role::Mavlink, + baud: 115_200, + }; 3]; + settings[sensor].role = Role::Mtf01; + let root = Ports::::new(settings); + let plan = ports::project_ports::<_, 3>(&root, hardware.serial(), 3400).unwrap(); + assert_eq!( + plan.iter().filter(|(_, p)| p.role == Role::Mavlink).count(), + 2 + ); + assert_eq!( + plan.iter().filter(|(_, p)| p.role == Role::Mtf01).count(), + 1 + ); + } + } + } + check(mpu); + check(icm); +} + +#[test] +fn double_sensor_unsupported_baud_and_missing_inventory_fields_are_rejected() { + let inventory = SerialInventory::new(&Both::DESCRIPTORS); + let mut root = Ports::::new( + [PortSettings { + role: Role::Mtf01, + baud: 115_200, + }; 3], + ); + let error = ports::project_ports::<_, 3>(&root, inventory, 0).unwrap_err(); + assert_eq!(error.primary(), Some(UART2.role_field)); + assert_eq!(error.related_field(), Some(UART1.role_field)); + root.set_field(FieldId::new(203), ConfigValue::U8(0)) + .unwrap(); + root.set_field(FieldId::new(207), ConfigValue::U8(0)) + .unwrap(); + assert!(ports::project_ports::<_, 3>(&root, inventory, 0).is_ok()); + assert!( + root.set_field(FieldId::new(202), ConfigValue::U32(12345)) + .is_err() + ); + root.set_field(FieldId::new(202), ConfigValue::U32(230400)) + .unwrap(); + assert!(ports::project_ports::<_, 3>(&root, inventory, 0).is_err()); + root.set_field(FieldId::new(201), ConfigValue::U8(1)) + .unwrap(); + assert!(ports::project_ports::<_, 3>(&root, inventory, 23041).is_err()); + assert!( + ports::project_ports::<_, 3>(&geprc::GeprcF722V2::FACTORY_USER_CONFIG, inventory, 0) + .is_err() + ); +} + +#[test] +fn schema_inventory_and_storage_count_must_agree_even_when_disabled() { + let root = Ports::::new( + [PortSettings { + role: Role::Disabled, + baud: 115_200, + }; 3], + ); + // No silent unused fields, including fields whose current role is Disabled. + assert!(ports::project_ports::<_, 1>(&root, SerialInventory::new(&[UART1]), 0).is_err()); + assert!(ports::project_ports::<_, 0>(&root, SerialInventory::new(&[]), 0).is_err()); + assert!( + ports::project_ports::<_, 2>(&root, SerialInventory::new(&Both::DESCRIPTORS), 0).is_err() + ); + assert!( + ports::project_ports::<_, 3>(&root, SerialInventory::new(&[UART1, UART1, UART2]), 0) + .is_err() + ); +} + +#[test] +fn rejects_invalid_port_descriptors() { + let root = Ports::::new( + [PortSettings { + role: Role::Disabled, + baud: 115_200, + }; 3], + ); + const DISABLED_ROLES: [(u8, &str); 1] = [Role::CONFIG_ENUM_OPTIONS[0]]; + const INVALID_INVENTORIES: [[PortDescriptor; 3]; 6] = { + let mut rows = [[UART1, UART2, UART4]; 6]; + rows[0][0].id = kaptan_ports::PortId::new(3); + rows[1][0].resources = &[8, 9, 101]; + rows[2][0].role_field = FieldId::new(209); + rows[3][0].role_key = "ports.other.role"; + rows[4][0].allowed_bauds = &[9600]; + rows[5][0].supported_roles = &DISABLED_ROLES; + rows + }; + for descriptors in &INVALID_INVENTORIES { + let inventory = SerialInventory::new(descriptors); + assert!(ports::project_ports::<_, 3>(&root, inventory, 0).is_err()); + } + struct WrongMetadata; + impl PortCatalog<1> for WrongMetadata { + const DESCRIPTORS: [PortDescriptor; 1] = [PortDescriptor { + allowed_bauds: &[9600], + ..UART1 + }]; + } + let root = Ports::::new([PortSettings { + role: Role::Disabled, + baud: 9600, + }]); + assert!(ports::project_ports::<_, 1>(&root, SerialInventory::new(&[UART1]), 0).is_err()); +} + +#[test] +fn stable_ids_follow_catalog_order_and_disabled_ports_keep_their_slots() { + struct Reordered; + impl PortCatalog<2> for Reordered { + const DESCRIPTORS: [PortDescriptor; 2] = [UART4, UART1]; + } + let inventory = SerialInventory::new(&Reordered::DESCRIPTORS); + let mut root = Ports::::new( + [PortSettings { + role: Role::Disabled, + baud: 115_200, + }; 2], + ); + for role in [Role::Disabled, Role::Mavlink, Role::Mtf01] { + root.set_field(UART4.role_field, ConfigValue::U8(role.wire_value())) + .unwrap(); + let plan = ports::project_ports::<_, 2>(&root, inventory, 0).unwrap(); + assert_eq!( + plan.iter().map(|(id, _)| id.get()).collect::>(), + [4, 1] + ); + assert_eq!(plan.get(UART4.id).unwrap().role, role); + assert_eq!(inventory.index(UART4.id), Some(0)); + assert_eq!(inventory.index(UART1.id), Some(1)); + for id in [0, 2, 3, 5, 255] { + assert_eq!(inventory.index(kaptan_ports::PortId::new(id)), None); + } + } +} diff --git a/firmware/platforms/stm32f7/tests/support/timer_clock.rs b/firmware/platforms/stm32f7/tests/support/timer_clock.rs new file mode 100644 index 0000000..636e27a --- /dev/null +++ b/firmware/platforms/stm32f7/tests/support/timer_clock.rs @@ -0,0 +1,332 @@ +//! Injected hardware exercises the production shared-state and IRQ mechanism. + +extern crate std; + +use super::{SharedTimer, TimerBackend}; +use std::{ + collections::VecDeque, + sync::{Arc, Mutex, mpsc}, + thread, + time::Duration, + vec::Vec, +}; + +const CC1IF: u32 = 1 << 1; +const UIF: u32 = 1; + +#[derive(Debug, PartialEq)] +enum Access { + Counter(u32), + Ack, + Compare(u32), + Enable, + Pend, +} + +#[derive(Default)] +struct Registers { + cnt: u32, + flags: u32, + compare: u32, + enabled: bool, + pended: bool, + // Advance while writing CCR. `true` means the new compare took effect + // before CNT passed it, so hardware raises a fresh CC1IF. + write_delays: VecDeque<(u32, bool)>, + accesses: Vec, +} + +struct Counter { + registers: Arc>, + read_gate: Option<(mpsc::Sender<()>, mpsc::Receiver<()>)>, +} + +impl TimerBackend for Counter { + fn counter(&mut self) -> u32 { + let cnt = { + let mut registers = self.registers.lock().unwrap(); + let cnt = registers.cnt; + registers.accesses.push(Access::Counter(cnt)); + cnt + }; + if let Some((sampled, resume)) = self.read_gate.take() { + sampled.send(()).unwrap(); + resume.recv().unwrap(); + } + cnt + } + + fn compare_pending(&mut self) -> bool { + self.registers.lock().unwrap().flags & CC1IF != 0 + } + + fn acknowledge_compare(&mut self) { + let mut registers = self.registers.lock().unwrap(); + registers.flags &= !CC1IF; + registers.accesses.push(Access::Ack); + } + + fn set_compare(&mut self, counter: u32) { + let mut registers = self.registers.lock().unwrap(); + registers.compare = counter; + registers.accesses.push(Access::Compare(counter)); + if let Some((advance, raises_flag)) = registers.write_delays.pop_front() { + registers.cnt = registers.cnt.wrapping_add(advance); + if raises_flag { + registers.flags |= CC1IF; + } + } + } + + fn enable_compare_interrupt(&mut self) { + let mut registers = self.registers.lock().unwrap(); + registers.enabled = true; + registers.accesses.push(Access::Enable); + } + + fn pend_interrupt(&mut self) { + let mut registers = self.registers.lock().unwrap(); + registers.pended = true; + registers.accesses.push(Access::Pend); + } +} + +fn timer() -> (SharedTimer, Arc>) { + let registers = Arc::new(Mutex::new(Registers::default())); + let shared = SharedTimer::new(); + assert!(shared.initialize(|| Counter { + registers: registers.clone(), + read_gate: None, + })); + (shared, registers) +} + +fn tick(shared: &SharedTimer, registers: &Mutex, cnt: u32) -> bool { + { + let mut registers = registers.lock().unwrap(); + registers.cnt = cnt; + registers.flags |= CC1IF; + } + shared.on_interrupt() +} + +#[test] +fn one_time_publication_keeps_irq_disabled_until_successful_init_finishes() { + let (shared, registers) = timer(); + assert!(!shared.initialize(|| panic!("must not reinitialize hardware"))); + assert!(!registers.lock().unwrap().enabled); + // Other initialization may take time, or fail without ever enabling IRQs. + registers.lock().unwrap().cnt = 2_000_000; + registers.lock().unwrap().flags = CC1IF | UIF; + assert_eq!(shared.now().as_microseconds(), 2_000_000); + assert!(!registers.lock().unwrap().enabled); + registers.lock().unwrap().accesses.clear(); + shared.enable_interrupt(); + let registers = registers.lock().unwrap(); + assert!(registers.enabled); + assert_eq!(registers.flags, UIF); + assert_eq!( + registers.accesses, + [ + Access::Ack, + Access::Counter(2_000_000), + Access::Compare(2_010_000), + Access::Counter(2_000_000), + Access::Enable, + ] + ); +} + +#[test] +fn readers_and_irq_extend_one_epoch_across_rollover() { + let (shared, registers) = timer(); + let reader_a = &shared; + let reader_b = &shared; + registers.lock().unwrap().cnt = u32::MAX - 5; + assert_eq!(reader_a.now().as_microseconds(), 4_294_967_290); + assert!(tick(&shared, ®isters, 4)); + assert_eq!(reader_b.now().as_microseconds(), 4_294_967_300); + assert_eq!(registers.lock().unwrap().compare, 2_704); + registers.lock().unwrap().cnt = 100; + assert_eq!(reader_a.now().as_microseconds(), 4_294_967_396); +} + +#[test] +fn future_compare_can_cross_counter_wrap() { + let (shared, registers) = timer(); + assert!(tick(&shared, ®isters, u32::MAX - 5)); + let registers = registers.lock().unwrap(); + assert_eq!(registers.compare, 2_704); + assert!(!registers.pended); +} + +#[test] +fn silent_reader_retains_time_across_multiple_irq_serviced_wraps() { + let (shared, registers) = timer(); + let silent_reader = &shared; + assert_eq!(silent_reader.now().as_microseconds(), 0); + // Deliberately delayed service, still below the single-wrap observation + // bound. The silent consumer misses more than two whole counter wraps. + for second in 1..=10u64 { + assert!(tick(&shared, ®isters, (second * 1_000_000_000) as u32)); + } + assert_eq!(silent_reader.now().as_microseconds(), 10_000_000_000); +} + +#[test] +fn on_time_and_two_second_delayed_irq_each_emit_one_event_and_future_compare() { + let (shared, registers) = timer(); + for (cnt, expected) in [(10_000, 20_000), (2_010_000, 2_020_000)] { + registers.lock().unwrap().accesses.clear(); + assert!(tick(&shared, ®isters, cnt)); + assert_eq!(shared.now().as_microseconds(), u64::from(cnt)); + // This represents the consumer callback, after on_interrupt returns. + assert_eq!(registers.lock().unwrap().compare, expected); + assert!(!shared.on_interrupt()); + assert_eq!( + registers + .lock() + .unwrap() + .accesses + .iter() + .filter(|access| matches!(access, Access::Compare(_))) + .count(), + 1 + ); + } +} + +#[test] +fn stale_nvic_pend_without_cc1if_neither_rearms_nor_notifies() { + let (shared, registers) = timer(); + registers.lock().unwrap().flags = UIF; + assert!(!shared.on_interrupt()); + let registers = registers.lock().unwrap(); + assert!(registers.accesses.is_empty()); + assert_eq!(registers.flags, UIF); +} + +#[test] +fn target_passed_during_ccr_write_is_retried_before_callbacks() { + let (shared, registers) = timer(); + registers + .lock() + .unwrap() + .write_delays + .push_back((10_001, false)); + assert!(tick(&shared, ®isters, 10_000)); + assert_eq!(shared.now().as_microseconds(), 20_001); + let registers = registers.lock().unwrap(); + assert_eq!(registers.compare, 30_000); + assert!(!registers.pended); + assert_eq!( + registers.accesses, + [ + Access::Ack, + Access::Counter(10_000), + Access::Compare(20_000), + Access::Counter(20_001), + Access::Compare(30_000), + Access::Counter(20_001), + Access::Counter(20_001), + ] + ); +} + +#[test] +fn repeated_missed_writes_pend_bounded_recovery_without_fabricating_an_event() { + let (shared, registers) = timer(); + registers.lock().unwrap().write_delays = [(10_001, false), (10_000, false)].into(); + assert!(tick(&shared, ®isters, 10_000)); + { + let mut registers = registers.lock().unwrap(); + assert!(registers.pended); + assert_eq!(registers.compare, 30_000); + assert_eq!(registers.cnt, 30_001); + assert_eq!(registers.flags, 0); + assert_eq!( + registers + .accesses + .iter() + .filter(|a| matches!(a, Access::Compare(_))) + .count(), + 2 + ); + registers.pended = false; // NVIC takes the recovery exception. + } + assert!(!shared.on_interrupt()); + assert_eq!(registers.lock().unwrap().compare, 40_000); + assert!(!registers.lock().unwrap().pended); + assert!(tick(&shared, ®isters, 40_000)); + assert_eq!(registers.lock().unwrap().compare, 50_000); +} + +#[test] +fn new_compare_flag_during_rearm_and_unrelated_flags_are_preserved() { + let (shared, registers) = timer(); + registers.lock().unwrap().flags = UIF; + registers + .lock() + .unwrap() + .write_delays + .push_back((10_000, true)); + assert!(tick(&shared, ®isters, 10_000)); + assert_eq!(registers.lock().unwrap().flags, UIF | CC1IF); + assert_eq!(registers.lock().unwrap().compare, 30_000); + assert!(shared.on_interrupt()); + assert_eq!(registers.lock().unwrap().flags, UIF); + assert!(!shared.on_interrupt()); +} + +#[test] +fn irq_cannot_sample_between_a_readers_cnt_read_and_epoch_update() { + let registers = Arc::new(Mutex::new(Registers::default())); + let shared = SharedTimer::new(); + let (sampled_tx, sampled_rx) = mpsc::channel(); + let (resume_tx, resume_rx) = mpsc::channel(); + assert!(shared.initialize(|| Counter { + registers: registers.clone(), + read_gate: Some((sampled_tx, resume_rx)), + })); + registers.lock().unwrap().cnt = 100; + thread::scope(|scope| { + let reader = scope.spawn(|| shared.now()); + sampled_rx.recv().unwrap(); + // Hardware continues ticking with PRIMASK set. The IRQ tries to run + // while the reader has sampled CNT but has not committed its delta. + registers.lock().unwrap().cnt = 150; + registers.lock().unwrap().flags = CC1IF; + let (attempt_tx, attempt_rx) = mpsc::channel(); + let (done_tx, done_rx) = mpsc::channel(); + let shared = &shared; + let irq = scope.spawn(move || { + attempt_tx.send(()).unwrap(); + let event = shared.on_interrupt(); + done_tx.send(()).unwrap(); + event + }); + attempt_rx.recv().unwrap(); + let blocked = done_rx.recv_timeout(Duration::from_millis(20)).is_err(); + // Release before assertions so a failing test cannot strand either thread. + resume_tx.send(()).unwrap(); + assert_eq!(reader.join().unwrap().as_microseconds(), 100); + assert!(irq.join().unwrap()); + assert!(blocked, "IRQ entered while the reader was extending CNT"); + }); + assert_eq!(shared.now().as_microseconds(), 150); +} + +#[test] +fn full_wrap_blackout_reset_and_stopped_clock_are_not_detectable_from_cnt() { + let (shared, registers) = timer(); + registers.lock().unwrap().cnt = 100; + assert_eq!(shared.now().as_microseconds(), 100); + // Stopped/deep-sleep time and a whole unseen wrap both present CNT=100. + // No software correction is possible. These modes require a system reset. + assert_eq!(shared.now().as_microseconds(), 100); + registers.lock().unwrap().cnt = ((1u64 << 32) + 100) as u32; + assert_eq!(shared.now().as_microseconds(), 100); + // An illicit reset/regression is indistinguishable from a forward wrap. + registers.lock().unwrap().cnt = 90; + assert_eq!(shared.now().as_microseconds(), 4_294_967_386); +} diff --git a/firmware/platforms/stm32f7/tests/support/usb_bus.rs b/firmware/platforms/stm32f7/tests/support/usb_bus.rs new file mode 100644 index 0000000..6b4295e --- /dev/null +++ b/firmware/platforms/stm32f7/tests/support/usb_bus.rs @@ -0,0 +1,407 @@ +//! Shared packet-level USB hardware fake; runs the real CDC/USB stack. +extern crate std; +use super::UsbMavlink; +use std::{ + collections::VecDeque, + sync::{Arc, Mutex}, + vec::Vec, +}; +use usb_device::{ + UsbDirection, UsbError, + bus::{PollResult, UsbBus, UsbBusAllocator}, + device::{UsbDeviceBuilder, UsbVidPid}, + endpoint::{EndpointAddress, EndpointType}, +}; +use usbd_serial::CdcAcmClass; + +const RXFLVL: u32 = 1 << 4; +const USBSUSP: u32 = 1 << 11; +const USBRST: u32 = 1 << 12; +const ENUMDNE: u32 = 1 << 13; +const IEPINT: u32 = 1 << 18; +const WKUPINT: u32 = 1 << 31; + +struct RxPacket { + endpoint: usize, + setup: bool, + bytes: Vec, +} + +#[derive(Default)] +struct BusState { + offline: bool, + fail_poll: bool, + read_error: Option, + write_error: Option, + events: VecDeque, + input: [Option>; 4], + output: [Option>; 4], + sizes: [[u16; 4]; 2], + stalled: [[bool; 4]; 2], + bulk_in: usize, + bulk_out: usize, + // Hardware FIFO is distinct from the driver's one-packet endpoint buffer + // (`output`). RXFLVL stays asserted if that buffer prevents popping the FIFO. + rx_fifo: VecDeque, + setup: u16, + gintmsk: u32, + nvic_enabled: bool, + nvic_pending: bool, + endpoints_configured: bool, +} +impl BusState { + fn raw_status(&self) -> u32 { + self.events.iter().fold( + if self.rx_fifo.is_empty() { 0 } else { RXFLVL }, + |status, event| { + status + | match event { + // synopsys-usb-otg returns Reset only for ENUMDNE. + PollResult::Reset => ENUMDNE, + PollResult::Suspend => USBSUSP, + PollResult::Resume => WKUPINT, + PollResult::Data { ep_in_complete, .. } if *ep_in_complete != 0 => IEPINT, + _ => 0, + } + }, + ) + } + fn latch_interrupt(&mut self) { + self.nvic_pending |= self.raw_status() & self.gintmsk != 0; + } +} +#[derive(Clone, Default)] +pub(crate) struct Host(Arc>); +pub(crate) struct PacketBus(Arc>); +impl super::UsbBusHealth for PacketBus { + fn is_offline(&self) -> bool { + self.0.lock().unwrap().offline + } + fn disable(&self) { + let mut state = self.0.lock().unwrap(); + state.offline = true; + state.nvic_enabled = false; + state.nvic_pending = false; + } +} +impl UsbBus for PacketBus { + fn alloc_ep( + &mut self, + direction: UsbDirection, + address: Option, + kind: EndpointType, + size: u16, + _: u8, + ) -> Result { + let mut state = self.0.lock().unwrap(); + let sizes = &mut state.sizes[usize::from(direction == UsbDirection::In)]; + let index = match address { + Some(address) => address.index(), + None => (1..sizes.len()) + .find(|index| sizes[*index] == 0) + .ok_or(UsbError::EndpointOverflow)?, + }; + if index >= sizes.len() || sizes[index] != 0 { + return Err(UsbError::InvalidEndpoint); + } + sizes[index] = size; + if kind == EndpointType::Bulk { + match direction { + UsbDirection::In => state.bulk_in = index, + UsbDirection::Out => state.bulk_out = index, + } + } + Ok(EndpointAddress::from_parts(index, direction)) + } + fn enable(&mut self) { + let mut state = self.0.lock().unwrap(); + state.gintmsk = RXFLVL | USBSUSP | USBRST | ENUMDNE | IEPINT | WKUPINT; + state.nvic_enabled = true; + } + fn reset(&self) { + let mut state = self.0.lock().unwrap(); + state.input.fill(None); + // configure_all flushes FIFOs but does not reset the bulk OUT software + // EndpointBuffer::has_data. Transport cleanup must discard that packet. + let bulk_out = state.bulk_out; + let buffered = state.output[bulk_out].take(); + state.output.fill(None); + state.output[bulk_out] = buffered; + state.events.clear(); + state.rx_fifo.clear(); + state.setup = 0; + state.endpoints_configured = true; + } + fn set_device_address(&self, _: u8) {} + fn write(&self, address: EndpointAddress, bytes: &[u8]) -> Result { + let mut state = self.0.lock().unwrap(); + assert!(!state.offline, "write after terminal USB fault"); + if let Some(error) = state.write_error.take() { + return Err(error); + } + if bytes.len() > usize::from(state.sizes[1][address.index()]) { + return Err(UsbError::BufferOverflow); + } + let packet = &mut state.input[address.index()]; + if packet.is_some() { + return Err(UsbError::WouldBlock); + } + *packet = Some(bytes.to_vec()); + Ok(bytes.len()) + } + fn read(&self, address: EndpointAddress, bytes: &mut [u8]) -> Result { + let mut state = self.0.lock().unwrap(); + assert!(!state.offline, "read after terminal USB fault"); + if let Some(error) = state.read_error.take() { + return Err(error); + } + let packet = state.output[address.index()] + .as_ref() + .ok_or(UsbError::WouldBlock)?; + if packet.len() > bytes.len() { + return Err(UsbError::BufferOverflow); + } + let packet = state.output[address.index()].take().unwrap(); + state.setup &= !(1 << address.index()); + bytes[..packet.len()].copy_from_slice(&packet); + Ok(packet.len()) + } + fn set_stalled(&self, address: EndpointAddress, stalled: bool) { + self.0.lock().unwrap().stalled[usize::from(address.is_in())][address.index()] = stalled; + } + fn is_stalled(&self, address: EndpointAddress) -> bool { + self.0.lock().unwrap().stalled[usize::from(address.is_in())][address.index()] + } + fn suspend(&self) {} + fn resume(&self) {} + fn poll(&self) -> PollResult { + let mut state = self.0.lock().unwrap(); + assert!(!state.offline, "poll after terminal USB fault"); + if state.fail_poll { + state.offline = true; + state.nvic_enabled = false; + state.nvic_pending = false; + return PollResult::None; + } + // Like synopsys-usb-otg 0.4.0, polling reads raw status, not GINTMSK. + // Non-RX sources remain serviceable even with a blocked FIFO head. + let mut ep_in_complete = 0; + if let Some(event) = state.events.pop_front() { + match event { + PollResult::Data { + ep_in_complete: complete, + .. + } => ep_in_complete = complete, + _ => return event, + } + } + if let Some(packet) = state.rx_fifo.front() + && state.output[packet.endpoint].is_none() + { + let packet = state.rx_fifo.pop_front().unwrap(); + if packet.setup { + state.setup |= 1 << packet.endpoint; + } + state.output[packet.endpoint] = Some(packet.bytes); + } + let ep_setup = state.setup; + let ep_out = state + .output + .iter() + .enumerate() + .fold(0, |mask, (index, packet)| { + mask | if packet.is_some() { 1 << index } else { 0 } + }) + & !ep_setup; + // A still-asserted level can latch before the following gate write. + state.latch_interrupt(); + if ep_out | ep_in_complete | ep_setup != 0 { + PollResult::Data { + ep_out, + ep_in_complete, + ep_setup, + } + } else { + PollResult::None + } + } +} +impl Host { + pub(crate) fn fail_poll(&self) { + self.0.lock().unwrap().fail_poll = true; + } + pub(crate) fn fail_read(&self) { + self.0.lock().unwrap().read_error = Some(UsbError::InvalidState); + } + pub(crate) fn fail_write(&self) { + self.0.lock().unwrap().write_error = Some(UsbError::InvalidState); + } + pub(crate) fn enumeration_done(&self) { + let mut state = self.0.lock().unwrap(); + state.events.push_front(PollResult::Reset); + state.latch_interrupt(); + } + + pub(crate) fn event(&self, event: PollResult) { + let mut state = self.0.lock().unwrap(); + state.events.push_back(event); + state.latch_interrupt(); + } + #[allow(dead_code)] + pub(crate) fn events_empty(&self) -> bool { + self.0.lock().unwrap().events.is_empty() + } + fn control(&self, link: &mut UsbMavlink<'_, PacketBus>, request: [u8; 8]) { + self.queue_control(request); + link.poll(); + assert_eq!(self.take_input(0), Some(Vec::new())); + link.poll(); + } + pub(crate) fn queue_control(&self, request: [u8; 8]) { + let mut state = self.0.lock().unwrap(); + // EP0 shares the hardware FIFO. Never inject SETUP past a blocked bulk head. + state.rx_fifo.push_back(RxPacket { + endpoint: 0, + setup: true, + bytes: request.to_vec(), + }); + state.latch_interrupt(); + } + pub(crate) fn configure(&self, link: &mut UsbMavlink<'_, PacketBus>, configured: bool) { + self.control(link, [0, 9, u8::from(configured), 0, 0, 0, 0, 0]); + } + pub(crate) fn dtr(&self, link: &mut UsbMavlink<'_, PacketBus>, open: bool) { + self.control(link, [0x21, 0x22, u8::from(open), 0, 0, 0, 0, 0]); + } + pub(crate) fn write_packet(&self, bytes: &[u8]) { + assert!( + self.try_write_packet(bytes), + "host must retry when the RX FIFO is full" + ); + } + pub(crate) fn try_write_packet(&self, bytes: &[u8]) -> bool { + assert!(bytes.len() <= 64); + let mut state = self.0.lock().unwrap(); + // Driver allocation: two 16-word OUT buffers + 30 words. Model packet + // data and one receive status word, not cycle-level USB handshakes. + let occupied: usize = state + .rx_fifo + .iter() + .map(|packet| packet.bytes.len().div_ceil(4) + 1) + .sum(); + if occupied + bytes.len().div_ceil(4) + 1 > 62 { + return false; + } + let endpoint = state.bulk_out; + state.rx_fifo.push_back(RxPacket { + endpoint, + setup: false, + bytes: bytes.to_vec(), + }); + state.latch_interrupt(); + true + } + fn take_input(&self, endpoint: usize) -> Option> { + let mut state = self.0.lock().unwrap(); + let packet = state.input[endpoint].take()?; + state.events.push_back(PollResult::Data { + ep_out: 0, + ep_in_complete: 1 << endpoint, + ep_setup: 0, + }); + state.latch_interrupt(); + Some(packet) + } + + pub(crate) fn set_rx_interrupt_enabled(&self, enabled: bool) { + let mut state = self.0.lock().unwrap(); + state.gintmsk = if enabled { + state.gintmsk | RXFLVL + } else { + state.gintmsk & !RXFLVL + }; + // Masking never acknowledges pending status. Rearm sees the current level. + state.latch_interrupt(); + } + + #[allow(dead_code)] // Also shared with transport-only tests without a scheduler. + pub(crate) fn rx_interrupt_enabled(&self) -> bool { + self.0.lock().unwrap().gintmsk & RXFLVL != 0 + } + + #[allow(dead_code)] + pub(crate) fn nvic_enabled(&self) -> bool { + self.0.lock().unwrap().nvic_enabled + } + + #[allow(dead_code)] + pub(crate) fn interrupt_status(&self) -> (u32, u32) { + let state = self.0.lock().unwrap(); + (state.raw_status(), state.gintmsk) + } + + #[allow(dead_code)] + pub(crate) fn enter_irq(&self) -> bool { + let mut state = self.0.lock().unwrap(); + state.latch_interrupt(); + if !state.nvic_enabled || !state.nvic_pending { + return false; + } + state.nvic_pending = false; // Exception entry, not an enable/disable side effect. + true + } + + #[allow(dead_code)] + pub(crate) fn receive_backlog(&self) -> (usize, bool) { + let state = self.0.lock().unwrap(); + (state.rx_fifo.len(), state.output[state.bulk_out].is_some()) + } + + /// Host ACK without polling device software. The device must service TX completion. + pub(crate) fn take_packet(&self) -> Option> { + let state = self.0.lock().unwrap(); + if !state.endpoints_configured { + return None; + } + let endpoint = state.bulk_in; + drop(state); + self.take_input(endpoint) + } + pub(crate) fn collect_packets(&self, link: &mut UsbMavlink<'_, PacketBus>) -> Vec> { + let mut packets = Vec::new(); + for _ in 0..16 { + let Some(packet) = self.take_packet() else { + return packets; + }; + packets.push(packet); + link.poll(); + } + panic!("transmission did not finish"); + } +} +pub(crate) fn with_link(test: impl FnOnce(&Host, &mut UsbMavlink<'_, PacketBus>)) { + let host = Host::default(); + let allocator = UsbBusAllocator::new(PacketBus(host.0.clone())); + let class = CdcAcmClass::new(&allocator, 64); + let device = UsbDeviceBuilder::new(&allocator, UsbVidPid(0x1209, 1)) + .max_packet_size_0(64) + .unwrap() + .build(); + let mut link = UsbMavlink::new(device, class); + host.enumeration_done(); + link.poll(); + host.configure(&mut link, true); + host.dtr(&mut link, true); + link.with_rx_interrupt_gate( + |link| assert!(link.is_open()), + |enabled| host.set_rx_interrupt_enabled(enabled), + ); + // Finish initialization's latched exceptions before scheduling test traffic. + while host.enter_irq() { + link.with_rx_interrupt_gate( + |link| link.poll(), + |enabled| host.set_rx_interrupt_enabled(enabled), + ); + } + test(&host, &mut link); +} diff --git a/firmware/platforms/stm32f7/tests/support/usb_irq.rs b/firmware/platforms/stm32f7/tests/support/usb_irq.rs new file mode 100644 index 0000000..666a5fa --- /dev/null +++ b/firmware/platforms/stm32f7/tests/support/usb_irq.rs @@ -0,0 +1,391 @@ +//! Level-triggered interrupt scheduling around the real USB transport operations. +extern crate std; +use super::*; +use crate::f722::usb; +use kaptan_mavlink::{encoder::MavlinkEncoder, generated::kaptan::FILE_DATA_DATA}; +use kaptan_tx::FrameClass; +use std::{vec, vec::Vec}; +use usb::test_bus as usb_bus; +use usb_bus::{Host, PacketBus, with_link}; +use usb_device::bus::PollResult; + +fn poll_irq(host: &Host, usb: &mut usb::UsbMavlink<'_, PacketBus>) -> bool { + usb.with_rx_interrupt_gate( + |usb| usb.poll(), + |enabled| host.set_rx_interrupt_enabled(enabled), + ) +} +fn exchange( + host: &Host, + usb: &mut usb::UsbMavlink<'_, PacketBus>, + frame: Option, +) -> IoBatch { + exchange_usb( + usb, + frame.map(|frame| PendingFrame { + class: FrameClass::Critical, + frame, + }), + |enabled| host.set_rx_interrupt_enabled(enabled), + ) +} +fn file_frame(length: usize) -> MavlinkFrame { + let mut data = FILE_DATA_DATA::DEFAULT; + data.data[..length - 21].fill(0x5a); + let frame = MavlinkEncoder::new(1, 1).encode(&data); + assert_eq!(frame.raw_bytes().len(), length); + frame +} + +#[test] +fn register_rmw_changes_only_rxflvlm_and_preserves_adjacent_registers() { + for bit in 0..32 { + for initial in [1u32 << bit, !(1u32 << bit)] { + let mut registers = [0xa5a5_5a5a, initial, 0x5a5a_a5a5]; + for enabled in [false, false, true, true, false] { + // SAFETY: this aligned RAM word substitutes for the RW register. + // No concurrent access exists and RAM needs no device barrier. + unsafe { set_rx_fifo_interrupt_enabled(&mut registers[1], enabled) }; + assert_eq!(registers[1] & !(1 << 4), initial & !(1 << 4)); + assert_eq!(registers[1] & (1 << 4) != 0, enabled); + assert_eq!(registers[0], 0xa5a5_5a5a); + assert_eq!(registers[2], 0x5a5a_a5a5); + } + } + } +} + +// The asserted, enabled priority-8 vector wins over the priority-7 consumer. +fn step( + host: &Host, + usb: &mut usb::UsbMavlink<'_, PacketBus>, + ready: &mut bool, + frames: &mut Vec, +) -> bool { + if host.enter_irq() { + *ready |= poll_irq(host, usb); + false + } else if core::mem::take(ready) { + let batch = exchange(host, usb, None); + *ready |= batch.pending; + if let Some(frame) = batch.usb { + frames.push(*frame.raw()); + } + true + } else { + false + } +} + +#[test] +fn level_triggered_burst_starves_legacy_consumer_but_gated_consumer_recovers_exact_frames() { + for gate in [false, true] { + with_link(|host, usb| { + let mut encoder = MavlinkEncoder::new(42, 7); + let expected = core::array::from_fn::<_, 3, _>(|_| encoder.heartbeat()); + for frame in expected { + host.write_packet(frame.raw_bytes()); + } + let mut ready = false; + let mut frames = Vec::new(); + let mut workers = 0; + for _ in 0..32 { + if gate { + workers += usize::from(step(host, usb, &mut ready, &mut frames)); + } else if host.enter_irq() { + ready |= usb.poll(); // Original handler: no RX source gate. + } else if core::mem::take(&mut ready) { + workers += 1; + let batch = exchange(host, usb, None); + ready |= batch.pending; + if let Some(frame) = batch.usb { + frames.push(*frame.raw()); + } + } + } + if gate { + assert_eq!(frames, expected); + assert!(workers > 0); + assert_eq!(host.receive_backlog(), (0, false)); + assert!(host.rx_interrupt_enabled()); + assert!(host.nvic_enabled()); + } else { + assert_eq!( + workers, 0, + "level IRQ prevents the only consumer from running" + ); + assert!(frames.is_empty()); + assert_eq!( + host.receive_backlog(), + (1, true), + "hardware FIFO and driver buffer are both occupied" + ); + assert!( + host.enter_irq(), + "source is still asserted after repeated polls" + ); + } + }); + } +} + +#[test] +fn split_coalesced_bad_crc_and_zlp_packets_preserve_rx_and_live_tx_boundaries() { + with_link(|host, usb| { + let incoming = file_frame(130); + let mut encoder = MavlinkEncoder::new(42, 7); + let first = encoder.heartbeat(); + let mut corrupt = encoder.heartbeat().raw_bytes().to_vec(); + *corrupt.last_mut().unwrap() ^= 1; + let last = encoder.heartbeat(); + host.write_packet(&incoming.raw_bytes()[..64]); + host.write_packet(&[]); // An OUT ZLP must not mask a partial MAVLink frame. + host.write_packet(&incoming.raw_bytes()[64..128]); + host.write_packet( + &[ + &incoming.raw_bytes()[128..], + first.raw_bytes(), + &corrupt, + last.raw_bytes(), + ] + .concat(), + ); + + let outgoing = file_frame(128); + let after = encoder.heartbeat(); + assert!(exchange(host, usb, Some(outgoing)).accepted); + assert!(host.enter_irq()); + assert!(poll_irq(host, usb)); + assert!(!host.rx_interrupt_enabled()); + assert!(host.nvic_enabled()); + let mut packets = vec![host.take_packet().unwrap()]; + assert!( + host.enter_irq(), + "IN completion interrupts while RX is masked" + ); + assert!(poll_irq(host, usb)); + packets.push(host.take_packet().unwrap()); + assert_eq!(packets.iter().map(Vec::len).collect::>(), [64, 64]); + assert!(host.enter_irq()); + assert!(poll_irq(host, usb)); + packets.push(host.take_packet().unwrap()); + assert_eq!(packets[2], [], "TX ZLP progresses without the RX consumer"); + + let sent = exchange(host, usb, Some(after)); + assert!(sent.accepted); + assert!( + !sent.pending, + "TX admission's IoBatch flag is not the IRQ policy" + ); + assert!( + !host.rx_interrupt_enabled(), + "TX must preserve the actual unread RX gate" + ); + if let Some(packet) = host.take_packet() { + packets.push(packet); + } + + let partial = exchange(host, usb, None); + assert!(partial.usb.is_none()); + assert!(!partial.pending); + assert!( + host.rx_interrupt_enabled(), + "the partial framer is waiting for more bytes, not the worker" + ); + + let mut ready = false; + let mut frames = Vec::new(); + for _ in 0..64 { + step(host, usb, &mut ready, &mut frames); + if let Some(packet) = host.take_packet() { + packets.push(packet); + } + } + assert_eq!(frames, [incoming, first, last]); + assert_eq!( + packets.iter().map(Vec::len).collect::>(), + [64, 64, 0, 13] + ); + assert_eq!( + packets.concat(), + [outgoing.raw_bytes(), after.raw_bytes()].concat() + ); + assert!(host.nvic_enabled()); + }); +} + +#[test] +fn final_poll_refill_stays_masked_and_rearm_preserves_already_pending_irq() { + with_link(|host, usb| { + let frame = MavlinkEncoder::new(42, 7).heartbeat(); + host.write_packet(&frame.raw_bytes()[..8]); + assert!(host.enter_irq()); + usb.with_rx_interrupt_gate( + |usb| { + assert!(usb.poll()); + // Latch NVIC pending just before masking the peripheral source. + host.write_packet(&frame.raw_bytes()[8..]); + }, + |enabled| host.set_rx_interrupt_enabled(enabled), + ); + let partial = exchange(host, usb, None); + assert!(partial.usb.is_none()); + assert!(partial.pending, "the final poll refilled rx_remaining"); + assert!(!host.rx_interrupt_enabled()); + assert!(host.nvic_enabled()); + let complete = exchange(host, usb, None); + assert_eq!(complete.usb.unwrap().raw(), &frame); + assert!(!complete.pending); + assert_eq!(host.receive_backlog(), (0, false)); + assert!( + host.enter_irq(), + "RX rearm preserves latched NVIC pending even after polling consumed the source" + ); + assert!(!poll_irq(host, usb)); + }); +} + +#[test] +fn arrivals_on_either_side_of_rearm_are_not_lost() { + for before in [true, false] { + with_link(|host, usb| { + let mut encoder = MavlinkEncoder::new(42, 7); + let first = encoder.heartbeat(); + let next = encoder.heartbeat(); + host.write_packet(first.raw_bytes()); + assert!(host.enter_irq()); + assert!(poll_irq(host, usb)); + let batch = exchange_usb(usb, None, |enabled| { + assert!(enabled); + if before { + host.write_packet(next.raw_bytes()); + } + host.set_rx_interrupt_enabled(enabled); + if !before { + host.write_packet(next.raw_bytes()); + } + }); + assert_eq!(batch.usb.unwrap().raw(), &first); + assert!(!batch.pending); + assert!(host.enter_irq()); + assert!(poll_irq(host, usb)); + assert_eq!(exchange(host, usb, None).usb.unwrap().raw(), &next); + }); + } +} + +#[test] +fn backpressure_keeps_reset_suspend_and_resume_interrupts_serviceable() { + for reset in [true, false] { + with_link(|host, usb| { + let frame = file_frame(130); + assert!(exchange(host, usb, Some(frame)).accepted); + for _ in 0..3 { + host.write_packet(&frame.raw_bytes()[..8]); + } + assert!(host.enter_irq()); + assert!(poll_irq(host, usb)); + assert!(host.enter_irq(), "one already-pending exception is allowed"); + assert!(poll_irq(host, usb)); + assert_eq!(host.receive_backlog(), (1, true)); + assert!(!host.enter_irq()); + let (_, original_mask) = host.interrupt_status(); + host.event(if reset { + PollResult::Reset + } else { + PollResult::Suspend + }); + assert!(host.enter_irq()); + poll_irq(host, usb); + assert!(!usb.is_open()); + assert!(host.events_empty()); + assert!(host.nvic_enabled()); + assert_eq!( + host.interrupt_status().1 & !(1 << 4), + original_mask & !(1 << 4) + ); + if reset { + assert!(host.rx_interrupt_enabled()); + assert_eq!(host.receive_backlog(), (0, false)); + host.configure(usb, true); + host.dtr(usb, true); + assert!( + host.collect_packets(usb).is_empty(), + "reset discarded the old TX queue" + ); + } else { + assert!(!host.rx_interrupt_enabled(), "suspend retains unread RX"); + assert_eq!(host.receive_backlog(), (1, true)); + host.event(PollResult::Resume); + assert!(host.enter_irq()); + poll_irq(host, usb); + assert!(usb.is_open()); + assert_eq!(host.collect_packets(usb).concat(), frame.raw_bytes()); + } + }); + } +} + +#[test] +fn ep0_setup_cannot_bypass_a_blocked_bulk_fifo_head() { + with_link(|host, usb| { + let frame = MavlinkEncoder::new(42, 7).heartbeat(); + assert!(exchange(host, usb, Some(frame)).accepted); + for _ in 0..3 { + host.write_packet(frame.raw_bytes()); + } + assert!(host.enter_irq()); + poll_irq(host, usb); + assert!(host.enter_irq()); + poll_irq(host, usb); + assert_eq!(host.receive_backlog(), (1, true)); + assert!(!host.enter_irq()); + host.queue_control([0x21, 0x22, 0, 0, 0, 0, 0, 0]); // DTR close behind bulk + assert_eq!(host.receive_backlog(), (2, true)); + assert!( + !host.enter_irq(), + "EP0 arrival also uses the masked RX source" + ); + let (raw, mask) = host.interrupt_status(); + assert_ne!(raw & (1 << 4), 0); + assert_eq!(raw & mask, 0); + host.take_packet().unwrap(); // IN completion can interrupt and poll raw RX. + assert!(host.enter_irq()); + poll_irq(host, usb); + assert_eq!(host.receive_backlog(), (2, true)); + assert!( + usb.is_open(), + "SETUP cannot jump over the blocked FIFO head" + ); + assert_eq!(exchange(host, usb, None).usb.unwrap().raw(), &frame); + exchange(host, usb, None); + assert!(!usb.is_open(), "consumer progress finally allows DTR close"); + assert!(host.rx_interrupt_enabled()); + assert!(host.nvic_enabled()); + }); +} + +#[test] +fn full_fifo_backpressures_host_then_consumer_progress_allows_retry() { + with_link(|host, usb| { + let frame = file_frame(64); + for _ in 0..3 { + assert!(host.try_write_packet(frame.raw_bytes())); + } + assert!(!host.try_write_packet(frame.raw_bytes())); + let mut ready = false; + let mut frames = Vec::new(); + for _ in 0..32 { + step(host, usb, &mut ready, &mut frames); + } + assert_eq!(frames, [frame; 3]); + assert!(host.try_write_packet(frame.raw_bytes())); + for _ in 0..16 { + step(host, usb, &mut ready, &mut frames); + } + assert_eq!(frames, [frame; 4]); + assert_eq!(host.receive_backlog(), (0, false)); + assert!(host.rx_interrupt_enabled()); + assert!(host.nvic_enabled()); + }); +} diff --git a/firmware/platforms/stm32f7/tests/usb_mavlink.rs b/firmware/platforms/stm32f7/tests/usb_mavlink.rs index 5de68b4..7ebb37f 100644 --- a/firmware/platforms/stm32f7/tests/usb_mavlink.rs +++ b/firmware/platforms/stm32f7/tests/usb_mavlink.rs @@ -1,210 +1,146 @@ -use std::{ - collections::VecDeque, - sync::{Arc, Mutex}, -}; - use kaptan_mavlink::{ MavlinkFrame, encoder::MavlinkEncoder, generated::kaptan::{FILE_DATA_DATA, MavMessage}, }; use kaptan_tx::{FrameClass, PendingFrame}; -use usb_device::{ - UsbDirection, UsbError, - bus::{PollResult, UsbBus, UsbBusAllocator}, - device::{UsbDeviceBuilder, UsbVidPid}, - endpoint::{EndpointAddress, EndpointType}, -}; -use usbd_serial::CdcAcmClass; +use usb_device::bus::PollResult; #[path = "../src/f722/usb.rs"] mod usb; -use usb::UsbMavlink; +use usb::test_bus as usb_bus; +use usb_bus::with_link; +#[path = "../src/f722/serial.rs"] +mod serial; +use serial::{IrqTransmit, UartIo}; -// One pending packet per endpoint, with host-controlled completion. Device and -// CDC control requests run through the real usb-device/usbd-serial code. #[derive(Default)] -struct BusState { - events: VecDeque, - input: [Option>; 4], - output: [Option>; 4], - sizes: [[u16; 4]; 2], - stalled: [[bool; 4]; 2], - bulk_in: usize, - bulk_out: usize, +struct MockUart { + bytes: Vec, + ready: bool, + complete: bool, + ready_irq: bool, + complete_irq: bool, +} +impl UartIo for MockUart { + fn tx_ready(&mut self) -> bool { + self.ready + } + fn tx_complete(&mut self) -> bool { + self.complete + } + fn write_byte(&mut self, byte: u8) { + self.bytes.push(byte); + self.complete = false; + } + fn interrupts(&mut self, ready: bool, complete: bool) { + self.ready_irq = ready; + self.complete_irq = complete; + } +} +fn flush_uart(tx: &mut IrqTransmit, io: &mut MockUart) { + io.ready = true; + for _ in 0..2048 { + if !io.ready_irq && !io.complete_irq { + return; + } + let previous = io.bytes.len(); + io.complete = true; + tx.on_interrupt(io); + assert!( + io.bytes.len() <= previous + 1, + "IRQ work is bounded to one byte" + ); + } + panic!("UART did not drain its bounded queue"); } -#[derive(Clone, Default)] -struct Host(Arc>); +#[test] +fn moving_irq_queue_and_waiting_for_tc_preserves_exact_frame_bytes() { + let mut io = MockUart { + ready: true, + ..MockUart::default() + }; + let mut tx = IrqTransmit::new(); + let mut encoder = MavlinkEncoder::new(1, 1); + let frame = encoder.encode(&kaptan_mavlink::generated::kaptan::HEARTBEAT_DATA::default()); + assert!(tx.send( + &mut io, + PendingFrame { + class: FrameClass::Vital, + frame + } + )); + assert!(!tx.on_interrupt(&mut io)); + let mut moved = tx; + for _ in 1..frame.raw_bytes().len() { + assert!(!moved.on_interrupt(&mut io)); + } + assert_eq!(io.bytes, frame.raw_bytes()); + assert!(!io.ready_irq && io.complete_irq); + assert!(!moved.on_interrupt(&mut io)); + io.complete = true; + assert!(moved.on_interrupt(&mut io)); + assert!(!io.ready_irq && !io.complete_irq); + assert!(!moved.on_interrupt(&mut io)); +} -struct PacketBus(Arc>); - -impl UsbBus for PacketBus { - fn alloc_ep( - &mut self, - direction: UsbDirection, - address: Option, - kind: EndpointType, - size: u16, - _: u8, - ) -> Result { - let mut state = self.0.lock().unwrap(); - let sizes = &mut state.sizes[usize::from(direction == UsbDirection::In)]; - let index = match address { - Some(address) => address.index(), - None => (1..sizes.len()) - .find(|index| sizes[*index] == 0) - .ok_or(UsbError::EndpointOverflow)?, +#[test] +fn usb_and_two_uarts_have_independent_sequences_queues_and_completion() { + use kaptan_data_channel::{DataChannel, DataChannelRates, TelemetrySamples}; + use kaptan_time::Timestamp; + with_link(|host, usb| { + host.configure(usb, true); + host.dtr(usb, true); + let rates = DataChannelRates { + imu_hz: 20, + heartbeat_hz: 2, + range_hz: None, + optical_flow_hz: None, }; - if index >= sizes.len() || sizes[index] != 0 { - return Err(UsbError::InvalidEndpoint); - } - sizes[index] = size; - if kind == EndpointType::Bulk { - match direction { - UsbDirection::In => state.bulk_in = index, - UsbDirection::Out => state.bulk_out = index, + let mut channels = core::array::from_fn::<_, 3, _>(|_| { + DataChannel::<100>::new(1, 1, rates, 100_000).unwrap() + }); + let mut uart1 = IrqTransmit::new(); + let mut uart2 = IrqTransmit::new(); + let mut slow = MockUart::default(); + let mut fast = MockUart::default(); + let samples = TelemetrySamples::default(); + let mut usb_bytes = Vec::new(); + let mut expected = Vec::new(); + let mut dropped = false; + for tick in 0..400 { + let now = Timestamp::from_microseconds(tick * 10_000); + let frames = channels + .each_mut() + .map(|channel| channel.on_service_tick(&samples, now)); + if let [Some(a), Some(b), Some(c)] = frames { + assert_eq!(a.frame.raw_bytes(), b.frame.raw_bytes()); + assert_eq!(b.frame.raw_bytes(), c.frame.raw_bytes()); + expected.extend_from_slice(c.frame.raw_bytes()); + assert!(usb.send(a)); + dropped |= !uart1.send(&mut slow, b); + assert!(uart2.send(&mut fast, c)); } + flush_uart(&mut uart2, &mut fast); + usb.poll(); + usb_bytes.extend(host.collect_packets(usb).into_iter().flatten()); } - Ok(EndpointAddress::from_parts(index, direction)) - } - - fn enable(&mut self) {} - - fn reset(&self) { - let mut state = self.0.lock().unwrap(); - state.input.fill(None); - state.output.fill(None); - state.events.clear(); - } - - fn set_device_address(&self, _: u8) {} - - fn write(&self, address: EndpointAddress, bytes: &[u8]) -> Result { - let mut state = self.0.lock().unwrap(); - if bytes.len() > usize::from(state.sizes[1][address.index()]) { - return Err(UsbError::BufferOverflow); - } - let packet = &mut state.input[address.index()]; - if packet.is_some() { - return Err(UsbError::WouldBlock); - } - *packet = Some(bytes.to_vec()); - Ok(bytes.len()) - } - - fn read(&self, address: EndpointAddress, bytes: &mut [u8]) -> Result { - let mut state = self.0.lock().unwrap(); - let packet = state.output[address.index()] - .as_ref() - .ok_or(UsbError::WouldBlock)?; - if packet.len() > bytes.len() { - return Err(UsbError::BufferOverflow); - } - let packet = state.output[address.index()].take().unwrap(); - bytes[..packet.len()].copy_from_slice(&packet); - Ok(packet.len()) - } - - fn set_stalled(&self, address: EndpointAddress, stalled: bool) { - self.0.lock().unwrap().stalled[usize::from(address.is_in())][address.index()] = stalled; - } - - fn is_stalled(&self, address: EndpointAddress) -> bool { - self.0.lock().unwrap().stalled[usize::from(address.is_in())][address.index()] - } - - fn suspend(&self) {} - fn resume(&self) {} - - fn poll(&self) -> PollResult { - self.0 - .lock() - .unwrap() - .events - .pop_front() - .unwrap_or(PollResult::None) - } -} - -impl Host { - fn event(&self, event: PollResult) { - self.0.lock().unwrap().events.push_back(event); - } - - fn control(&self, link: &mut UsbMavlink<'_, PacketBus>, request: [u8; 8]) { - self.0.lock().unwrap().output[0] = Some(request.to_vec()); - self.event(PollResult::Data { - ep_out: 0, - ep_in_complete: 0, - ep_setup: 1, - }); - link.poll(); - assert_eq!(self.take_input(0), Some(Vec::new())); - link.poll(); - } - - fn configure(&self, link: &mut UsbMavlink<'_, PacketBus>, configured: bool) { - self.control(link, [0, 9, u8::from(configured), 0, 0, 0, 0, 0]); - } - - fn dtr(&self, link: &mut UsbMavlink<'_, PacketBus>, open: bool) { - self.control(link, [0x21, 0x22, u8::from(open), 0, 0, 0, 0, 0]); - } - - fn write_packet(&self, bytes: &[u8]) { - assert!(bytes.len() <= 64); - let mut state = self.0.lock().unwrap(); - let endpoint = state.bulk_out; - assert!(state.output[endpoint].is_none()); - state.output[endpoint] = Some(bytes.to_vec()); - state.events.push_back(PollResult::Data { - ep_out: 1 << endpoint, - ep_in_complete: 0, - ep_setup: 0, - }); - } - - fn take_input(&self, endpoint: usize) -> Option> { - let mut state = self.0.lock().unwrap(); - let packet = state.input[endpoint].take()?; - state.events.push_back(PollResult::Data { - ep_out: 0, - ep_in_complete: 1 << endpoint, - ep_setup: 0, - }); - Some(packet) - } - - fn collect_packets(&self, link: &mut UsbMavlink<'_, PacketBus>) -> Vec> { - let endpoint = self.0.lock().unwrap().bulk_in; - let mut packets = Vec::new(); - for _ in 0..16 { - let Some(packet) = self.take_input(endpoint) else { - return packets; - }; - packets.push(packet); - link.poll(); - } - panic!("transmission did not finish"); - } -} - -fn with_link(test: impl FnOnce(&Host, &mut UsbMavlink<'_, PacketBus>)) { - let host = Host::default(); - let allocator = UsbBusAllocator::new(PacketBus(host.0.clone())); - let class = CdcAcmClass::new(&allocator, 64); - let device = UsbDeviceBuilder::new(&allocator, UsbVidPid(0x1209, 1)) - .max_packet_size_0(64) - .unwrap() - .build(); - let mut link = UsbMavlink::new(device, class); - host.event(PollResult::Reset); - link.poll(); - host.configure(&mut link, true); - host.dtr(&mut link, true); - assert!(link.is_open()); - test(&host, &mut link); + assert!( + dropped, + "blocked UART must exhaust only its own bounded queue" + ); + assert!(slow.bytes.is_empty()); + assert_eq!(usb_bytes, expected); + assert_eq!(fast.bytes, expected); + let fast_before = fast.bytes.clone(); + flush_uart(&mut uart1, &mut slow); + assert!(!slow.bytes.is_empty()); + assert_eq!( + fast.bytes, fast_before, + "UART1 completions cannot advance UART2" + ); + }); } fn pending(frame: MavlinkFrame) -> PendingFrame { @@ -269,7 +205,7 @@ fn drains_endpoint_backlog_after_its_poll_event_was_consumed() { assert_eq!(link.take_received().unwrap().raw(), &first); assert!(link.take_received().is_none()); - assert!(host.0.lock().unwrap().events.is_empty()); + assert!(host.events_empty()); assert!(link.poll()); assert_eq!(link.take_received().unwrap().raw(), &second); }); @@ -432,3 +368,30 @@ fn bus_reset_discards_transmit_and_receive_state() { assert_eq!(link.take_received().unwrap().raw(), &next); }); } + +#[test] +fn bus_fault_closes_link_and_discards_pending_data() { + for fail in [ + usb_bus::Host::fail_poll, + usb_bus::Host::fail_read, + usb_bus::Host::fail_write, + ] { + with_link(|host, link| { + let frame = file_frame(130); + assert!(link.send(pending(frame))); + host.write_packet(&frame.raw_bytes()[..8]); + link.poll(); + assert!(link.take_received().is_none()); + fail(host); + link.with_rx_interrupt_gate( + |link| assert!(!link.poll()), + |_| panic!("interrupt register access after terminal fault"), + ); + assert!(!link.is_open()); + assert!(!link.send(pending(frame))); + assert!(link.take_received().is_none()); + assert!(!link.poll()); + assert!(!host.nvic_enabled()); + }); + } +} diff --git a/firmware/tools/configurator/src/api.rs b/firmware/tools/configurator/src/api.rs index 5235b1d..eae867e 100644 --- a/firmware/tools/configurator/src/api.rs +++ b/firmware/tools/configurator/src/api.rs @@ -4,33 +4,105 @@ use std::{ io::{Read, Write}, net::TcpStream, sync::Arc, + time::{Duration, Instant}, }; use serde::{Deserialize, Serialize}; use crate::{ - serial_worker::{Command, SerialController}, - state::SharedState, + serial_worker::{Action, SerialController}, + state::{SharedState, ViewContext}, }; const INDEX: &str = include_str!("../web/index.html"); const APP: &str = include_str!("../web/app.js"); +const VIEW: &str = include_str!("../web/view.js"); const CSS: &str = include_str!("../web/styles.css"); const CSP: &str = "default-src 'self'; connect-src 'self'; img-src 'self'; style-src 'self'; script-src 'self'; base-uri 'none'; form-action 'self'; frame-ancestors 'none'"; +const HTTP_PHASE_TIMEOUT: Duration = Duration::from_secs(2); +const MAX_REQUEST_BYTES: usize = 64 * 1024; +const MAX_BODY_BYTES: usize = 60 * 1024; + +struct HttpConnection { + stream: TcpStream, + read_deadline: Instant, + write_deadline: Option, + timeout: Duration, +} + +impl HttpConnection { + fn new(stream: TcpStream, timeout: Duration) -> Self { + Self { + stream, + read_deadline: Instant::now() + timeout, + write_deadline: None, + timeout, + } + } +} + +fn remaining(deadline: Instant) -> std::io::Result { + deadline + .checked_duration_since(Instant::now()) + .filter(|duration| !duration.is_zero()) + .ok_or_else(|| std::io::Error::new(std::io::ErrorKind::TimedOut, "HTTP deadline expired")) +} + +impl Read for HttpConnection { + fn read(&mut self, bytes: &mut [u8]) -> std::io::Result { + self.stream + .set_read_timeout(Some(remaining(self.read_deadline)?))?; + self.stream.read(bytes) + } +} + +impl Write for HttpConnection { + fn write(&mut self, bytes: &[u8]) -> std::io::Result { + let deadline = *self + .write_deadline + .get_or_insert_with(|| Instant::now() + self.timeout); + self.stream.set_write_timeout(Some(remaining(deadline)?))?; + self.stream.write(bytes) + } + fn flush(&mut self) -> std::io::Result<()> { + self.stream.flush() + } +} #[derive(Deserialize)] struct Connect { + context: ViewContext, path: String, } #[derive(Deserialize)] struct EditDraft { + context: ViewContext, field_id: u16, value: serde_json::Value, } +#[derive(Deserialize)] +struct Mutation { + context: ViewContext, +} + pub fn serve_connection( - mut stream: TcpStream, + stream: TcpStream, + state: &Arc, + serial: &SerialController, + origin: &str, +) -> std::io::Result<()> { + serve_http( + HttpConnection::new(stream, HTTP_PHASE_TIMEOUT), + state, + serial, + origin, + ) +} + +fn serve_http( + mut stream: HttpConnection, state: &Arc, serial: &SerialController, origin: &str, @@ -39,30 +111,42 @@ pub fn serve_connection( let header_end = loop { let mut chunk = [0_u8; 4096]; let length = stream.read(&mut chunk)?; - if length == 0 || request.len() + length > 64 * 1024 { + if length == 0 || request.len() + length > MAX_REQUEST_BYTES { + return reply(&mut stream, 400, "text/plain", "bad request"); + } + // Only the previous chunk's last three bytes can start a split separator. + let search_start = request.len().saturating_sub(3); + request.extend_from_slice(&chunk[..length]); + if let Some(separator) = request[search_start..] + .windows(4) + .position(|window| window == b"\r\n\r\n") + { + break search_start + separator + 4; + } + }; + let Some(content_length) = content_length(&request[..header_end - 4]) else { + return reply(&mut stream, 400, "text/plain", "bad request"); + }; + if content_length > MAX_BODY_BYTES || content_length > MAX_REQUEST_BYTES - header_end { + return reply(&mut stream, 400, "text/plain", "bad request"); + } + let body_end = header_end + content_length; + while request.len() < body_end { + let mut chunk = [0_u8; 4096]; + let remaining = (body_end - request.len()).min(chunk.len()); + let length = stream.read(&mut chunk[..remaining])?; + if length == 0 { return reply(&mut stream, 400, "text/plain", "bad request"); } request.extend_from_slice(&chunk[..length]); - if let Some(separator) = request.windows(4).position(|window| window == b"\r\n\r\n") { - let end = separator + 4; - let head = String::from_utf8_lossy(&request[..separator]); - let content_length = header(&head, "Content-Length") - .and_then(|value| value.parse::().ok()) - .unwrap_or(0); - if content_length > 60 * 1024 { - return reply(&mut stream, 400, "text/plain", "bad request"); - } - if request.len() >= end + content_length { - break end; - } - } - }; - let head = String::from_utf8_lossy(&request[..header_end - 4]); - let body = &request[header_end..]; + } + // This connection serves one request and closes. Ignore any pipelined bytes. + let head = std::str::from_utf8(&request[..header_end - 4]).expect("validated HTTP headers"); + let body = &request[header_end..body_end]; let mut parts = head.lines().next().unwrap_or_default().split_whitespace(); let method = parts.next().unwrap_or_default(); let path = parts.next().unwrap_or_default(); - if method == "POST" && !same_origin(&head, origin) { + if method == "POST" && !same_origin(head, origin) { return reply(&mut stream, 403, "text/plain", "forbidden"); } match (method, path) { @@ -74,32 +158,54 @@ pub fn serve_connection( APP, ), ("GET", "/styles.css") => reply(&mut stream, 200, "text/css; charset=utf-8", CSS), + ("GET", "/view.js") => reply( + &mut stream, + 200, + "application/javascript; charset=utf-8", + VIEW, + ), + ("GET", "/api/view") => json(&mut stream, 200, &state.view()), ("GET", "/api/state") => json(&mut stream, 200, &state.snapshot()), ("GET", "/api/config") => json(&mut stream, 200, &state.configuration()), ("GET", "/api/packets") => json(&mut stream, 200, &state.packets()), ("GET", "/api/logs") => json(&mut stream, 200, &state.logs()), ("POST", "/api/connect") => match serde_json::from_slice::(body) { - Ok(value) if !value.path.trim().is_empty() => match state.begin_connect() { - Ok(()) => { - let result = serial.connect(value.path.trim().into()); - if let Err(error) = &result { - state.worker_failed(error.clone()); - } - endpoint(&mut stream, result) - } - Err(error) => json(&mut stream, 409, &serde_json::json!({"error":error})), - }, + Ok(value) if !value.path.trim().is_empty() => endpoint( + &mut stream, + state, + serial.connect(value.path.trim().into(), value.context), + ), _ => json( &mut stream, 400, &serde_json::json!({"error":"A serial device path is required."}), ), }, - ("POST", "/api/disconnect") => endpoint(&mut stream, serial.send(Command::Disconnect)), + ("POST", "/api/disconnect" | "/api/config/save" | "/api/config/reset") => { + match serde_json::from_slice::(body) { + Ok(mutation) => { + let action = match path { + "/api/disconnect" => Action::Disconnect, + "/api/config/save" => Action::Save, + _ => Action::ResetSaved, + }; + endpoint(&mut stream, state, serial.submit(mutation.context, action)) + } + Err(_) => json( + &mut stream, + 400, + &serde_json::json!({"error":"Expected the displayed view context."}), + ), + } + } ("POST", "/api/config/edit") => match serde_json::from_slice::(body) { - Ok(edit) => match state.edit(edit.field_id, &edit.value) { - Ok(config) => json(&mut stream, 200, &config), - Err(error) => json(&mut stream, 409, &serde_json::json!({"error":error})), + Ok(edit) => match state.edit_context(edit.context, edit.field_id, &edit.value) { + Ok(view) => json(&mut stream, 200, &view), + Err(error) => json( + &mut stream, + 409, + &serde_json::json!({"error":error, "view":state.view()}), + ), }, Err(_) => json( &mut stream, @@ -107,20 +213,46 @@ pub fn serve_connection( &serde_json::json!({"error":"Expected a stable field_id and typed value."}), ), }, - ("POST", "/api/config/save") => match state.begin_save() { - Ok(save) => { - let result = serial.send(Command::Save(save)); - if let Err(error) = &result { - state.worker_failed(error.clone()); - } - endpoint(&mut stream, result) - } - Err(error) => json(&mut stream, 409, &serde_json::json!({"error":error})), - }, _ => reply(&mut stream, 404, "text/plain", "not found"), } } +fn content_length(head: &[u8]) -> Option { + let head = std::str::from_utf8(head).ok()?; + let mut lines = head.split("\r\n"); + if lines.next()?.bytes().any(|byte| byte.is_ascii_control()) { + return None; + } + let mut content_length = None; + for line in lines { + let (key, value) = line.split_once(':')?; + if key.is_empty() + || !key + .bytes() + .all(|byte| byte.is_ascii_alphanumeric() || b"!#$%&'*+-.^_`|~".contains(&byte)) + || value + .bytes() + .any(|byte| byte.is_ascii_control() && byte != b'\t') + { + return None; + } + if key.eq_ignore_ascii_case("Transfer-Encoding") { + return None; + } + if key.eq_ignore_ascii_case("Content-Length") { + let value = value.trim_matches([' ', '\t']); + if content_length.is_some() + || value.is_empty() + || !value.bytes().all(|byte| byte.is_ascii_digit()) + { + return None; + } + content_length = Some(value.parse::().ok()?); + } + } + Some(content_length.unwrap_or(0)) +} + fn same_origin(head: &str, origin: &str) -> bool { let Some(expected_host) = origin.strip_prefix("http://") else { return false; @@ -135,14 +267,22 @@ fn header<'a>(head: &'a str, name: &str) -> Option<&'a str> { }) } -fn endpoint(stream: &mut TcpStream, result: Result<(), String>) -> std::io::Result<()> { +fn endpoint( + stream: &mut HttpConnection, + state: &SharedState, + result: Result<(), String>, +) -> std::io::Result<()> { match result { - Ok(()) => json(stream, 202, &serde_json::json!({"ok":true})), - Err(error) => json(stream, 409, &serde_json::json!({"error":error})), + Ok(()) => json(stream, 202, &state.view()), + Err(error) => json( + stream, + 409, + &serde_json::json!({"error":error, "view":state.view()}), + ), } } -fn json(stream: &mut TcpStream, status: u16, value: &T) -> std::io::Result<()> { +fn json(stream: &mut HttpConnection, status: u16, value: &T) -> std::io::Result<()> { match serde_json::to_string(value) { Ok(body) => reply(stream, status, "application/json; charset=utf-8", &body), Err(_) => reply(stream, 500, "text/plain", "serialization error"), @@ -150,7 +290,7 @@ fn json(stream: &mut TcpStream, status: u16, value: &T) -> std::io } fn reply( - stream: &mut TcpStream, + stream: &mut HttpConnection, status: u16, content_type: &str, body: &str, @@ -171,6 +311,10 @@ fn reply( ) } +#[cfg(test)] +#[path = "api_hosted_tests.rs"] +mod hosted_tests; + #[cfg(test)] mod tests { use super::*; @@ -213,23 +357,4 @@ mod tests { assert!(APP.contains("truncated.textContent = 'TRUNCATED'")); assert!(INDEX.contains("id=\"log-entries\"")); } - - #[test] - fn browser_clears_recovered_errors_and_retries_failed_configuration_fetches() { - assert!(APP.contains("showError(state.error ?? '')")); - assert!(APP.contains("q('#configuration').hidden = !state.has_config")); - - let fetch = APP - .find("const config = await api('/api/config')") - .expect("configuration refresh fetches the latest document"); - let render = APP[fetch..] - .find("renderConfig(config)") - .map(|offset| fetch + offset) - .expect("a fetched configuration is rendered"); - let revision = APP[render..] - .find("configRevision = state.config_revision") - .map(|offset| render + offset) - .expect("a successful render advances the observed revision"); - assert!(fetch < render && render < revision); - } } diff --git a/firmware/tools/configurator/src/api_hosted_tests.rs b/firmware/tools/configurator/src/api_hosted_tests.rs new file mode 100644 index 0000000..5539059 --- /dev/null +++ b/firmware/tools/configurator/src/api_hosted_tests.rs @@ -0,0 +1,416 @@ +use super::*; +use crate::{config_client::ClientEvent, serial_worker::SerialBaud, state::tests::unknown_board}; +use kaptan_file_mavlink::Principal; +use std::{net::TcpListener, os::fd::AsRawFd, sync::mpsc, thread}; + +fn controller(state: &Arc) -> Arc { + Arc::new( + SerialController::new( + Arc::clone(state), + SerialBaud::DEFAULT, + Principal::new(255, 190), + ) + .unwrap(), + ) +} + +fn request( + state: &Arc, + serial: &Arc, + path: &str, + body: Option, +) -> (u16, serde_json::Value) { + let listener = TcpListener::bind("127.0.0.1:0").unwrap(); + let address = listener.local_addr().unwrap(); + let state = Arc::clone(state); + let serial = Arc::clone(serial); + let server = thread::spawn(move || { + serve_connection( + listener.accept().unwrap().0, + &state, + &serial, + &format!("http://{address}"), + ) + .unwrap(); + }); + let mut client = TcpStream::connect(address).unwrap(); + client + .set_read_timeout(Some(Duration::from_secs(2))) + .unwrap(); + let method = if body.is_some() { "POST" } else { "GET" }; + let body = body.map(|value| value.to_string()).unwrap_or_default(); + write!(client, "{method} {path} HTTP/1.1\r\nHost: {address}\r\nOrigin: http://{address}\r\nContent-Length: {}\r\n\r\n{body}", body.len()).unwrap(); + let mut response = String::new(); + client.read_to_string(&mut response).unwrap(); + server.join().unwrap(); + let (head, body) = response.split_once("\r\n\r\n").unwrap(); + let status = head.split_whitespace().nth(1).unwrap().parse().unwrap(); + (status, serde_json::from_str(body).unwrap()) +} + +#[test] +fn stale_tab_cannot_edit_save_reset_or_disconnect_a_new_controller_over_http() { + let state = Arc::new(SharedState::default()); + let serial = controller(&state); + state.connected("controller-A".into()); + state.configuration_event(ClientEvent::Configuration(unknown_board())); + let (status, a) = request(&state, &serial, "/api/view", None); + assert_eq!(status, 200); + let context = a["state"]["context"].clone(); + assert!(context["connection"].is_string()); + state.disconnected(None); + state.connected("controller-B".into()); + state.configuration_event(ClientEvent::Configuration(unknown_board())); + state.edit(401, &serde_json::json!(50)).unwrap(); + let before = state.snapshot(); + for path in [ + "/api/config/edit", + "/api/config/save", + "/api/config/reset", + "/api/disconnect", + ] { + let (status, response) = request( + &state, + &serial, + path, + Some(serde_json::json!({ + "context": context, "field_id": 401, "value": 60, + })), + ); + assert_eq!(status, 409, "{path}: {response}"); + assert!(response["error"].as_str().unwrap().contains("stale view")); + assert_eq!(state.snapshot().config_revision, before.config_revision); + assert_eq!(state.snapshot().path.as_deref(), Some("controller-B")); + assert_eq!( + state.configuration().fields[0].draft, + Some(serde_json::json!(50)) + ); + } + state.disconnected(None); + let (status, _) = request( + &state, + &serial, + "/api/connect", + Some(serde_json::json!({ + "context": context, "path": "/not-opened", + })), + ); + assert_eq!(status, 409); + assert!(!state.snapshot().can_disconnect); + assert_eq!( + request( + &state, + &serial, + "/api/config/reset", + Some(serde_json::json!({})) + ) + .0, + 400 + ); +} + +#[test] +fn idle_http_connection_expires_and_the_sequential_server_serves_the_next_client() { + let state = Arc::new(SharedState::default()); + let serial = controller(&state); + let listener = TcpListener::bind("127.0.0.1:0").unwrap(); + let address = listener.local_addr().unwrap(); + let (accepted, ready) = mpsc::channel(); + let server = thread::spawn(move || { + let stream = listener.accept().unwrap().0; + accepted.send(()).unwrap(); + let started = Instant::now(); + let error = serve_http( + HttpConnection::new(stream, Duration::from_millis(40)), + &state, + &serial, + &format!("http://{address}"), + ) + .unwrap_err(); + assert!(matches!( + error.kind(), + std::io::ErrorKind::TimedOut | std::io::ErrorKind::WouldBlock + )); + assert!(started.elapsed() < Duration::from_secs(1)); + serve_connection( + listener.accept().unwrap().0, + &state, + &serial, + &format!("http://{address}"), + ) + .unwrap(); + }); + let _idle = TcpStream::connect(address).unwrap(); + ready.recv_timeout(Duration::from_secs(1)).unwrap(); + let mut next = TcpStream::connect(address).unwrap(); + next.set_read_timeout(Some(Duration::from_secs(1))).unwrap(); + next.write_all(b"GET /api/view HTTP/1.1\r\n\r\n").unwrap(); + let mut response = String::new(); + next.read_to_string(&mut response).unwrap(); + assert!(response.starts_with("HTTP/1.1 200")); + server.join().unwrap(); +} + +#[test] +fn http_deadlines_are_total_even_after_partial_progress() { + let listener = TcpListener::bind("127.0.0.1:0").unwrap(); + let mut client = TcpStream::connect(listener.local_addr().unwrap()).unwrap(); + let server = listener.accept().unwrap().0; + let mut connection = HttpConnection::new(server, Duration::from_millis(40)); + client.write_all(b"xy").unwrap(); + let mut byte = [0]; + connection.read_exact(&mut byte).unwrap(); + connection.read_deadline = Instant::now(); + assert_eq!( + connection.read(&mut byte).unwrap_err().kind(), + std::io::ErrorKind::TimedOut + ); + + let size: libc::c_int = 4096; + // SAFETY: the live TCP fd and pointer/length describe one integer option. + let configured = unsafe { + libc::setsockopt( + connection.stream.as_raw_fd(), + libc::SOL_SOCKET, + libc::SO_SNDBUF, + (&size as *const libc::c_int).cast(), + std::mem::size_of_val(&size) as libc::socklen_t, + ) + }; + assert_eq!(configured, 0); + let started = Instant::now(); + let error = connection.write_all(&vec![0; 4 * 1024 * 1024]).unwrap_err(); + assert!(matches!( + error.kind(), + std::io::ErrorKind::TimedOut | std::io::ErrorKind::WouldBlock + )); + assert!(started.elapsed() < Duration::from_secs(1)); + connection.write_deadline = Some(Instant::now()); + assert_eq!( + connection.write(b"z").unwrap_err().kind(), + std::io::ErrorKind::TimedOut + ); +} + +fn raw_request( + state: &Arc, + serial: &Arc, + fragments: impl FnOnce(String) -> Vec>, +) -> (u16, String) { + let listener = TcpListener::bind("127.0.0.1:0").unwrap(); + let address = listener.local_addr().unwrap(); + let state = Arc::clone(state); + let serial = Arc::clone(serial); + let server = thread::spawn(move || { + serve_connection( + listener.accept().unwrap().0, + &state, + &serial, + &format!("http://{address}"), + ) + .unwrap(); + }); + let mut client = TcpStream::connect(address).unwrap(); + client.set_nodelay(true).unwrap(); + client + .set_read_timeout(Some(Duration::from_secs(3))) + .unwrap(); + for (index, fragment) in fragments(address.to_string()).iter().enumerate() { + if index != 0 { + thread::sleep(Duration::from_millis(2)); + } + client.write_all(fragment).unwrap(); + } + if let Err(error) = client.shutdown(std::net::Shutdown::Write) { + assert!(matches!( + error.kind(), + std::io::ErrorKind::NotConnected | std::io::ErrorKind::ConnectionReset + )); + } + let mut response = String::new(); + if let Err(error) = client.read_to_string(&mut response) { + // Closing a one-request connection with unread pipeline bytes can reset + // TCP. A complete response is still required below. + assert_eq!(error.kind(), std::io::ErrorKind::ConnectionReset); + } + server.join().unwrap(); + let (head, body) = response.split_once("\r\n\r\n").unwrap(); + assert_eq!( + header(head, "Content-Length") + .unwrap() + .parse::() + .unwrap(), + body.len() + ); + assert_eq!(header(head, "Connection"), Some("close")); + let status = head.split_whitespace().nth(1).unwrap().parse().unwrap(); + (status, body.into()) +} + +fn active_edit() -> (Arc, Arc, String) { + let state = Arc::new(SharedState::default()); + let serial = controller(&state); + state.connected("parser-test-controller".into()); + state.configuration_event(ClientEvent::Configuration(unknown_board())); + let body = serde_json::json!({ + "context": state.snapshot().context, "field_id": 401, "value": 50, + }) + .to_string(); + (state, serial, body) +} + +#[test] +fn malformed_framing_cannot_apply_a_current_context_edit() { + let (state, serial, body) = active_edit(); + let before = serde_json::to_value(state.view()).unwrap(); + for framing in [ + "Content-Length:".into(), + "Content-Length: invalid".into(), + format!("Content-Length: +{}", body.len()), + "Content-Length: -1".into(), + format!("Content-Length: {}", usize::MAX), + format!("Content-Length: {}0", usize::MAX), + format!("Content-Length: {}, {}", body.len(), body.len()), + format!( + "Content-Length: {}\r\ncOnTeNt-LeNgTh: {}", + body.len(), + body.len() + ), + format!("Content-Length: {}\r\nContent-Length: 0", body.len()), + format!("Content-Length : {}", body.len()), + "Transfer-Encoding: chunked".into(), + format!( + "Content-Length: {}\r\nTransfer-Encoding: identity", + body.len() + ), + format!("Content-Length: {}", body.len() + 1), + ] { + let (status, response) = raw_request(&state, &serial, |address| { + vec![format!( + "POST /api/config/edit HTTP/1.1\r\nHost: {address}\r\nOrigin: http://{address}\r\n{framing}\r\n\r\n{body}" + ).into_bytes()] + }); + assert_eq!(status, 400, "{framing}: {response}"); + assert_eq!(serde_json::to_value(state.view()).unwrap(), before); + } + // The same context and body are actionable when framing is valid. + assert_eq!( + request( + &state, + &serial, + "/api/config/edit", + Some(serde_json::from_str(&body).unwrap()) + ) + .0, + 200 + ); + assert_eq!( + state.configuration().fields[0].draft, + Some(serde_json::json!(50)) + ); +} + +#[test] +fn fragmented_headers_separator_and_body_apply_one_current_edit() { + let (state, serial, body) = active_edit(); + let (status, response) = raw_request(&state, &serial, |address| { + let mut fragments = vec![ + format!("POST /api/config/edit HTTP/1.1\r\nHost: {address}\r\nOrigin: http://{address}\r\nContent-Len").into_bytes(), + format!("gth: {}", body.len()).into_bytes(), + b"\r".to_vec(), b"\n".to_vec(), b"\r".to_vec(), b"\n".to_vec(), + ]; + fragments.extend(body.as_bytes().chunks(13).map(<[u8]>::to_vec)); + fragments + }); + assert_eq!(status, 200, "{response}"); + assert_eq!( + state.configuration().fields[0].draft, + Some(serde_json::json!(50)) + ); +} + +#[test] +fn malformed_request_line_and_wrong_origin_cannot_edit_an_active_context() { + let (state, serial, body) = active_edit(); + let before = serde_json::to_value(state.view()).unwrap(); + for (line, same_origin, expected) in [ + ( + "POST /api/config/edit HTTP/1.1\nContent-Length: 0", + true, + 400, + ), + ("POST /api/config/edit HTTP/1.1", false, 403), + ] { + let (status, _) = raw_request(&state, &serial, |address| { + let origin = if same_origin { + format!("http://{address}") + } else { + "http://example.invalid".into() + }; + vec![format!( + "{line}\r\nHost: {address}\r\nOrigin: {origin}\r\nContent-Length: {}\r\n\r\n{body}", body.len() + ).into_bytes()] + }); + assert_eq!(status, expected); + assert_eq!(serde_json::to_value(state.view()).unwrap(), before); + } +} + +#[test] +fn exact_body_length_excludes_trailing_bytes_and_pipeline_is_not_dispatched() { + let (state, serial, body) = active_edit(); + let before = serde_json::to_value(state.view()).unwrap(); + let (status, _) = raw_request(&state, &serial, |address| { + vec![format!( + "GET /api/view HTTP/1.1\r\n\r\nPOST /api/config/edit HTTP/1.1\r\nHost: {address}\r\nOrigin: http://{address}\r\nContent-Length: {}\r\n\r\n{body}", body.len() + ).into_bytes()] + }); + assert_eq!(status, 200); + assert_eq!(serde_json::to_value(state.view()).unwrap(), before); + let (status, response) = raw_request(&state, &serial, |address| { + vec![format!( + "POST /api/config/edit HTTP/1.1\r\nHost: {address}\r\nOrigin: http://{address}\r\nContent-Length: {}\r\n\r\n{body}GET /api/view HTTP/1.1\r\n\r\n", body.len() + ).into_bytes()] + }); + assert_eq!(status, 200, "{response}"); + assert_eq!( + state.configuration().fields[0].draft, + Some(serde_json::json!(50)) + ); +} + +#[test] +fn request_and_body_caps_reject_before_mutation_and_accept_exact_body_cap() { + let (state, serial, body) = active_edit(); + let before = serde_json::to_value(state.view()).unwrap(); + for (extra_header, length) in [ + (String::new(), MAX_BODY_BYTES + 1), + ( + format!("X-Padding: {}\r\n", "x".repeat(4096)), + MAX_BODY_BYTES, + ), + ( + format!("X-Padding: {}\r\n", "x".repeat(MAX_REQUEST_BYTES)), + body.len(), + ), + ] { + let (status, _) = raw_request(&state, &serial, |address| { + vec![format!( + "POST /api/config/edit HTTP/1.1\r\nHost: {address}\r\nOrigin: http://{address}\r\n{extra_header}Content-Length: {length}\r\n\r\n{body}" + ).into_bytes()] + }); + assert_eq!(status, 400); + assert_eq!(serde_json::to_value(state.view()).unwrap(), before); + } + let (status, response) = raw_request(&state, &serial, |address| { + let padding = " ".repeat(MAX_BODY_BYTES - body.len()); + vec![format!( + "POST /api/config/edit HTTP/1.1\r\nHost: {address}\r\nOrigin: http://{address}\r\nContent-Length: {MAX_BODY_BYTES}\r\n\r\n{body}{padding}" + ).into_bytes()] + }); + assert_eq!(status, 200, "{response}"); + assert_eq!( + state.configuration().fields[0].draft, + Some(serde_json::json!(50)) + ); +} diff --git a/firmware/tools/configurator/src/config_client.rs b/firmware/tools/configurator/src/config_client.rs index 98662a9..0c8acf2 100644 --- a/firmware/tools/configurator/src/config_client.rs +++ b/firmware/tools/configurator/src/config_client.rs @@ -14,12 +14,12 @@ use kaptan_mavlink::receiver::IntegrityMavlinkReceiver; use kaptan_mavlink::{MavlinkFrame, encoder::MavlinkEncoder, generated::kaptan::MavMessage}; const CONFIG_PATH: &[u8] = b"/config/user.kcfg"; +const ACTIVE_PATH: &[u8] = b"/config/active.kcfg"; +const RESET_PATH: &[u8] = b"/config/reset.kcfg"; const CHUNK_BYTES: usize = DATA_BYTES; const MAX_CONFIG_FILE_BYTES: usize = 1024 * 1024; const REQUEST_TIMEOUT: Duration = Duration::from_millis(500); -// Closing a write may erase a 128-KiB STM32F7 sector. The controller cannot -// service serial traffic during that erase, so this operation gets its own -// bounded timeout. +// Closing a write may erase a 128-KiB STM32F7 sector, blocking serial service. const CLOSE_WRITE_TIMEOUT: Duration = Duration::from_secs(5); const REQUEST_ATTEMPTS: u8 = 3; // The extra second keeps the retry beyond the controller's lease despite @@ -45,26 +45,30 @@ pub(crate) fn scalar_json(value: ConfigValue) -> serde_json::Value { ConfigValue::U32(value) => value.into(), // JSON numbers cannot preserve the full u64 domain in a browser. ConfigValue::U64(value) => value.to_string().into(), - ConfigValue::F32(value) => serde_json::Value::from(value), - ConfigValue::F64(value) => serde_json::Value::from(value), + // Strings preserve signed zero through JavaScript. Nonfinite values + // carry all IEEE bits, including NaN sign, signaling bit and payload. + ConfigValue::F32(value) if value.is_finite() => value.to_string().into(), + ConfigValue::F64(value) if value.is_finite() => value.to_string().into(), + ConfigValue::F32(value) => format!("bits:0x{:08x}", value.to_bits()).into(), + ConfigValue::F64(value) => format!("bits:0x{:016x}", value.to_bits()).into(), } } -pub(crate) const fn scalar_less_than(left: ConfigValue, right: ConfigValue) -> bool { +const fn scalar_less_equal(left: ConfigValue, right: ConfigValue) -> bool { match (left, right) { - (ConfigValue::I32(left), ConfigValue::I32(right)) => left < right, - (ConfigValue::U8(left), ConfigValue::U8(right)) => left < right, - (ConfigValue::U16(left), ConfigValue::U16(right)) => left < right, - (ConfigValue::U32(left), ConfigValue::U32(right)) => left < right, - (ConfigValue::U64(left), ConfigValue::U64(right)) => left < right, - (ConfigValue::F32(left), ConfigValue::F32(right)) => left < right, - (ConfigValue::F64(left), ConfigValue::F64(right)) => left < right, - (ConfigValue::Bool(left), ConfigValue::Bool(right)) => !left && right, + (ConfigValue::I32(left), ConfigValue::I32(right)) => left <= right, + (ConfigValue::U8(left), ConfigValue::U8(right)) => left <= right, + (ConfigValue::U16(left), ConfigValue::U16(right)) => left <= right, + (ConfigValue::U32(left), ConfigValue::U32(right)) => left <= right, + (ConfigValue::U64(left), ConfigValue::U64(right)) => left <= right, + (ConfigValue::F32(left), ConfigValue::F32(right)) => left <= right, + (ConfigValue::F64(left), ConfigValue::F64(right)) => left <= right, + (ConfigValue::Bool(left), ConfigValue::Bool(right)) => !left || right, _ => false, } } -#[derive(Clone)] +#[derive(Clone, PartialEq)] pub(crate) struct RemoteEnumOption { pub(crate) value: u8, pub(crate) label: String, @@ -80,16 +84,70 @@ pub(crate) struct RemoteField { pub(crate) maximum: Option, pub(crate) factory: ConfigValue, pub(crate) value: ConfigValue, + pub(crate) active: Option, pub(crate) options: Vec, + pub(crate) finite: bool, + pub(crate) nonzero: bool, +} + +impl RemoteField { + pub(crate) fn validate_value(&self, value: ConfigValue) -> Result<(), String> { + if value.kind() != self.kind { + return Err(format!("{} has the wrong value type", self.key)); + } + if self.finite && non_finite(value) { + return Err(format!("{} must be finite", self.key)); + } + let zero = match value { + ConfigValue::Bool(_) => false, + ConfigValue::U8(v) => v == 0, + ConfigValue::U16(v) => v == 0, + ConfigValue::I32(v) => v == 0, + ConfigValue::U32(v) => v == 0, + ConfigValue::U64(v) => v == 0, + ConfigValue::F32(v) => v == 0.0, + ConfigValue::F64(v) => v == 0.0, + }; + if self.nonzero && zero { + return Err(format!("{} must be nonzero", self.key)); + } + if self + .minimum + .is_some_and(|minimum| !scalar_less_equal(minimum, value)) + || self + .maximum + .is_some_and(|maximum| !scalar_less_equal(value, maximum)) + { + return Err(format!("{} is outside its inclusive bounds", self.key)); + } + if !self.options.is_empty() + && !matches!(value, ConfigValue::U8(value) + if self.options.iter().any(|option| option.value == value)) + { + return Err(format!("{} requires a named choice", self.key)); + } + Ok(()) + } } #[derive(Clone)] pub(crate) struct RemoteConfig { pub(crate) schema_id: [u8; 16], pub(crate) restart_required: bool, + pub(crate) committed_error: Option, + /// Sorted by stable ID during decoding, for deterministic bounded lookups. pub(crate) fields: Vec, } +impl RemoteConfig { + pub(crate) fn field(&self, id: FieldId) -> Option<&RemoteField> { + self.fields + .binary_search_by_key(&id, |field| field.id) + .ok() + .map(|index| &self.fields[index]) + } +} + pub(crate) struct Edit { pub(crate) field_id: FieldId, pub(crate) value: ConfigValue, @@ -162,16 +220,16 @@ enum OpenConflictRecovery { Waiting(Instant), } -/// Downloads and imports exactly one self-describing configuration artifact. -/// -/// The client contains no board IDs, field names, schema IDs, or field catalog. -/// The sole known resource is the stable generic virtual path. +/// Downloads saved and boot-active configuration with their field metadata. pub(crate) struct ConfigClient { local: Principal, encoder: MavlinkEncoder, next_transfer_id: u16, target: Option, downloaded: Option, + active: Option, + reading_active: bool, + committed_error: Option, transfer: Option, pending: Option, abort_failure: Option, @@ -186,6 +244,9 @@ impl ConfigClient { next_transfer_id: 1, target: None, downloaded: None, + active: None, + reading_active: false, + committed_error: None, transfer: None, pending: None, abort_failure: None, @@ -196,6 +257,9 @@ impl ConfigClient { pub(crate) fn reset(&mut self) { self.target = None; self.downloaded = None; + self.active = None; + self.reading_active = false; + self.committed_error = None; self.transfer = None; self.pending = None; self.abort_failure = None; @@ -221,7 +285,12 @@ impl ConfigClient { file_size: 0, checksum_crc32c: 0, transfer_id, - content: Content::try_new(CONFIG_PATH).expect("static config path is bounded"), + content: Content::try_new(if self.reading_active { + ACTIVE_PATH + } else { + CONFIG_PATH + }) + .expect("static config path is bounded"), }, )) } @@ -236,9 +305,37 @@ impl ConfigClient { let downloaded = self .downloaded .as_ref() - .ok_or("configuration has not been downloaded")?; + .ok_or("saved configuration is unknown; re-read or reset saved configuration")?; + if self.active.is_none() { + return Err("boot-active configuration has not been downloaded".into()); + } validate_edits(&downloaded.config, &edits)?; let bytes = encode_edited_document(&downloaded.document, &edits)?; + self.start_write(bytes, CONFIG_PATH) + } + + pub(crate) fn reset_saved(&mut self) -> Result { + if self.pending.is_some() || self.transfer.is_some() { + return Err("configuration file transfer is busy".into()); + } + let active = self + .active + .as_ref() + .ok_or("boot-active configuration is unavailable")?; + let edits = active + .config + .fields + .iter() + .map(|field| Edit { + field_id: field.id, + value: field.factory, + }) + .collect::>(); + let bytes = encode_edited_document(&active.document, &edits)?; + self.start_write(bytes, RESET_PATH) + } + + fn start_write(&mut self, bytes: Vec, path: &[u8]) -> Result { let size = u32::try_from(bytes.len()) .map_err(|_| "configuration artifact exceeds the file protocol".to_owned())?; let checksum = crc32c(&bytes); @@ -259,7 +356,7 @@ impl ConfigClient { file_size: size, checksum_crc32c: checksum, transfer_id, - content: Content::try_new(CONFIG_PATH).expect("static config path is bounded"), + content: Content::try_new(path).expect("static config path is bounded"), }, )) } @@ -317,7 +414,7 @@ impl ConfigClient { ); if matches!(kind, PendingKind::CloseWrite { .. }) { return self.durable_state_unknown(format!( - "{detail}; the durable state is unknown, so power-cycle the controller and reconnect before another save" + "{detail}; saved configuration is unknown; re-read or reset saved configuration before another save" )); } if matches!(kind, PendingKind::Abort { .. }) { @@ -329,6 +426,12 @@ impl ConfigClient { self.abort_after(detail) } + pub(crate) fn transmitted(&mut self) { + if let Some(pending) = &mut self.pending { + pending.sent_at = Instant::now(); + } + } + fn observe_control(&mut self, data: ControlResponse) -> Outcome { if data.target != self.local_target() { return Outcome::Idle; @@ -351,6 +454,12 @@ impl ConfigClient { .take() .unwrap_or_else(|| "configuration file transfer aborted".into()); self.transfer = None; + if data.status == FileStatus::Ok + && !self.reading_active + && self.committed_error.is_some() + { + return self.begin_active_read(); + } return if data.status == FileStatus::Ok { self.fail(original) } else { @@ -380,13 +489,35 @@ impl ConfigClient { } if data.status != FileStatus::Ok { let detail = rejected_detail(&data); + if !self.reading_active + && data.status == FileStatus::StorageFailure + && matches!( + kind, + PendingKind::OpenRead { .. } + | PendingKind::Read { .. } + | PendingKind::CloseRead { .. } + ) + { + self.downloaded = None; + self.committed_error = Some(detail.clone()); + return if matches!(kind, PendingKind::OpenRead { .. }) { + self.begin_active_read() + } else { + self.abort_after(detail) + }; + } return match kind { PendingKind::OpenRead { .. } => self.rediscovery_required(detail), - PendingKind::OpenWrite => self.fail(detail), - PendingKind::CloseWrite { .. } if data.status == FileStatus::StorageFailure => self + PendingKind::OpenWrite | PendingKind::CloseWrite { .. } if data.status == FileStatus::StorageFailure => self .durable_state_unknown(format!( - "{detail}; power-cycle the controller and reconnect before another save" + "{detail}; saved configuration is unknown; re-read or reset saved configuration" )), + PendingKind::OpenWrite => self.fail(detail), + PendingKind::CloseWrite { .. } if data.status == FileStatus::RequestConflict => { + self.rediscovery_required(format!( + "{detail}; obsolete saved baseline: stale draft discarded; re-read required" + )) + } PendingKind::Read { .. } | PendingKind::Write { .. } | PendingKind::CloseRead { .. } @@ -466,12 +597,6 @@ impl ConfigClient { self.abort_after("configuration file checksum did not match".into()) } else { let transfer_id = read.id; - let document = std::mem::take(&mut read.bytes); - let config = match decode_document(&document) { - Ok(config) => config, - Err(error) => return self.abort_after(error), - }; - self.downloaded = Some(DownloadedConfig { document, config }); let request = self.request_close(PendingKind::CloseRead { position: next }, transfer_id, next); self.follow_up(request) @@ -511,17 +636,86 @@ impl ConfigClient { if data.status != FileStatus::Ok || data.position != expected_position { return self.fail("controller did not close the configuration export"); } - self.transfer = None; - let Some(config) = self - .downloaded - .as_ref() - .map(|downloaded| downloaded.config.clone()) - else { - return self.fail("configuration artifact was not decoded"); + let Some(Transfer::Read(read)) = self.transfer.take() else { + return self.rediscovery_required("configuration export has no artifact"); + }; + let config = match decode_document(&read.bytes) { + Ok(config) => config, + Err(error) if !self.reading_active => { + self.committed_error = Some(error); + return self.begin_active_read(); + } + Err(error) => { + return self.rediscovery_required(format!("boot-active configuration: {error}")); + } + }; + let downloaded = DownloadedConfig { + document: read.bytes, + config, + }; + if !self.reading_active { + self.downloaded = Some(downloaded); + return self.begin_active_read(); + } + self.active = Some(downloaded); + let config = match self.combined_config() { + Ok(config) => config, + Err(error) => return self.rediscovery_required(error), }; Outcome::Event(ClientEvent::Configuration(config)) } + fn begin_active_read(&mut self) -> Outcome { + self.transfer = None; + self.reading_active = true; + self.open_conflict_recovery = OpenConflictRecovery::FreshId; + let request = self.request_open_read(); + self.follow_up(request) + } + + fn combined_config(&self) -> Result { + let active = &self + .active + .as_ref() + .ok_or("boot-active configuration unavailable")? + .config; + let mut config = self + .downloaded + .as_ref() + .map_or_else(|| active.clone(), |saved| saved.config.clone()); + if config.schema_id != active.schema_id || config.fields.len() != active.fields.len() { + return Err( + "saved and boot-active configuration schemas differ; re-read required".into(), + ); + } + for (field, boot) in config.fields.iter_mut().zip(&active.fields) { + if field.id != boot.id { + return Err("saved and boot-active field IDs differ; re-read required".into()); + } + if field.key != boot.key + || field.kind != boot.kind + || field.unit != boot.unit + || field.minimum != boot.minimum + || field.maximum != boot.maximum + || field.factory != boot.factory + || field.options != boot.options + || field.finite != boot.finite + || field.nonzero != boot.nonzero + { + return Err("saved and boot-active field metadata differ; re-read required".into()); + } + field.active = Some(boot.value); + } + config.committed_error = self.committed_error.clone(); + config.restart_required = self.downloaded.is_some() + && (config.restart_required + || config + .fields + .iter() + .any(|field| field.active != Some(field.value))); + Ok(config) + } + fn opened_write(&mut self, data: ControlResponse) -> Outcome { let Some(Transfer::Write(write)) = self.transfer.as_mut() else { return self.fail("configuration write response has no pending artifact"); @@ -588,23 +782,14 @@ impl ConfigClient { "controller returned an invalid configuration import result", ); } - let restart_required = result.restart_required(); - let Some(Transfer::Write(write)) = self.transfer.take() else { - return self.rediscovery_required("completed save has no local artifact"); - }; - let document = match encode_document_state(&write.bytes, restart_required) { - Ok(document) => document, + // The uploaded artifact still carries the *old* expected base. Only a + // fresh target export is a baseline for the next save. + let target = match self.target() { + Ok(target) => target, Err(error) => return self.rediscovery_required(error), }; - let config = match decode_document(&document) { - Ok(config) => config, - Err(error) => return self.rediscovery_required(error), - }; - self.downloaded = Some(DownloadedConfig { - document, - config: config.clone(), - }); - Outcome::Event(ClientEvent::Configuration(config)) + let request = self.discover(target); + self.follow_up(request) } fn request_read(&mut self, position: u32) -> Result { @@ -767,6 +952,11 @@ impl ConfigClient { } fn fail(&mut self, detail: impl Into) -> Outcome { + // A partial discovery is never a usable saved/active pair, including + // readback after a successful commit. Drop the old UI baseline too. + if self.active.is_none() { + return self.rediscovery_required(detail); + } self.pending = None; self.transfer = None; self.abort_failure = None; @@ -784,6 +974,20 @@ impl ConfigClient { Outcome::Event(ClientEvent::DurableStateUnknown(detail)) } + pub(crate) fn interruption(&mut self) -> Option { + if matches!( + self.pending.as_ref().map(|pending| pending.kind), + Some(PendingKind::CloseWrite { .. }) + ) { + let Outcome::Event(event) = self.durable_state_unknown( + "Link interrupted during configuration close; saved configuration is unknown. Re-read or reset saved configuration after reconnecting." + ) else { unreachable!() }; + Some(event) + } else { + None + } + } + const fn local_target(&self) -> Target { Target::new(self.local.system(), self.local.component()) } @@ -804,27 +1008,10 @@ impl ConfigClient { fn validate_edits(config: &RemoteConfig, edits: &[Edit]) -> Result<(), String> { for edit in edits { - let Some(field) = config.fields.iter().find(|field| field.id == edit.field_id) else { + let Some(field) = config.field(edit.field_id) else { return Err(format!("unknown field id {}", edit.field_id.get())); }; - if edit.value.kind() != field.kind { - return Err(format!( - "field {} has the wrong value type", - edit.field_id.get() - )); - } - if !field.options.is_empty() - && !matches!( - edit.value, - ConfigValue::U8(value) - if field.options.iter().any(|option| option.value == value) - ) - { - return Err(format!( - "field {} has an unnamed enum value", - edit.field_id.get() - )); - } + field.validate_value(edit.value)?; } Ok(()) } @@ -905,14 +1092,27 @@ fn rejected_detail(data: &ControlResponse) -> String { } } -// KCFG owns the schema and value representation. These two adapters are the -// only place where the host configurator depends on that artifact codec. fn decode_document(bytes: &[u8]) -> Result { - kaptan_config::decode_kcfg(bytes) + decode_artifact(bytes) .map_err(|error| format!("invalid KCFG artifact: {error:?}")) .and_then(remote_config) } +fn decode_artifact( + bytes: &[u8], +) -> Result, kaptan_config::KcfgDecodeError> { + // Initialize in heap storage. Box::new([0; 8192]) can create a large stack + // temporary in debug builds, including on the serial worker thread. + let mut scratch = vec![0_u8; kaptan_config::KCFG_FIELD_ID_SCRATCH_BYTES]; + kaptan_config::decode_kcfg( + bytes, + scratch + .as_mut_slice() + .try_into() + .expect("fixed scratch length"), + ) +} + fn remote_config(artifact: kaptan_config::KcfgArtifact<'_>) -> Result { let mut fields = Vec::with_capacity(usize::from(artifact.field_count())); for field in artifact.fields() { @@ -932,73 +1132,42 @@ fn remote_config(artifact: kaptan_config::KcfgArtifact<'_>) -> Result = fields.iter().map(|field| field.key.as_str()).collect(); + keys.sort_unstable(); + if keys.windows(2).any(|pair| pair[0] == pair[1]) { + return Err("KCFG artifact contains a duplicate display key".into()); + } + fields.sort_unstable_by_key(|field| field.id); Ok(RemoteConfig { schema_id: artifact.schema_id(), restart_required: artifact.restart_required(), + committed_error: None, fields, }) } fn validate_remote_field(field: &RemoteField) -> Result<(), String> { - let values = [ - Some(field.value), - Some(field.factory), - field.minimum, - field.maximum, - ]; - if values.into_iter().flatten().any(non_finite) { - return Err(format!( - "KCFG field {} contains a non-finite value", - field.id.get() - )); - } if field.minimum.is_some_and(|minimum| { field .maximum - .is_some_and(|maximum| scalar_less_than(maximum, minimum)) - }) || field - .minimum - .is_some_and(|minimum| scalar_less_than(field.value, minimum)) - || field - .maximum - .is_some_and(|maximum| scalar_less_than(maximum, field.value)) - || field - .minimum - .is_some_and(|minimum| scalar_less_than(field.factory, minimum)) - || field - .maximum - .is_some_and(|maximum| scalar_less_than(maximum, field.factory)) - { + .is_some_and(|maximum| !scalar_less_equal(minimum, maximum)) + }) { return Err(format!( "KCFG field {} has inconsistent bounds", field.id.get() )); } - if !field.options.is_empty() - && ![field.value, field.factory].into_iter().all(|value| { - matches!( - value, - ConfigValue::U8(value) - if field.options.iter().any(|option| option.value == value) - ) - }) - { - return Err(format!( - "KCFG field {} contains an unnamed enum value", - field.id.get() - )); - } + field.validate_value(field.value)?; + field.validate_value(field.factory)?; Ok(()) } @@ -1016,14 +1185,21 @@ const fn non_finite(value: ConfigValue) -> bool { } fn encode_edited_document(document: &[u8], edits: &[Edit]) -> Result, String> { - let artifact = kaptan_config::decode_kcfg(document) + let artifact = decode_artifact(document) .map_err(|error| format!("cannot edit KCFG artifact: {error:?}"))?; + let mut edits: Vec<_> = edits.iter().collect(); + edits.sort_unstable_by_key(|edit| edit.field_id); + if edits + .windows(2) + .any(|pair| pair[0].field_id == pair[1].field_id) + { + return Err("duplicate edited field id".into()); + } let mut output = vec![0_u8; document.len()]; let encoded = kaptan_config::encode_edited_kcfg(&artifact, true, &mut output, |field| { edits - .iter() - .find(|edit| edit.field_id == field.id()) - .map_or_else(|| field.value(), |edit| edit.value) + .binary_search_by_key(&field.id(), |edit| edit.field_id) + .map_or_else(|_| field.value(), |index| edits[index].value) }) .map_err(|error| format!("cannot encode KCFG artifact: {error:?}"))?; let length = encoded.len(); @@ -1031,22 +1207,12 @@ fn encode_edited_document(document: &[u8], edits: &[Edit]) -> Result, St Ok(output) } -fn encode_document_state(document: &[u8], restart_required: bool) -> Result, String> { - let artifact = kaptan_config::decode_kcfg(document) - .map_err(|error| format!("cannot update KCFG artifact state: {error:?}"))?; - let mut output = vec![0_u8; document.len()]; - let encoded = - kaptan_config::encode_edited_kcfg(&artifact, restart_required, &mut output, |field| { - field.value() - }) - .map_err(|error| format!("cannot encode KCFG artifact state: {error:?}"))?; - let length = encoded.len(); - output.truncate(length); - Ok(output) -} +#[cfg(test)] +#[path = "config_client_interop.rs"] +mod interop; #[cfg(test)] -mod tests { +pub(crate) mod tests { use super::*; use kaptan_file_mavlink::{ ControlResponse, DataResponse, FileRequest, FileResponse, FileStatus, @@ -1056,27 +1222,191 @@ mod tests { const CONTROLLER: Target = Target::new(42, 7); const CLIENT: Principal = Principal::new(255, 190); const FIELD_ID: FieldId = FieldId::new(401); + const ROLE_ID: FieldId = FieldId::new(402); + + fn maximum_document(duplicate_key: bool) -> Vec { + let mut bytes = vec![0; 24]; + bytes[..4].copy_from_slice(b"KCFG"); + bytes[4] = 3; + bytes[22..24].copy_from_slice(&(u16::MAX - 1).to_le_bytes()); + for id in (1..u16::MAX).rev() { + bytes.extend_from_slice(&id.to_le_bytes()); + bytes.extend_from_slice(&[ValueKind::U8 as u8, 0, 5, 0, 0, 1, 1, 1]); + let key = if duplicate_key && id == 1 { 2 } else { id }; + bytes.extend_from_slice(format!("k{key:04x}").as_bytes()); + } + bytes.extend_from_slice(&crc32c(&bytes).to_be_bytes()); + assert!(bytes.len() <= MAX_CONFIG_FILE_BYTES); + bytes + } + + #[test] + fn maximum_host_fields_sort_join_and_edit_with_a_late_duplicate_key_rejected() { + let bytes = maximum_document(false); + let config = decode_document(&bytes).unwrap(); + assert_eq!(config.fields.len(), 65_534); + assert_eq!(config.fields[0].id.get(), 1); + assert_eq!(config.fields[65_533].id.get(), 65_534); + let edits: Vec<_> = config + .fields + .iter() + .rev() + .map(|field| Edit { + field_id: field.id, + value: ConfigValue::U8(2), + }) + .collect(); + validate_edits(&config, &edits).unwrap(); + let edited = encode_edited_document(&bytes, &edits).unwrap(); + assert!( + decode_document(&edited) + .unwrap() + .fields + .iter() + .all(|field| field.value == ConfigValue::U8(2)) + ); + let mut client = ConfigClient::new(CLIENT); + client.downloaded = Some(DownloadedConfig { + document: edited.clone(), + config: decode_document(&edited).unwrap(), + }); + client.active = Some(DownloadedConfig { + document: bytes, + config, + }); + let combined = client.combined_config().unwrap(); + assert!(combined.restart_required); + assert!( + combined + .fields + .iter() + .all(|field| field.active == Some(ConfigValue::U8(1))) + ); + assert!( + decode_document(&maximum_document(true)) + .err() + .unwrap() + .contains("duplicate display key") + ); + assert!( + encode_edited_document( + &edited, + &[ + Edit { + field_id: FIELD_ID, + value: ConfigValue::U8(3) + }, + Edit { + field_id: FIELD_ID, + value: ConfigValue::U8(4) + } + ] + ) + .is_err() + ); + } + + #[derive(Clone)] + struct Candidate { + rate: u8, + role: u8, + } + + impl kaptan_config::ConfigModel for Candidate { + type Active = Self; + type Context = (); + const SCHEMA_ID: [u8; 16] = if OTHER_SCHEMA { + *b"ANOTHER-SCHEMA01" + } else { + *b"UNKNOWN-SCHEMA01" + }; + + fn descriptor(index: u16) -> Option<&'static kaptan_config::FieldDescriptor> { + const FIELDS: [kaptan_config::FieldDescriptor; 2] = [ + kaptan_config::FieldDescriptor { + id: FIELD_ID, + display_key: "telemetry.heartbeat_hz", + unit: "hz", + kind: ValueKind::U8, + minimum: Some(ConfigValue::U8(1)), + maximum: Some(ConfigValue::U8(100)), + enum_options: &[], + finite: false, + nonzero: false, + }, + kaptan_config::FieldDescriptor { + id: ROLE_ID, + display_key: "future.port.role", + unit: "", + kind: ValueKind::U8, + minimum: None, + maximum: None, + enum_options: &[(1, "Disabled"), (3, "Telemetry")], + finite: false, + nonzero: false, + }, + ]; + FIELDS.get(usize::from(index)) + } + + fn field_count() -> u16 { + 2 + } + + fn field_value(&self, id: FieldId) -> Option { + match id { + FIELD_ID => Some(ConfigValue::U8(self.rate)), + ROLE_ID => Some(ConfigValue::U8(self.role)), + _ => None, + } + } + + fn active_field_value(active: &Self, id: FieldId) -> Option { + active.field_value(id) + } + + fn set_field( + &mut self, + id: FieldId, + value: ConfigValue, + ) -> Result<(), kaptan_config::FieldWriteError> { + match (id, value) { + (FIELD_ID, ConfigValue::U8(value)) => self.rate = value, + (ROLE_ID, ConfigValue::U8(value)) => self.role = value, + _ => return Err(kaptan_config::FieldWriteError::UnknownField(id)), + } + Ok(()) + } + + fn validate(&self, _: &()) -> Result { + if self.rate == 99 && self.role == 3 { + return Err(kaptan_config::ConfigViolation::related( + FIELD_ID, + ROLE_ID, + kaptan_config::ValidationRule::Inconsistent, + )); + } + Ok(self.clone()) + } + } fn artifact(current: u8) -> Vec { - let key = b"telemetry.heartbeat_hz"; - let unit = b"hz"; - let mut bytes = Vec::new(); - bytes.extend_from_slice(b"KCFG"); - bytes.push(1); // KCFG version 1. - bytes.push(0); - bytes.extend_from_slice(b"UNKNOWN-SCHEMA01"); - bytes.extend_from_slice(&1_u16.to_le_bytes()); - bytes.extend_from_slice(&FIELD_ID.get().to_le_bytes()); - bytes.push(ValueKind::U8 as u8); - bytes.push(3); - bytes.push(key.len() as u8); - bytes.push(unit.len() as u8); - bytes.push(0); - bytes.extend_from_slice(&[current, 2, 1, 100]); - bytes.extend_from_slice(key); - bytes.extend_from_slice(unit); - let checksum = crc32c(&bytes); - bytes.extend_from_slice(&checksum.to_be_bytes()); + artifact_state::(current, 3, false) + } + + fn edit_rate(value: u8) -> Vec { + vec![Edit { + field_id: FIELD_ID, + value: ConfigValue::U8(value), + }] + } + + fn artifact_state(rate: u8, role: u8, restart: bool) -> Vec { + let current = Candidate:: { rate, role }; + let factory = Candidate:: { rate: 2, role: 1 }; + let exporter = kaptan_config::KcfgExporter::new(¤t, &factory, restart).unwrap(); + let mut bytes = vec![0; exporter.length()]; + assert_eq!(exporter.read(0, &mut bytes).unwrap(), bytes.len()); bytes } @@ -1150,16 +1480,41 @@ mod tests { fn discover(client: &mut ConfigClient, document: &[u8]) -> RemoteConfig { let open = client.discover(CONTROLLER).unwrap(); + finish_discovery(client, open, document, document) + } + + fn finish_discovery( + client: &mut ConfigClient, + open: MavlinkFrame, + saved: &[u8], + active: &[u8], + ) -> RemoteConfig { + let FileRequest::Control(control) = request(&open) else { + panic!("expected open") + }; + assert_eq!(control.content.as_bytes(), CONFIG_PATH); + let active_open = sent(finish_read(client, open, saved)); + let FileRequest::Control(control) = request(&active_open) else { + panic!("expected active open") + }; + assert_eq!(control.content.as_bytes(), ACTIVE_PATH); + let ClientEvent::Configuration(config) = event(finish_read(client, active_open, active)) + else { + panic!("closed exports must publish configuration"); + }; + config + } + + fn finish_read(client: &mut ConfigClient, open: MavlinkFrame, document: &[u8]) -> Outcome { let FileRequest::Control(open) = request(&open) else { panic!("open-read must be a control request"); }; assert_eq!(open.operation, FileOperation::OpenRead); - assert_eq!(open.content.as_bytes(), CONFIG_PATH); assert_ne!(open.transfer_id, 0); let size = document.len() as u32; let checksum = crc32c(document); - let read = sent(client.observe_frame(&control_response( + let mut next = sent(client.observe_frame(&control_response( FileOperation::OpenRead, open.transfer_id, 0, @@ -1167,40 +1522,39 @@ mod tests { checksum, FileStatus::Ok, ))); - let FileRequest::Control(read) = request(&read) else { - panic!("read must be a control request"); - }; - assert_eq!(read.operation, FileOperation::Read); - assert_eq!(read.position, 0); - - let mut data = [0_u8; CHUNK_BYTES]; - data[..document.len()].copy_from_slice(document); - let close = sent( - client.observe_frame(&response(FileResponse::Data(DataResponse { - target: Target::new(CLIENT.system(), CLIENT.component()), - position: 0, - transfer_id: open.transfer_id, - data, - }))), - ); - let FileRequest::Control(close) = request(&close) else { + for (index, chunk) in document.chunks(CHUNK_BYTES).enumerate() { + let position = (index * CHUNK_BYTES) as u32; + let FileRequest::Control(read) = request(&next) else { + panic!("expected read") + }; + assert_eq!(read.operation, FileOperation::Read); + assert_eq!(read.position, position); + assert_eq!(read.transfer_id, open.transfer_id); + let mut data = [0_u8; CHUNK_BYTES]; + data[..chunk.len()].copy_from_slice(chunk); + next = sent( + client.observe_frame(&response(FileResponse::Data(DataResponse { + target: Target::new(CLIENT.system(), CLIENT.component()), + position, + transfer_id: open.transfer_id, + data, + }))), + ); + } + let FileRequest::Control(close) = request(&next) else { panic!("close-read must be a control request"); }; assert_eq!(close.operation, FileOperation::Close); assert_eq!(close.position, size); - let event = event(client.observe_frame(&control_response( + client.observe_frame(&control_response( FileOperation::Close, open.transfer_id, size, size, checksum, FileStatus::Ok, - ))); - let ClientEvent::Configuration(config) = event else { - panic!("closed export must publish configuration"); - }; - config + )) } #[test] @@ -1354,16 +1708,26 @@ mod tests { } #[test] - fn one_file_export_discovers_an_unknown_schema_without_a_catalog() { + fn sequential_exports_discover_an_unknown_schema_without_a_catalog() { let mut client = ConfigClient::new(CLIENT); - let config = discover(&mut client, &artifact(20)); + let open = client.discover(CONTROLLER).unwrap(); + let config = finish_discovery( + &mut client, + open, + &artifact_state::(30, 1, true), + &artifact(20), + ); assert_eq!(config.schema_id, *b"UNKNOWN-SCHEMA01"); - assert!(!config.restart_required); - assert_eq!(config.fields.len(), 1); + assert!(config.restart_required); + assert!(config.committed_error.is_none()); + assert_eq!(config.fields.len(), 2); assert_eq!(config.fields[0].id, FIELD_ID); assert_eq!(config.fields[0].key, "telemetry.heartbeat_hz"); - assert_eq!(config.fields[0].value, ConfigValue::U8(20)); + assert_eq!(config.fields[0].value, ConfigValue::U8(30)); + assert_eq!(config.fields[0].active, Some(ConfigValue::U8(20))); + assert_eq!(config.fields[1].active, Some(ConfigValue::U8(3))); + assert_eq!(config.fields[1].value, ConfigValue::U8(1)); assert_eq!(config.fields[0].factory, ConfigValue::U8(2)); assert_eq!(config.fields[0].minimum, Some(ConfigValue::U8(1))); assert_eq!(config.fields[0].maximum, Some(ConfigValue::U8(100))); @@ -1474,9 +1838,13 @@ mod tests { }; let written = &data.data[..open.file_size as usize]; assert_eq!(crc32c(written), open.checksum_crc32c); - let edited = kaptan_config::decode_kcfg(written).unwrap(); + let edited = decode_artifact(written).unwrap(); assert!(edited.restart_required()); assert_eq!(edited.fields().next().unwrap().value(), ConfigValue::U8(30)); + assert_eq!( + edited.fields().next().unwrap().expected_base_value(), + ConfigValue::U8(20) + ); let close = sent(client.observe_frame(&control_response( FileOperation::WriteAck, @@ -1492,7 +1860,7 @@ mod tests { assert_eq!(close.operation, FileOperation::Close); assert_eq!(close.position, open.file_size); - let event = event(client.observe_frame(&control_response_with_content( + let refresh = sent(client.observe_frame(&control_response_with_content( FileOperation::Close, open.transfer_id, open.file_size, @@ -1501,11 +1869,27 @@ mod tests { FileStatus::Ok, Content::try_new(&KcfgResultDetail::success(true).encode()).unwrap(), ))); - let ClientEvent::Configuration(config) = event else { - panic!("completed import must publish configuration"); - }; + assert!( + client.downloaded.is_none(), + "uploaded expected-base must not become the new baseline" + ); + assert!(client.save(edit_rate(40)).is_err()); + let config = finish_discovery( + &mut client, + refresh, + &artifact_state::(30, 3, true), + &original, + ); assert!(config.restart_required); assert_eq!(config.fields[0].value, ConfigValue::U8(30)); + assert_eq!(config.fields[0].active, Some(ConfigValue::U8(20))); + + let open = client.save(edit_rate(40)).unwrap(); + let (_, body) = upload(&mut client, open); + let edited = decode_artifact(&body).unwrap(); + let field = edited.fields().next().unwrap(); + assert_eq!(field.value(), ConfigValue::U8(40)); + assert_eq!(field.expected_base_value(), ConfigValue::U8(30)); } #[test] @@ -1519,6 +1903,7 @@ mod tests { ) .unwrap(); let mut client = ConfigClient::new(CLIENT); + client.target = Some(CONTROLLER); client.transfer = Some(Transfer::Write(WriteTransfer { id: 9, position: document.len() as u32, @@ -1526,7 +1911,7 @@ mod tests { checksum: crc32c(&document), })); - let event = event(client.closed_write(ControlResponse { + let refresh = sent(client.closed_write(ControlResponse { operation: FileOperation::Close, target: Target::new(CLIENT.system(), CLIENT.component()), position: document.len() as u32, @@ -1537,12 +1922,10 @@ mod tests { content: Content::try_new(&KcfgResultDetail::success(false).encode()).unwrap(), })); - let ClientEvent::Configuration(config) = event else { - panic!("restored values must publish configuration"); - }; + let config = finish_discovery(&mut client, refresh, &artifact(2), &artifact(2)); assert!(!config.restart_required); assert!( - !kaptan_config::decode_kcfg(&client.downloaded.as_ref().unwrap().document) + !decode_artifact(&client.downloaded.as_ref().unwrap().document) .unwrap() .restart_required() ); @@ -1577,6 +1960,390 @@ mod tests { (client, document) } + pub(crate) fn pending_close() -> (ConfigClient, MavlinkFrame) { + let (client, _) = client_awaiting_close(); + let frame = client.pending.as_ref().unwrap().raw; + (client, frame) + } + + fn upload(client: &mut ConfigClient, frame: MavlinkFrame) -> (ControlRequest, Vec) { + let FileRequest::Control(open) = request(&frame) else { + panic!("expected write open") + }; + assert_eq!(open.operation, FileOperation::OpenWrite); + let mut next = sent(client.observe_frame(&control_response( + FileOperation::OpenWrite, + open.transfer_id, + 0, + open.file_size, + open.checksum_crc32c, + FileStatus::Ok, + ))); + let mut bytes = Vec::new(); + loop { + match request(&next) { + FileRequest::Data(data) => { + assert_eq!(data.position as usize, bytes.len()); + let count = (open.file_size as usize - bytes.len()).min(CHUNK_BYTES); + bytes.extend_from_slice(&data.data[..count]); + next = sent(client.observe_frame(&control_response( + FileOperation::WriteAck, + open.transfer_id, + bytes.len() as u32, + open.file_size, + open.checksum_crc32c, + FileStatus::Ok, + ))); + } + FileRequest::Control(close) => { + assert_eq!(close.operation, FileOperation::Close); + assert_eq!(close.position, open.file_size); + assert_eq!(crc32c(&bytes), open.checksum_crc32c); + return (close, bytes); + } + } + } + } + + fn discover_unknown(client: &mut ConfigClient) -> RemoteConfig { + let frame = client.discover(CONTROLLER).unwrap(); + let FileRequest::Control(open) = request(&frame) else { + panic!("expected open") + }; + let active = sent(client.observe_frame(&control_response( + FileOperation::OpenRead, + open.transfer_id, + 0, + 0, + 0, + FileStatus::StorageFailure, + ))); + let FileRequest::Control(open) = request(&active) else { + panic!("expected active open") + }; + assert_eq!(open.content.as_bytes(), ACTIVE_PATH); + let ClientEvent::Configuration(config) = event(finish_read(client, active, &artifact(20))) + else { + panic!("active recovery must publish read-only configuration"); + }; + config + } + + #[test] + fn unknown_committed_can_reset_every_field_from_active_factory_metadata() { + let mut client = ConfigClient::new(CLIENT); + let config = discover_unknown(&mut client); + assert!( + config + .committed_error + .as_ref() + .unwrap() + .contains("StorageFailure") + ); + assert_eq!(config.fields[0].active, Some(ConfigValue::U8(20))); + assert!(client.save(edit_rate(30)).is_err()); + + let open = client.reset_saved().unwrap(); + let FileRequest::Control(control) = request(&open) else { + panic!("expected reset open") + }; + assert_eq!(control.content.as_bytes(), RESET_PATH); + let (close, bytes) = upload(&mut client, open); + let decoded = decode_artifact(&bytes).unwrap(); + assert_eq!(decoded.field_count(), 2); + for field in decoded.fields() { + assert_eq!(field.value(), field.factory_value()); + } + let refresh = sent(client.observe_frame(&control_response_with_content( + FileOperation::Close, + close.transfer_id, + close.position, + bytes.len() as u32, + crc32c(&bytes), + FileStatus::Ok, + Content::try_new(&KcfgResultDetail::success(true).encode()).unwrap(), + ))); + let config = finish_discovery( + &mut client, + refresh, + &artifact_state::(2, 1, true), + &artifact(20), + ); + assert!(config.committed_error.is_none()); + assert!(config.restart_required); + assert_eq!(config.fields[0].value, ConfigValue::U8(2)); + assert_eq!(config.fields[0].active, Some(ConfigValue::U8(20))); + assert_eq!(config.fields[1].value, ConfigValue::U8(1)); + assert_eq!(config.fields[1].active, Some(ConfigValue::U8(3))); + } + + #[test] + fn undecodable_committed_uses_active_for_recovery_instead_of_claiming_ready() { + let mut client = ConfigClient::new(CLIENT); + let open = client.discover(CONTROLLER).unwrap(); + // Valid artifact and metadata, but its saved scalar violates local bounds. + let config = finish_discovery(&mut client, open, &artifact(200), &artifact(20)); + assert!(config.committed_error.as_ref().unwrap().contains("bounds")); + assert!(client.downloaded.is_none()); + assert!(client.reset_saved().is_ok()); + } + + #[test] + fn readable_relationally_invalid_committed_remains_editable_for_target_validation() { + use kaptan_config::ConfigModel; + assert!( + Candidate:: { rate: 99, role: 3 } + .validate(&()) + .is_err() + ); + let mut client = ConfigClient::new(CLIENT); + let open = client.discover(CONTROLLER).unwrap(); + let config = finish_discovery(&mut client, open, &artifact(99), &artifact(20)); + assert!(config.committed_error.is_none()); + let open = client.save(edit_rate(30)).unwrap(); + let (_, bytes) = upload(&mut client, open); + let mut importer = + kaptan_config::KcfgImporter::new(&Candidate:: { rate: 2, role: 1 }).unwrap(); + importer + .push(&Candidate:: { rate: 2, role: 1 }, &bytes) + .unwrap(); + let update = importer.finish().unwrap(); + assert_eq!(update.expected_base.rate, 99); + assert!(update.candidate.validate(&()).is_ok()); + } + + #[test] + fn mismatched_active_schema_or_metadata_blocks_save_and_reset() { + for active in [artifact_state::(20, 3, false), { + // Same schema ID but incompatible factory metadata. + let current = Candidate:: { rate: 20, role: 3 }; + let factory = Candidate:: { rate: 3, role: 1 }; + let exporter = kaptan_config::KcfgExporter::new(¤t, &factory, false).unwrap(); + let mut bytes = vec![0; exporter.length()]; + exporter.read(0, &mut bytes).unwrap(); + bytes + }] { + let mut client = ConfigClient::new(CLIENT); + let open = client.discover(CONTROLLER).unwrap(); + let next = sent(finish_read(&mut client, open, &artifact(20))); + assert!(matches!( + event(finish_read(&mut client, next, &active)), + ClientEvent::RediscoveryRequired(_) + )); + assert!(client.reset_saved().is_err()); + assert!(client.save(edit_rate(30)).is_err()); + } + } + + #[test] + fn unavailable_active_after_unknown_committed_never_enables_recovery_actions() { + let mut client = ConfigClient::new(CLIENT); + let frame = client.discover(CONTROLLER).unwrap(); + let FileRequest::Control(open) = request(&frame) else { + panic!("expected saved open") + }; + let active = sent(client.observe_frame(&control_response( + FileOperation::OpenRead, + open.transfer_id, + 0, + 0, + 0, + FileStatus::StorageFailure, + ))); + let FileRequest::Control(open) = request(&active) else { + panic!("expected active open") + }; + assert_eq!(open.content.as_bytes(), ACTIVE_PATH); + let result = client.observe_frame(&control_response( + FileOperation::OpenRead, + open.transfer_id, + 0, + 0, + 0, + FileStatus::StorageFailure, + )); + assert!(matches!(event(result), ClientEvent::RediscoveryRequired(_))); + assert!(client.save(edit_rate(30)).is_err()); + assert!(client.reset_saved().is_err()); + } + + #[test] + fn failed_active_readback_after_commit_does_not_reenable_the_old_draft() { + let mut client = ConfigClient::new(CLIENT); + let state = crate::state::SharedState::default(); + state.connected("test".into()); + state.configuration_event(ClientEvent::Configuration(discover( + &mut client, + &artifact(20), + ))); + state.edit(FIELD_ID.get(), &serde_json::json!(30)).unwrap(); + let open = client.save(state.begin_save().unwrap()).unwrap(); + let (close, bytes) = upload(&mut client, open); + let saved = sent(client.observe_frame(&control_response_with_content( + FileOperation::Close, + close.transfer_id, + close.position, + bytes.len() as u32, + crc32c(&bytes), + FileStatus::Ok, + Content::try_new(&KcfgResultDetail::success(true).encode()).unwrap(), + ))); + let active = sent(finish_read(&mut client, saved, &artifact(30))); + // The file transport succeeds, but the active artifact is unusable. + let result = finish_read(&mut client, active, b"not a KCFG artifact"); + state.configuration_event(event(result)); + assert!(!state.snapshot().can_edit); + assert!(!state.snapshot().can_save); + assert!(!state.snapshot().can_reset); + assert!(state.begin_save().is_err()); + assert!(client.save(edit_rate(40)).is_err()); + } + + #[test] + fn write_storage_failure_keeps_active_for_reset_and_blocks_normal_save() { + for at_close in [false, true] { + let mut client = ConfigClient::new(CLIENT); + discover(&mut client, &artifact(20)); + let frame = client.save(edit_rate(30)).unwrap(); + let (operation, id, position, size, checksum) = if at_close { + let (close, bytes) = upload(&mut client, frame); + ( + FileOperation::Close, + close.transfer_id, + close.position, + bytes.len() as u32, + crc32c(&bytes), + ) + } else { + let FileRequest::Control(open) = request(&frame) else { + panic!("expected open") + }; + (FileOperation::OpenWrite, open.transfer_id, 0, 0, 0) + }; + let result = client.observe_frame(&control_response( + operation, + id, + position, + size, + checksum, + FileStatus::StorageFailure, + )); + assert!(matches!(event(result), ClientEvent::DurableStateUnknown(_))); + assert!(client.save(edit_rate(40)).is_err()); + let frame = client.reset_saved().unwrap(); + let FileRequest::Control(open) = request(&frame) else { + panic!("expected reset open") + }; + assert_eq!(open.content.as_bytes(), RESET_PATH); + } + } + + #[test] + fn target_validation_rejection_keeps_the_baseline_for_a_corrected_save() { + let mut client = ConfigClient::new(CLIENT); + discover(&mut client, &artifact(20)); + let frame = client.save(edit_rate(99)).unwrap(); + let (close, bytes) = upload(&mut client, frame); + let violation = kaptan_config::ConfigViolation::related( + FIELD_ID, + ROLE_ID, + kaptan_config::ValidationRule::Inconsistent, + ); + let result = client.observe_frame(&control_response_with_content( + FileOperation::Close, + close.transfer_id, + close.position, + bytes.len() as u32, + crc32c(&bytes), + FileStatus::ContentRejected, + Content::try_new(&KcfgResultDetail::violation(violation).encode()).unwrap(), + )); + let ClientEvent::Failure(detail) = event(result) else { + panic!("validation failure must not report save success") + }; + assert!(detail.contains("field 401")); + assert!(detail.contains("related field 402")); + let frame = client.save(edit_rate(30)).unwrap(); + let (_, bytes) = upload(&mut client, frame); + let artifact = decode_artifact(&bytes).unwrap(); + let field = artifact.fields().next().unwrap(); + assert_eq!(field.expected_base_value(), ConfigValue::U8(20)); + assert_eq!(field.value(), ConfigValue::U8(30)); + } + + #[test] + fn close_conflict_discards_obsolete_baseline_and_requires_a_new_discovery() { + let mut client = ConfigClient::new(CLIENT); + discover(&mut client, &artifact(20)); + let open = client.save(edit_rate(30)).unwrap(); + let (close, bytes) = upload(&mut client, open); + let ClientEvent::RediscoveryRequired(detail) = + event(client.observe_frame(&control_response( + FileOperation::Close, + close.transfer_id, + close.position, + bytes.len() as u32, + crc32c(&bytes), + FileStatus::RequestConflict, + ))) + else { + panic!("obsolete baseline must require rediscovery") + }; + assert!(detail.contains("stale draft discarded")); + assert!(client.downloaded.is_none()); + assert!(client.save(edit_rate(40)).is_err()); + let open = client.discover(CONTROLLER).unwrap(); + let config = finish_discovery(&mut client, open, &artifact(35), &artifact(20)); + assert_eq!(config.fields[0].value, ConfigValue::U8(35)); + assert_eq!(config.fields[0].active, Some(ConfigValue::U8(20))); + } + + #[test] + fn interruption_during_close_survives_disconnect_and_reconnect_until_readback() { + let mut client = ConfigClient::new(CLIENT); + let state = crate::state::SharedState::default(); + state.connected("test".into()); + state.configuration_event(ClientEvent::Configuration(discover( + &mut client, + &artifact(20), + ))); + state.edit(FIELD_ID.get(), &serde_json::json!(30)).unwrap(); + let open = client.save(state.begin_save().unwrap()).unwrap(); + assert!(client.interruption().is_none(), "open-write cannot commit"); + upload(&mut client, open); + state.configuration_event( + client + .interruption() + .expect("close is durability-sensitive"), + ); + assert!(!state.snapshot().can_save); + assert!(state.snapshot().can_reset); + state.disconnected(Some("serial link failed".into())); + assert!(state.snapshot().durable_state_unknown); + assert!(state.snapshot().error.unwrap().contains("unknown")); + state.begin_connect().unwrap(); + state.connected("test".into()); + state.discovering(); + assert!(state.snapshot().durable_state_unknown); + assert!(state.snapshot().error.unwrap().contains("unknown")); + let open = client.discover(CONTROLLER).unwrap(); + state.configuration_event(ClientEvent::Configuration(finish_discovery( + &mut client, + open, + &artifact_state::(30, 3, true), + &artifact(20), + ))); + assert!(!state.snapshot().durable_state_unknown); + assert_eq!( + state.configuration().fields[0].saved, + Some(serde_json::json!(30)) + ); + assert_eq!( + state.configuration().fields[0].active, + Some(serde_json::json!(20)) + ); + } + #[test] fn malformed_success_and_storage_uncertainty_never_publish_configuration() { let (mut malformed, document) = client_awaiting_close(); diff --git a/firmware/tools/configurator/src/config_client_interop.rs b/firmware/tools/configurator/src/config_client_interop.rs new file mode 100644 index 0000000..9698721 --- /dev/null +++ b/firmware/tools/configurator/src/config_client_interop.rs @@ -0,0 +1,648 @@ +//! Host client interoperability with the real file service and configuration lifecycle. + +use std::{cell::RefCell, rc::Rc}; + +use kaptan_config::{ + Config, ConfigEnum, ConfigManager, ConfigModel, ConfigStore, ConfigViolation, FieldDescriptor, + FieldWriteError, KcfgExporter, KcfgImporter, ReplaceError, +}; +use kaptan_file_mavlink::{ + FileFailure, FileMetadata, FilePath, FileProvider, FileService, IngressId, TransferPeer, + wire::{decode_request, encode_response}, +}; + +use super::*; + +const CONTROLLER: Target = Target::new(42, 7); +const CLIENT: Principal = Principal::new(255, 190); +const PRIMARY_ROLE: FieldId = FieldId::new(11); +const PRIMARY_BAUD: FieldId = FieldId::new(12); + +#[derive(Clone, Copy, Debug, PartialEq, ConfigEnum)] +#[repr(u8)] +enum Role { + Disabled = 0, + Mavlink = 1, + Sensor = 2, +} + +// Generated metadata makes this a multi-chunk artifact without padding or a +// dependency on a particular board or its port catalog. +#[derive(Clone, Debug, PartialEq, Config)] +struct Candidate { + #[config(enum, id = 11, key = "ports.primary.role", unit = "")] + primary_role: Role, + #[config( + id = 12, + key = "ports.primary.baud", + unit = "baud", + range(min = 9600, max = 921600) + )] + primary_baud: u32, + #[config(enum, id = 21, key = "ports.secondary.role", unit = "")] + secondary_role: Role, + #[config( + id = 22, + key = "ports.secondary.baud", + unit = "baud", + range(min = 9600, max = 921600) + )] + secondary_baud: u32, + #[config(enum, id = 31, key = "ports.auxiliary.role", unit = "")] + auxiliary_role: Role, + #[config( + id = 32, + key = "ports.auxiliary.baud", + unit = "baud", + range(min = 9600, max = 921600) + )] + auxiliary_baud: u32, + #[config(id = 41, key = "scalar.free", unit = "")] + free: f32, + #[config(id = 42, key = "scalar.nonzero", unit = "", nonzero)] + nonzero: f64, + #[config(id = 43, key = "scalar.finite", unit = "", finite)] + finite: f64, +} + +impl Candidate { + fn factory() -> Self { + Self { + primary_role: Role::Disabled, + primary_baud: 115_200, + secondary_role: Role::Disabled, + secondary_baud: 115_200, + auxiliary_role: Role::Disabled, + auxiliary_baud: 115_200, + free: 0.0, + nonzero: 1.0, + finite: 2.0, + } + } + + fn boot() -> Self { + Self { + primary_role: Role::Mavlink, + secondary_role: Role::Sensor, + auxiliary_baud: 57_600, + ..Self::factory() + } + } +} + +impl ConfigModel for Candidate { + type Active = Self; + type Context = (); + const SCHEMA_ID: [u8; 16] = ::SCHEMA_ID; + + fn descriptor(index: u16) -> Option<&'static FieldDescriptor> { + ::descriptor(usize::from(index)) + } + + fn field_count() -> u16 { + Self::FIELD_COUNT.try_into().unwrap() + } + + fn field_value(&self, id: FieldId) -> Option { + ::field_value(self, id) + } + + fn active_field_value(active: &Self, id: FieldId) -> Option { + ::field_value(active, id) + } + + fn set_field(&mut self, id: FieldId, value: ConfigValue) -> Result<(), FieldWriteError> { + ::set_field(self, id, value) + } + + fn validate(&self, _: &()) -> Result { + ::validate(self)?; + Ok(self.clone()) + } +} + +struct Disk { + saved: Option, + persist_calls: usize, + fail_next: bool, +} + +struct MemoryStore(Rc>); + +impl ConfigStore for MemoryStore { + type Error = (); + + fn load(&mut self, _: &Candidate) -> Result, ()> { + Ok(self.0.borrow().saved.clone()) + } + + fn persist(&mut self, candidate: &Candidate) -> Result<(), ()> { + let mut disk = self.0.borrow_mut(); + disk.persist_calls += 1; + disk.saved = Some(candidate.clone()); + // A store error can occur after durability. The manager must invalidate + // its committed view instead of assuming rollback. + if std::mem::take(&mut disk.fail_next) { + Err(()) + } else { + Ok(()) + } + } +} + +enum OpenFile { + Read(Vec), + Write { + importer: KcfgImporter, + reset: bool, + }, +} + +struct ConfigFiles { + manager: ConfigManager, + open: Option, +} + +fn failure(status: FileStatus) -> FileFailure { + FileFailure::new(status, Content::EMPTY).unwrap() +} + +impl FileProvider for ConfigFiles { + fn open_read(&mut self, path: &FilePath) -> Result { + let (current, restart_required) = match path.as_bytes() { + CONFIG_PATH => ( + self.manager + .committed() + .ok_or_else(|| failure(FileStatus::StorageFailure))?, + self.manager.restart_required(), + ), + ACTIVE_PATH => (self.manager.active(), false), + _ => return Err(failure(FileStatus::NotFound)), + }; + let exporter = + KcfgExporter::new(current, self.manager.factory(), restart_required).unwrap(); + let mut bytes = vec![0; exporter.length()]; + assert_eq!(exporter.read(0, &mut bytes).unwrap(), bytes.len()); + let metadata = FileMetadata { + size: bytes.len().try_into().unwrap(), + checksum_crc32c: exporter.checksum_crc32c(), + }; + self.open = Some(OpenFile::Read(bytes)); + Ok(metadata) + } + + fn open_write(&mut self, path: &FilePath, _: FileMetadata) -> Result<(), FileFailure> { + let reset = match path.as_bytes() { + CONFIG_PATH => false, + RESET_PATH => true, + _ => return Err(failure(FileStatus::NotFound)), + }; + if !reset && self.manager.committed().is_none() { + return Err(failure(FileStatus::StorageFailure)); + } + self.open = Some(OpenFile::Write { + importer: KcfgImporter::new(self.manager.factory()).unwrap(), + reset, + }); + Ok(()) + } + + fn read(&mut self, position: u32, output: &mut [u8]) -> Result<(), FileFailure> { + let Some(OpenFile::Read(bytes)) = &self.open else { + return Err(failure(FileStatus::InvalidState)); + }; + let start = usize::try_from(position).unwrap(); + output.copy_from_slice(&bytes[start..start + output.len()]); + Ok(()) + } + + fn write(&mut self, _: u32, data: &[u8]) -> Result<(), FileFailure> { + let Some(OpenFile::Write { importer, .. }) = &mut self.open else { + return Err(failure(FileStatus::InvalidState)); + }; + importer + .push(self.manager.factory(), data) + .map_err(|_| failure(FileStatus::ContentRejected)) + } + + fn close(&mut self) -> Result { + let open = self + .open + .take() + .ok_or_else(|| failure(FileStatus::InvalidState))?; + let OpenFile::Write { importer, reset } = open else { + return Ok(Content::EMPTY); + }; + let update = importer + .finish() + .map_err(|_| failure(FileStatus::ContentRejected))?; + let result = if reset { + if !self.manager.is_factory(&update.candidate) { + return Err(failure(FileStatus::ContentRejected)); + } + self.manager.reset_to_factory() + } else { + self.manager + .replace_if_current(update.candidate, &update.expected_base) + }; + let outcome = result.map_err(|error| { + failure(match error { + ReplaceError::Conflict => FileStatus::RequestConflict, + ReplaceError::Store(()) | ReplaceError::DurableStateUnknown => { + FileStatus::StorageFailure + } + ReplaceError::InvalidCandidate(_) => FileStatus::ContentRejected, + }) + })?; + Ok( + Content::try_new(&KcfgResultDetail::success(outcome.restart_required).encode()) + .unwrap(), + ) + } + + fn abort(&mut self) { + self.open = None; + } +} + +struct Link { + service: FileService, + provider: ConfigFiles, + disk: Rc>, + encoder: MavlinkEncoder, + exchanges: Vec<(FileRequest, FileResponse)>, +} + +impl Link { + fn new() -> Self { + let disk = Rc::new(RefCell::new(Disk { + saved: Some(Candidate::boot()), + persist_calls: 0, + fail_next: false, + })); + let manager = + ConfigManager::recover(Candidate::factory(), (), MemoryStore(Rc::clone(&disk))) + .unwrap(); + Self { + service: FileService::new(CONTROLLER), + provider: ConfigFiles { + manager, + open: None, + }, + disk, + encoder: MavlinkEncoder::new(CONTROLLER.system(), CONTROLLER.component()), + exchanges: Vec::new(), + } + } + + fn exchange(&mut self, frame: MavlinkFrame) -> MavlinkFrame { + let request = decode_request(&parse_test_message(&frame).unwrap()).unwrap(); + let peer = TransferPeer { + principal: Principal::new(frame.system_id(), frame.component_id()), + ingress: IngressId(0), + }; + let result = self + .service + .dispatch(&mut self.provider, peer, request) + .expect("client must address the service endpoint"); + self.exchanges.push((request, result.response)); + match encode_response(result.response) { + MavMessage::FILE_CONTROL(data) => self.encoder.encode(&data), + MavMessage::FILE_DATA(data) => self.encoder.encode(&data), + _ => unreachable!(), + } + } + + fn run( + &mut self, + client: &mut ConfigClient, + mut request: MavlinkFrame, + drop_close_ack: bool, + ) -> ClientEvent { + let mut dropped = false; + // One write and its two readbacks each need at most chunks + 2 + // exchanges. The extra exchange permits one dropped-close retry. + let chunks = kaptan_config::kcfg_length::() + .unwrap() + .div_ceil(DATA_BYTES); + for _ in 0..3 * (chunks + 2) { + let mut reply = self.exchange(request); + if drop_close_ack + && !dropped + && matches!( + client.pending.as_ref().map(|pending| pending.kind), + Some(PendingKind::CloseWrite { .. }) + ) + { + dropped = true; + client.pending.as_mut().unwrap().sent_at = Instant::now() - CLOSE_WRITE_TIMEOUT; + let Outcome::Send(retry) = client.retry_due() else { + panic!("lost close acknowledgement must retry") + }; + assert_eq!(retry.raw_bytes(), request.raw_bytes()); + reply = self.exchange(retry); + } + match client.observe_frame(&reply) { + Outcome::Send(next) => request = next, + Outcome::Event(event) => { + assert_eq!(dropped, drop_close_ack); + return event; + } + Outcome::Idle => panic!("a real service reply must advance the client"), + } + } + panic!("configuration exchange exceeded its artifact-derived bound") + } + + fn discover(&mut self, client: &mut ConfigClient) -> RemoteConfig { + let request = client.discover(CONTROLLER).unwrap(); + configuration(self.run(client, request, false)) + } + + fn expire_lease(&mut self) { + // Switching editors requires expiry of the real service's replay lease. + assert!(!self.service.expire(&mut self.provider)); + assert!(self.provider.open.is_none()); + } +} + +fn configuration(event: ClientEvent) -> RemoteConfig { + match event { + ClientEvent::Configuration(config) => config, + ClientEvent::Failure(detail) + | ClientEvent::RediscoveryRequired(detail) + | ClientEvent::DurableStateUnknown(detail) => panic!("configuration failed: {detail}"), + } +} + +fn baud_edit(value: u32) -> Vec { + vec![Edit { + field_id: PRIMARY_BAUD, + value: ConfigValue::U32(value), + }] +} + +fn field(config: &RemoteConfig, id: FieldId) -> &RemoteField { + config.field(id).unwrap() +} + +#[test] +fn scalar_bits_and_rules_interoperate_through_browser_drafts_and_the_real_file_service() { + let mut link = Link::new(); + let mut client = ConfigClient::new(CLIENT); + let state = crate::state::SharedState::default(); + state.connected("test".into()); + let initial = link.discover(&mut client); + assert!(field(&initial, FieldId::new(42)).nonzero); + assert!(field(&initial, FieldId::new(43)).finite); + state.configuration_event(ClientEvent::Configuration(initial)); + state.edit(41, &serde_json::json!("-0")).unwrap(); + state + .edit(42, &serde_json::json!("bits:0xfff0000000000042")) + .unwrap(); + assert!(state.edit(43, &serde_json::json!("inf")).is_err()); + state.edit(43, &serde_json::json!("2")).unwrap(); + let request = client.save(state.begin_save().unwrap()).unwrap(); + let saved = configuration(link.run(&mut client, request, true)); + let nan = ConfigValue::F64(f64::from_bits(0xfff0_0000_0000_0042)); + assert_eq!( + field(&saved, FieldId::new(41)).value, + ConfigValue::F32(-0.0) + ); + assert_eq!(field(&saved, FieldId::new(42)).value, nan); + assert_eq!(link.disk.borrow().persist_calls, 1); + assert!(saved.restart_required); + state.configuration_event(ClientEvent::Configuration(saved)); + state + .edit(42, &serde_json::json!("bits:0xfff0000000000042")) + .unwrap(); + assert!(!state.snapshot().can_save); + let request = client + .save(vec![Edit { + field_id: FieldId::new(42), + value: nan, + }]) + .unwrap(); + configuration(link.run(&mut client, request, false)); + assert_eq!( + link.disk.borrow().persist_calls, + 1, + "bit-identical NaN is a no-op" + ); + let request = client.save(baud_edit(230_400)).unwrap(); + configuration(link.run(&mut client, request, false)); + assert_eq!( + link.disk.borrow().persist_calls, + 2, + "NaN expected base matches exact bits" + ); +} + +#[test] +fn multichunk_commit_replays_lost_close_once_and_refreshes_the_next_save_base() { + let mut link = Link::new(); + let mut client = ConfigClient::new(CLIENT); + let initial = link.discover(&mut client); + assert!(!initial.restart_required); + assert!(initial.committed_error.is_none()); + assert_eq!(initial.fields.len(), Candidate::FIELD_COUNT); + assert_eq!(field(&initial, PRIMARY_ROLE).options[1].label, "Mavlink"); + assert_eq!( + field(&initial, PRIMARY_ROLE).active, + Some(ConfigValue::U8(1)) + ); + let paths: Vec<_> = link + .exchanges + .iter() + .filter_map(|(request, _)| match request { + FileRequest::Control(control) if control.operation == FileOperation::OpenRead => { + Some(control.content.as_bytes()) + } + _ => None, + }) + .collect(); + assert_eq!( + paths, + [b"/config/user.kcfg".as_slice(), b"/config/active.kcfg"] + ); + let bytes = client.downloaded.as_ref().unwrap().document.len(); + assert!( + bytes > DATA_BYTES, + "the complete candidate must span chunks" + ); + let second_read_chunks = link.exchanges.iter().filter(|(_, response)| { + matches!(response, FileResponse::Data(data) if data.position == DATA_BYTES as u32) + }).count(); + assert_eq!( + second_read_chunks, 2, + "both snapshots must cross the chunk boundary" + ); + + let start = link.exchanges.len(); + let mut edits = baud_edit(230_400); + edits.push(Edit { + field_id: PRIMARY_ROLE, + value: ConfigValue::U8(0), + }); + let request = client.save(edits).unwrap(); + let saved = configuration(link.run(&mut client, request, true)); + assert_eq!(link.disk.borrow().persist_calls, 1); + let mut expected = Candidate::boot(); + expected.primary_role = Role::Disabled; + expected.primary_baud = 230_400; + assert_eq!(link.provider.manager.committed(), Some(&expected)); + assert_eq!(link.provider.manager.active(), &Candidate::boot()); + assert!(saved.restart_required); + assert_eq!(field(&saved, PRIMARY_ROLE).value, ConfigValue::U8(0)); + assert_eq!(field(&saved, PRIMARY_ROLE).active, Some(ConfigValue::U8(1))); + assert_eq!(field(&saved, PRIMARY_BAUD).value, ConfigValue::U32(230_400)); + assert_eq!( + field(&saved, PRIMARY_BAUD).active, + Some(ConfigValue::U32(115_200)) + ); + assert!(link.exchanges[start..].iter().any(|(request, _)| { + matches!(request, FileRequest::Data(data) if data.position == DATA_BYTES as u32) + })); + let replay = link.exchanges[start..].windows(2).find(|pair| { + pair[0].0 == pair[1].0 + && matches!(pair[0].0, FileRequest::Control(control) if control.operation == FileOperation::Close) + }).expect("close must traverse the real service twice"); + assert_eq!(replay[0].1, replay[1].1); + let baseline = super::decode_artifact(&client.downloaded.as_ref().unwrap().document).unwrap(); + assert_eq!( + baseline + .fields() + .find(|field| field.id() == PRIMARY_BAUD) + .unwrap() + .expected_base_value(), + ConfigValue::U32(230_400) + ); + + let request = client.save(baud_edit(460_800)).unwrap(); + let second = configuration(link.run(&mut client, request, false)); + assert_eq!(link.disk.borrow().persist_calls, 2); + expected.primary_baud = 460_800; + assert_eq!(link.disk.borrow().saved.as_ref(), Some(&expected)); + assert_eq!(link.provider.manager.committed(), Some(&expected)); + assert_eq!(link.provider.manager.active(), &Candidate::boot()); + assert_eq!( + field(&second, PRIMARY_BAUD).value, + ConfigValue::U32(460_800) + ); +} + +#[test] +fn another_editors_commit_rejects_stale_close_without_persisting_again() { + let mut link = Link::new(); + let mut first = ConfigClient::new(CLIENT); + let mut stale = ConfigClient::new(Principal::new(254, 190)); + link.discover(&mut first); + link.expire_lease(); + link.discover(&mut stale); + link.expire_lease(); + let request = first.save(baud_edit(230_400)).unwrap(); + configuration(link.run(&mut first, request, false)); + assert_eq!(link.disk.borrow().persist_calls, 1); + link.expire_lease(); + + let request = stale.save(baud_edit(460_800)).unwrap(); + let ClientEvent::RediscoveryRequired(detail) = link.run(&mut stale, request, false) else { + panic!("obsolete expected base must fail at close") + }; + assert!(detail.contains("RequestConflict")); + assert!(detail.contains("stale draft discarded")); + assert!(matches!(link.exchanges.last().unwrap().1, + FileResponse::Control(control) if control.operation == FileOperation::Close + && control.status == FileStatus::RequestConflict)); + assert_eq!(link.disk.borrow().persist_calls, 1); + assert_eq!( + link.provider.manager.committed().unwrap().primary_baud, + 230_400 + ); + assert_eq!(link.provider.manager.active(), &Candidate::boot()); + assert!(link.provider.open.is_none()); + assert!(stale.save(baud_edit(57_600)).is_err()); + let refreshed = link.discover(&mut stale); + assert_eq!( + field(&refreshed, PRIMARY_BAUD).value, + ConfigValue::U32(230_400) + ); +} + +#[test] +fn storage_failure_exports_active_and_factory_reset_recovers_unknown_committed() { + let mut link = Link::new(); + let mut client = ConfigClient::new(CLIENT); + link.discover(&mut client); + link.disk.borrow_mut().fail_next = true; + let request = client.save(baud_edit(230_400)).unwrap(); + assert!(matches!( + link.run(&mut client, request, false), + ClientEvent::DurableStateUnknown(_) + )); + assert_eq!(link.disk.borrow().persist_calls, 1); + assert!(link.provider.manager.committed().is_none()); + assert_eq!( + link.disk.borrow().saved.as_ref().unwrap().primary_baud, + 230_400 + ); + + let start = link.exchanges.len(); + let unknown = link.discover(&mut client); + assert!( + unknown + .committed_error + .as_ref() + .unwrap() + .contains("StorageFailure") + ); + assert!(matches!(link.exchanges[start].1, + FileResponse::Control(control) if control.operation == FileOperation::OpenRead + && control.status == FileStatus::StorageFailure)); + assert_eq!( + field(&unknown, PRIMARY_ROLE).active, + Some(ConfigValue::U8(1)) + ); + assert_eq!( + field(&unknown, PRIMARY_BAUD).active, + Some(ConfigValue::U32(115_200)) + ); + assert!(client.downloaded.is_none()); + assert!(client.save(baud_edit(460_800)).is_err()); + + let request = client.reset_saved().unwrap(); + let start = link.exchanges.len(); + let recovered = configuration(link.run(&mut client, request, true)); + assert!(matches!(&link.exchanges[start].0, + FileRequest::Control(control) if control.operation == FileOperation::OpenWrite + && control.content.as_bytes() == b"/config/reset.kcfg")); + assert_eq!( + link.disk.borrow().persist_calls, + 2, + "reset retry must not persist twice" + ); + assert_eq!( + link.disk.borrow().saved.as_ref(), + Some(&Candidate::factory()) + ); + assert_eq!( + link.provider.manager.committed(), + Some(&Candidate::factory()) + ); + assert_eq!(link.provider.manager.active(), &Candidate::boot()); + assert!(recovered.committed_error.is_none()); + assert!(recovered.restart_required); + assert!( + recovered + .fields + .iter() + .all(|field| field.value == field.factory) + ); + assert_eq!( + field(&recovered, PRIMARY_ROLE).active, + Some(ConfigValue::U8(1)) + ); + let request = client.save(baud_edit(57_600)).unwrap(); + configuration(link.run(&mut client, request, false)); + assert_eq!(link.disk.borrow().persist_calls, 3); +} diff --git a/firmware/tools/configurator/src/lib.rs b/firmware/tools/configurator/src/lib.rs index aaa4ea4..035e8dc 100644 --- a/firmware/tools/configurator/src/lib.rs +++ b/firmware/tools/configurator/src/lib.rs @@ -9,4 +9,4 @@ mod state; pub use api::serve_connection; pub use serial_worker::{SerialBaud, SerialController}; -pub use state::SharedState; +pub use state::{SharedState, ViewContext}; diff --git a/firmware/tools/configurator/src/main.rs b/firmware/tools/configurator/src/main.rs index cce8236..a6fcbea 100644 --- a/firmware/tools/configurator/src/main.rs +++ b/firmware/tools/configurator/src/main.rs @@ -19,7 +19,9 @@ fn main() -> std::io::Result<()> { ) .map_err(std::io::Error::other)?; if let Some(path) = arguments.serial_path { - serial.connect(path).map_err(std::io::Error::other)?; + serial + .connect(path, state.snapshot().context) + .map_err(std::io::Error::other)?; } let listener = TcpListener::bind(LISTEN_ADDRESS)?; let address = listener.local_addr()?; diff --git a/firmware/tools/configurator/src/serial_io_tests.rs b/firmware/tools/configurator/src/serial_io_tests.rs new file mode 100644 index 0000000..76bbe2d --- /dev/null +++ b/firmware/tools/configurator/src/serial_io_tests.rs @@ -0,0 +1,199 @@ +use super::*; +use crate::{ + config_client::{ClientEvent, tests::pending_close}, + state::tests::unknown_board, +}; +use std::{io, os::fd::FromRawFd, sync::mpsc}; + +fn pty() -> (File, File, String) { + let (mut master, mut slave) = (-1, -1); + // SAFETY: valid out-pointers receive two owned fds. Optional settings are null. + let opened = unsafe { + libc::openpty( + &mut master, + &mut slave, + std::ptr::null_mut(), + std::ptr::null(), + std::ptr::null(), + ) + }; + assert_eq!(opened, 0); + // SAFETY: openpty returned fresh fds whose ownership is transferred once. + let master = unsafe { File::from_raw_fd(master) }; + // SAFETY: the second fresh fd is distinct and is transferred once. + let slave = unsafe { File::from_raw_fd(slave) }; + // SAFETY: this live fd accepts the integer flags passed to fcntl. + let configured = unsafe { libc::fcntl(master.as_raw_fd(), libc::F_SETFL, libc::O_NONBLOCK) }; + assert_eq!(configured, 0); + let path = format!("/proc/self/fd/{}", slave.as_raw_fd()); + (master, slave, path) +} + +fn fill_transmit_queue(file: &mut File) { + for _ in 0..4096 { + match file.write(&[0; 4096]) { + Ok(count) => assert_ne!(count, 0), + Err(error) if error.kind() == io::ErrorKind::WouldBlock => return, + Err(error) => panic!("cannot fill PTY: {error}"), + } + } + panic!("PTY transmit queue did not reach its bound") +} + +fn stall_frame(session: &mut Session, frame: MavlinkFrame) { + // PTY line-discipline work can free a little space after the first EAGAIN. + // Keep filling the unread peer until an actual frame write is backpressured. + for _ in 0..64 { + fill_transmit_queue(session.link.as_mut().unwrap()); + write_raw(session, frame).unwrap(); + if !flush_pending(session).unwrap() { + return; + } + } + panic!("PTY did not backpressure a configuration frame") +} + +#[test] +fn queued_save_and_reset_recheck_connection_and_configuration_before_effects() { + for reconnect in [false, true] { + for action in [Action::Save, Action::ResetSaved] { + let state = SharedState::default(); + let connection = state.connected("A".into()); + state.configuration_event(ClientEvent::Configuration(unknown_board())); + state.edit(401, &serde_json::json!(30)).unwrap(); + let intent = state + .begin_intent(state.snapshot().context, action.kind()) + .unwrap(); + let (_master, _slave, path) = pty(); + let mut session = Session::new(Principal::new(255, 190)); + session.link = Some(open_serial(&path, SerialBaud::DEFAULT).unwrap()); + session.connection = Some(connection); + if reconnect { + state.disconnected(None); + session.connection = Some(state.connected("B".into())); + } + state.configuration_event(ClientEvent::Configuration(unknown_board())); + let before = state.snapshot(); + handle_command(Command { intent, action }, &mut session, &state); + assert!(session.outbound.is_none()); + assert!(session.client.target().is_err()); + assert_eq!(state.snapshot().config_revision, before.config_revision); + assert_eq!(state.snapshot().context, before.context); + assert_eq!(state.snapshot().path, before.path); + } + } +} + +#[test] +fn partial_writes_keep_exact_offsets_and_total_deadlines() { + struct ShortWriter { + bytes: Vec, + stalled: bool, + } + impl Write for ShortWriter { + fn write(&mut self, bytes: &[u8]) -> io::Result { + if self.stalled { + return Err(io::ErrorKind::WouldBlock.into()); + } + let count = bytes.len().min(3); + self.bytes.extend_from_slice(&bytes[..count]); + Ok(count) + } + fn flush(&mut self) -> io::Result<()> { + Ok(()) + } + } + let (_, frame) = pending_close(); + let mut pending = PendingWrite { + frame, + position: 0, + deadline: Instant::now() + WRITE_TIMEOUT, + }; + let mut output = ShortWriter { + bytes: Vec::new(), + stalled: false, + }; + assert!(!pending.advance(&mut output).unwrap()); + output.stalled = true; + assert!(!pending.advance(&mut output).unwrap()); + assert_eq!(pending.position, 3); + output.stalled = false; + for _ in 0..frame.raw_bytes().len() { + if pending.advance(&mut output).unwrap() { + break; + } + } + assert_eq!(output.bytes, frame.raw_bytes()); + pending.position = 0; + pending.deadline = Instant::now(); + assert!( + pending + .advance(&mut output) + .unwrap_err() + .contains("deadline") + ); + assert_eq!(output.bytes, frame.raw_bytes()); +} + +#[test] +fn nonblocking_pty_backpressure_can_be_cancelled_and_shutdown_preserves_close_uncertainty() { + let (_master, _slave, path) = pty(); + let mut file = open_serial(&path, SerialBaud::DEFAULT).unwrap(); + // SAFETY: fcntl reads flags for this still-owned valid fd. + let flags = unsafe { libc::fcntl(file.as_raw_fd(), libc::F_GETFL) }; + assert_ne!(flags & libc::O_NONBLOCK, 0); + fill_transmit_queue(&mut file); + let state = Arc::new(SharedState::default()); + let connection = state.connected(path); + state.configuration_event(ClientEvent::Configuration(unknown_board())); + let (client, frame) = pending_close(); + let mut session = Session::new(Principal::new(255, 190)); + session.client = client; + session.connection = Some(connection); + session.link = Some(file); + let started = Instant::now(); + stall_frame(&mut session, frame); + assert!(started.elapsed() < Duration::from_millis(100)); + + let stopping = Arc::new(AtomicBool::new(false)); + let (commands, receiver) = sync_channel(COMMAND_CAPACITY); + let worker_state = Arc::clone(&state); + let worker_stop = Arc::clone(&stopping); + let worker = thread::spawn(move || worker(receiver, worker_state, session, worker_stop)); + let controller = SerialController { + commands, + worker: Mutex::new(Some(worker)), + baud: SerialBaud::DEFAULT, + state: Arc::clone(&state), + stopping, + }; + let (done, completed) = mpsc::channel(); + let shutdown = thread::spawn(move || { + drop(controller); + done.send(()).unwrap(); + }); + completed + .recv_timeout(Duration::from_millis(500)) + .expect("shutdown must cancel a stalled write"); + shutdown.join().unwrap(); + assert!(state.snapshot().path.is_none()); + assert!(state.snapshot().durable_state_unknown); + assert!(state.snapshot().error.unwrap().contains("unknown")); +} + +#[test] +fn stalled_pty_write_hits_its_deadline_without_blocking_the_worker() { + let (_master, _slave, path) = pty(); + let mut file = open_serial(&path, SerialBaud::DEFAULT).unwrap(); + fill_transmit_queue(&mut file); + let mut session = Session::new(Principal::new(255, 190)); + session.link = Some(file); + let (_, frame) = pending_close(); + stall_frame(&mut session, frame); + session.outbound.as_mut().unwrap().deadline = Instant::now(); + assert!( + flush_pending(&mut session) + .unwrap_err() + .contains("deadline") + ); +} diff --git a/firmware/tools/configurator/src/serial_worker.rs b/firmware/tools/configurator/src/serial_worker.rs index 34fcd7e..c25ee63 100644 --- a/firmware/tools/configurator/src/serial_worker.rs +++ b/firmware/tools/configurator/src/serial_worker.rs @@ -7,17 +7,18 @@ use std::{ os::unix::fs::OpenOptionsExt, sync::{ Arc, Mutex, MutexGuard, + atomic::{AtomicBool, Ordering}, mpsc::{Receiver, SyncSender, TryRecvError, TrySendError, sync_channel}, }, thread::{self, JoinHandle}, - time::Duration, + time::{Duration, Instant}, }; use crate::{ - config_client::{ConfigClient, Edit, Outcome}, + config_client::{ConfigClient, Outcome}, log_monitor::{LogObservation, event_level_from_wire, wire_level_label}, packet_monitor::PacketObservation, - state::SharedState, + state::{IntentKind, IntentToken, Revision, SharedState, ViewContext}, }; use kaptan_file_mavlink::{Principal, Target}; use kaptan_log::Level; @@ -30,6 +31,7 @@ use mavlink_core::Message; const COMMAND_CAPACITY: usize = 8; const IDLE_POLL: Duration = Duration::from_millis(20); +const WRITE_TIMEOUT: Duration = Duration::from_secs(1); /// A line rate that the host termios adapter can represent exactly. #[derive(Clone, Copy, Debug, Eq, PartialEq)] @@ -64,17 +66,35 @@ impl SerialBaud { } } -pub(crate) enum Command { +pub(crate) enum Action { Connect { path: String, baud: SerialBaud }, Disconnect, - Save(Vec), - Shutdown, + Save, + ResetSaved, +} + +impl Action { + fn kind(&self) -> IntentKind { + match self { + Self::Connect { .. } => IntentKind::Connect, + Self::Disconnect => IntentKind::Disconnect, + Self::Save => IntentKind::Save, + Self::ResetSaved => IntentKind::Reset, + } + } +} + +struct Command { + intent: IntentToken, + action: Action, } pub struct SerialController { commands: SyncSender, worker: Mutex>>, baud: SerialBaud, + state: Arc, + stopping: Arc, } impl SerialController { @@ -84,36 +104,48 @@ impl SerialController { local: Principal, ) -> Result { let (commands, receiver) = sync_channel(COMMAND_CAPACITY); + let stopping = Arc::new(AtomicBool::new(false)); + let worker_state = Arc::clone(&state); + let worker_stopping = Arc::clone(&stopping); let worker = thread::Builder::new() .name("kaptan-config-serial".into()) - .spawn(move || worker(receiver, state, local)) + .spawn(move || worker(receiver, worker_state, Session::new(local), worker_stopping)) .map_err(|error| error.to_string())?; Ok(Self { commands, worker: Mutex::new(Some(worker)), baud, + state, + stopping, }) } /// Opens one explicit point-to-point serial device at the selected CLI rate. - pub fn connect(&self, path: String) -> Result<(), String> { - self.send(Command::Connect { - path, - baud: self.baud, + pub fn connect(&self, path: String, context: ViewContext) -> Result<(), String> { + self.submit( + context, + Action::Connect { + path, + baud: self.baud, + }, + ) + } + + pub(crate) fn submit(&self, context: ViewContext, action: Action) -> Result<(), String> { + self.state.queue_intent(context, action.kind(), |intent| { + self.commands + .try_send(Command { intent, action }) + .map_err(|error| match error { + TrySendError::Full(_) => "configuration transport is busy".into(), + TrySendError::Disconnected(_) => { + "configuration transport is unavailable".into() + } + }) }) } - pub(crate) fn send(&self, command: Command) -> Result<(), String> { - self.commands - .try_send(command) - .map_err(|error| match error { - TrySendError::Full(_) => "configuration transport is busy".into(), - TrySendError::Disconnected(_) => "configuration transport is unavailable".into(), - }) - } - fn shutdown(&self) { - let _ = self.commands.send(Command::Shutdown); + self.stopping.store(true, Ordering::Release); if let Some(worker) = lock(&self.worker).take() { let _ = worker.join(); } @@ -130,6 +162,37 @@ struct Session { link: Option, receiver: IntegrityMavlinkReceiver<1>, client: ConfigClient, + connection: Option, + outbound: Option, +} + +struct PendingWrite { + frame: MavlinkFrame, + position: usize, + deadline: Instant, +} + +impl PendingWrite { + fn advance(&mut self, writer: &mut impl Write) -> Result { + if Instant::now() >= self.deadline { + return Err("serial write deadline expired".into()); + } + let bytes = self.frame.raw_bytes(); + match writer.write(&bytes[self.position..]) { + Ok(0) => return Err("serial write returned zero bytes".into()), + Ok(count) => self.position += count, + Err(error) + if matches!( + error.kind(), + std::io::ErrorKind::WouldBlock | std::io::ErrorKind::Interrupted + ) => + { + return Ok(false); + } + Err(error) => return Err(error.to_string()), + } + Ok(self.position == bytes.len()) + } } impl Session { @@ -138,29 +201,35 @@ impl Session { link: None, receiver: IntegrityMavlinkReceiver::default(), client: ConfigClient::new(local), + connection: None, + outbound: None, } } - fn close(&mut self) { + fn close(&mut self, state: &SharedState) { + if let Some(event) = self.client.interruption() { + state.configuration_event(event); + } self.link = None; + self.connection = None; + self.outbound = None; self.receiver.reset(); self.client.reset(); } } -fn worker(commands: Receiver, state: Arc, local: Principal) { - let mut session = Session::new(local); - loop { - loop { +fn worker( + commands: Receiver, + state: Arc, + mut session: Session, + stopping: Arc, +) { + 'worker: while !stopping.load(Ordering::Acquire) { + for _ in 0..COMMAND_CAPACITY { match commands.try_recv() { - Ok(Command::Shutdown) => { - session.close(); - state.disconnected(None); - return; - } Ok(command) => handle_command(command, &mut session, &state), Err(TryRecvError::Empty) => break, - Err(TryRecvError::Disconnected) => return, + Err(TryRecvError::Disconnected) => break 'worker, } } @@ -168,48 +237,116 @@ fn worker(commands: Receiver, state: Arc, local: Principal thread::sleep(IDLE_POLL); continue; } + if stopping.load(Ordering::Acquire) { + break; + } + if state.link_cancelled() { + session.close(&state); + state.disconnected(None); + continue; + } + let link = session.link.as_ref().unwrap(); + let mut descriptor = libc::pollfd { + fd: link.as_raw_fd(), + events: if session.outbound.is_some() { + libc::POLLOUT + } else { + libc::POLLIN + }, + revents: 0, + }; + // SAFETY: poll receives one initialized descriptor that stays open. + let ready = unsafe { libc::poll(&mut descriptor, 1, IDLE_POLL.as_millis() as i32) }; + if stopping.load(Ordering::Acquire) { + break; + } + if state.link_cancelled() { + continue; + } + if ready < 0 { + let error = std::io::Error::last_os_error(); + if error.kind() != std::io::ErrorKind::Interrupted { + session.close(&state); + state.disconnected(Some(format!("serial poll failed: {error}"))); + } + continue; + } + if descriptor.revents & (libc::POLLERR | libc::POLLHUP | libc::POLLNVAL) != 0 { + session.close(&state); + state.disconnected(Some("serial link closed".into())); + continue; + } if let Err(error) = service_link(&mut session, &state) { - session.close(); + session.close(&state); state.disconnected(Some(format!("serial link failed: {error}"))); } } + session.close(&state); + state.disconnected(None); } fn handle_command(command: Command, session: &mut Session, state: &SharedState) { - match command { - Command::Connect { path, baud } => { - session.close(); + if matches!(command.action, Action::Save | Action::ResetSaved) + && session.connection != Some(command.intent.context.connection) + { + state.reject_intent( + command.intent, + "queued action belongs to a different serial connection".into(), + ); + return; + } + let edits = match state.claim_intent(command.intent) { + Ok(edits) => edits, + Err(error) => { + state.reject_intent(command.intent, error); + return; + } + }; + match command.action { + Action::Connect { path, baud } => { + session.close(state); match open_serial(&path, baud) { Ok(link) => { + let Some(connection) = state.connection_opened(command.intent, path) else { + drop(link); + state.disconnected(None); + return; + }; session.link = Some(link); - state.connected(path); + session.connection = Some(connection); } Err(error) => { state.disconnected(Some(format!("cannot open serial device: {error}"))) } } } - Command::Disconnect => { - session.close(); + Action::Disconnect => { + session.close(state); state.disconnected(None); } - Command::Save(edits) => { - let request = session.client.save(edits); + action @ (Action::Save | Action::ResetSaved) => { + let request = match action { + Action::Save => session.client.save(edits), + Action::ResetSaved => session.client.reset_saved(), + _ => unreachable!(), + }; match request { Ok(request) => { - if let Err(error) = write_raw(session, request.raw_bytes()) { - session.close(); + if let Err(error) = write_raw(session, request) { + session.close(state); state.disconnected(Some(format!("serial link failed: {error}"))); } } Err(error) => state.worker_failed(error), } } - Command::Shutdown => unreachable!("shutdown handled by worker"), } } fn service_link(session: &mut Session, state: &SharedState) -> Result<(), String> { + if !flush_pending(session)? { + return Ok(()); + } let mut bytes = [0_u8; 512]; let read = match session .link @@ -221,7 +358,9 @@ fn service_link(session: &mut Session, state: &SharedState) -> Result<(), String Err(error) if matches!( error.kind(), - std::io::ErrorKind::TimedOut | std::io::ErrorKind::WouldBlock + std::io::ErrorKind::TimedOut + | std::io::ErrorKind::WouldBlock + | std::io::ErrorKind::Interrupted ) => { 0 @@ -229,14 +368,11 @@ fn service_link(session: &mut Session, state: &SharedState) -> Result<(), String Err(error) => return Err(error.to_string()), }; - let mut frames = Vec::new(); for byte in &bytes[..read] { session.receiver.receive_byte(*byte); - if let Some(frame) = session.receiver.take_received() { - frames.push(frame); - } - } - for frame in frames { + let Some(frame) = session.receiver.take_received() else { + continue; + }; let Some(message) = frame.decode() else { continue; }; @@ -249,7 +385,7 @@ fn service_link(session: &mut Session, state: &SharedState) -> Result<(), String { state.discovering(); let request = session.client.discover(target)?; - write_raw(session, request.raw_bytes())?; + write_raw(session, request)?; } let Some(source) = Target::try_new(raw.system_id(), raw.component_id()) else { continue; @@ -258,8 +394,10 @@ fn service_link(session: &mut Session, state: &SharedState) -> Result<(), String handle_client_outcome(session, state, outcome)?; } - let outcome = session.client.retry_due(); - handle_client_outcome(session, state, outcome)?; + if session.outbound.is_none() { + let outcome = session.client.retry_due(); + handle_client_outcome(session, state, outcome)?; + } Ok(()) } @@ -419,17 +557,35 @@ fn handle_client_outcome( state.configuration_event(event); Ok(()) } - Outcome::Send(request) => write_raw(session, request.raw_bytes()), + Outcome::Send(request) => write_raw(session, request), } } -fn write_raw(session: &mut Session, bytes: &[u8]) -> Result<(), String> { - session - .link - .as_mut() - .ok_or("serial link is closed")? - .write_all(bytes) - .map_err(|error| error.to_string()) +fn write_raw(session: &mut Session, frame: MavlinkFrame) -> Result<(), String> { + if session.outbound.is_some() { + return Err("serial frame is still being transmitted".into()); + } + session.outbound = Some(PendingWrite { + frame, + position: 0, + deadline: Instant::now() + WRITE_TIMEOUT, + }); + Ok(()) +} + +/// One nonblocking write per worker iteration. Commands and cancellation are +/// serviced between partial writes, including a stalled kernel transmit queue. +fn flush_pending(session: &mut Session) -> Result { + let Some(write) = &mut session.outbound else { + return Ok(true); + }; + if write.advance(session.link.as_mut().ok_or("serial link is closed")?)? { + session.outbound = None; + session.client.transmitted(); + Ok(true) + } else { + Ok(false) + } } fn decoded_heartbeat_target(frame: &MavlinkFrame, message: &MavMessage) -> Option { @@ -452,7 +608,7 @@ fn open_serial(path: &str, baud: SerialBaud) -> Result { let file = OpenOptions::new() .read(true) .write(true) - .custom_flags(libc::O_NOCTTY) + .custom_flags(libc::O_NOCTTY | libc::O_NONBLOCK | libc::O_CLOEXEC) .open(path) .map_err(|error| error.to_string())?; let descriptor = file.as_raw_fd(); @@ -492,6 +648,10 @@ fn lock(mutex: &Mutex) -> MutexGuard<'_, T> { } } +#[cfg(test)] +#[path = "serial_io_tests.rs"] +mod io_tests; + #[cfg(test)] mod tests { use super::*; @@ -746,7 +906,7 @@ mod tests { &IMU_DATA { time_us: 123, acceleration_milligravity: [1, 2, 3], - angular_rate_milliradians_per_second: [-4, 5, -6], + angular_rate_milliradians_per_second: [-34_871, 34_870, -8_731], temperature_centidegrees_celsius: 2_500, }, )); @@ -755,7 +915,7 @@ mod tests { assert_eq!(observation.values["time_us"], 123); assert_eq!( observation.values["angular_rate_milliradians_per_second"], - serde_json::json!([-4, 5, -6]) + serde_json::json!([-34_871, 34_870, -8_731]) ); assert!(observation.values.as_str().is_none()); } diff --git a/firmware/tools/configurator/src/state.rs b/firmware/tools/configurator/src/state.rs index 9d30937..e1be6e2 100644 --- a/firmware/tools/configurator/src/state.rs +++ b/firmware/tools/configurator/src/state.rs @@ -7,10 +7,10 @@ use std::{ }; use kaptan_config::{ConfigValue, FieldId, ValueKind}; -use serde::Serialize; +use serde::{Deserialize, Serialize}; use crate::{ - config_client::{ClientEvent, Edit, RemoteConfig, RemoteField, scalar_json, scalar_less_than}, + config_client::{ClientEvent, Edit, RemoteConfig, RemoteField, scalar_json}, log_monitor::{LogMonitor, LogObservation, LogSnapshot}, packet_monitor::{PacketMonitor, PacketObservation, PacketSnapshot}, }; @@ -20,21 +20,106 @@ pub struct StateSnapshot { pub path: Option, pub status: String, pub error: Option, - pub config_revision: u64, + pub config_revision: Revision, + pub context: ViewContext, pub has_config: bool, pub can_edit: bool, pub can_save: bool, + pub can_reset: bool, + pub durable_state_unknown: bool, pub can_disconnect: bool, } #[derive(Clone, Serialize)] pub struct ConfigSnapshot { + pub context: ViewContext, + pub config_revision: Revision, pub schema_id: Option, pub state: String, + pub committed_known: bool, pub fields: Vec, pub validation: Vec, } +/// Decimal strings preserve the complete revision domain in browser JSON. +#[derive(Clone, Copy, Debug, Eq, PartialEq, Serialize, Deserialize)] +#[serde(try_from = "String", into = "String")] +pub struct Revision(u128); + +impl From for String { + fn from(value: Revision) -> Self { + value.0.to_string() + } +} + +impl TryFrom for Revision { + type Error = String; + fn try_from(value: String) -> Result { + let parsed = value.parse::().map_err(|_| "invalid revision")?; + if parsed.to_string() != value { + return Err("noncanonical revision".into()); + } + Ok(Self(parsed)) + } +} + +impl Revision { + fn advance(&mut self) { + self.0 = self.0.checked_add(1).expect("revision exhausted"); + } +} + +/// Local operator-intent binding, not authentication or target authorization. +#[derive(Clone, Copy, Debug, Eq, PartialEq, Serialize, Deserialize)] +pub struct ViewContext { + instance: Revision, + pub(crate) connection: Revision, + configuration: Revision, + draft: Revision, +} + +impl ViewContext { + fn new() -> Self { + let mut bytes = [0_u8; 16]; + // SAFETY: getrandom writes at most the size of this live local buffer. + // Nonblocking startup fails closed if an instance ID cannot be obtained. + let count = + unsafe { libc::getrandom(bytes.as_mut_ptr().cast(), bytes.len(), libc::GRND_NONBLOCK) }; + assert_eq!( + count, + bytes.len() as isize, + "cannot create configurator instance ID" + ); + Self { + instance: Revision(u128::from_ne_bytes(bytes)), + connection: Revision(0), + configuration: Revision(0), + draft: Revision(0), + } + } +} + +#[derive(Clone, Serialize)] +pub struct ViewSnapshot { + pub state: StateSnapshot, + pub config: ConfigSnapshot, +} + +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub(crate) enum IntentKind { + Connect, + Disconnect, + Save, + Reset, +} + +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub(crate) struct IntentToken { + pub(crate) context: ViewContext, + revision: Revision, + kind: IntentKind, +} + #[derive(Clone, Serialize)] pub struct FieldView { pub id: u16, @@ -47,8 +132,14 @@ pub struct FieldView { pub maximum: Option, pub factory: serde_json::Value, pub value: serde_json::Value, + pub active: Option, + pub saved: Option, + pub draft: Option, + pub error: Option, pub help: String, pub options: Vec, + pub finite: bool, + pub nonzero: bool, } #[derive(Clone, Serialize)] @@ -60,7 +151,9 @@ pub struct EnumOptionView { #[derive(Clone, Copy, Debug, Eq, PartialEq)] enum Operation { Disconnected, + Queued(IntentToken), Connecting, + Disconnecting, Discovering, Ready, Saving, @@ -74,7 +167,10 @@ struct State { error: Option, config: Option, draft: BTreeMap, - config_revision: u64, + invalid_edits: BTreeMap, + durable_uncertainty: Option, + config_revision: Revision, + context: ViewContext, packets: PacketMonitor, logs: LogMonitor, } @@ -88,7 +184,10 @@ impl Default for State { error: None, config: None, draft: BTreeMap::new(), - config_revision: 0, + invalid_edits: BTreeMap::new(), + durable_uncertainty: None, + config_revision: Revision(0), + context: ViewContext::new(), packets: PacketMonitor::default(), logs: LogMonitor::default(), } @@ -101,42 +200,84 @@ pub struct SharedState(Mutex); impl SharedState { pub fn snapshot(&self) -> StateSnapshot { let state = lock(&self.0); + Self::state_snapshot(&state) + } + + fn state_snapshot(state: &State) -> StateSnapshot { StateSnapshot { path: state.path.clone(), status: state.status.clone(), error: state.error.clone(), config_revision: state.config_revision, + context: state.context, has_config: state.config.is_some(), - can_edit: state.operation == Operation::Ready && state.config.is_some(), - can_save: state.operation == Operation::Ready && !state.draft.is_empty(), + can_edit: editable(state), + can_save: editable(state) && !state.draft.is_empty() && state.invalid_edits.is_empty(), + can_reset: resettable(state), + durable_state_unknown: state.durable_uncertainty.is_some(), can_disconnect: !matches!(state.operation, Operation::Disconnected), } } pub fn configuration(&self) -> ConfigSnapshot { let state = lock(&self.0); + Self::config_snapshot(&state) + } + + pub fn view(&self) -> ViewSnapshot { + Self::view_snapshot(&lock(&self.0)) + } + + fn view_snapshot(state: &State) -> ViewSnapshot { + ViewSnapshot { + state: Self::state_snapshot(state), + config: Self::config_snapshot(state), + } + } + + fn config_snapshot(state: &State) -> ConfigSnapshot { let Some(config) = &state.config else { return ConfigSnapshot { + context: state.context, + config_revision: state.config_revision, schema_id: None, state: "unavailable".into(), + committed_known: false, fields: Vec::new(), validation: state.error.clone().into_iter().collect(), }; }; ConfigSnapshot { + context: state.context, + config_revision: state.config_revision, schema_id: Some(hex(&config.schema_id)), - state: if config.restart_required { + state: if config.committed_error.is_some() { + "committed unknown" + } else if config.restart_required { "restart required" } else { "active" } .into(), + committed_known: config.committed_error.is_none(), fields: config .fields .iter() - .map(|field| field_view(field, state.draft.get(&field.id).copied())) + .map(|field| { + field_view( + field, + config.committed_error.is_none(), + state.draft.get(&field.id).copied(), + state.invalid_edits.get(&field.id), + ) + }) + .collect(), + validation: state + .error + .clone() + .into_iter() + .chain(state.invalid_edits.values().map(|(_, error)| error.clone())) .collect(), - validation: state.error.clone().into_iter().collect(), } } @@ -156,148 +297,122 @@ impl SharedState { lock(&self.0).logs.snapshot() } - pub fn begin_connect(&self) -> Result<(), String> { + #[cfg(test)] + pub(crate) fn begin_intent( + &self, + context: ViewContext, + kind: IntentKind, + ) -> Result { let mut state = lock(&self.0); - if state.operation != Operation::Disconnected { - return Err("disconnect the current serial link first".into()); + Self::reserve_intent(&mut state, context, kind) + } + + pub(crate) fn queue_intent( + &self, + context: ViewContext, + kind: IntentKind, + enqueue: impl FnOnce(IntentToken) -> Result<(), String>, + ) -> Result<(), String> { + let mut state = lock(&self.0); + let previous = state.operation; + let status = state.status.clone(); + let intent = Self::reserve_intent(&mut state, context, kind)?; + // The closure only performs bounded try_send. Keeping the state lock + // here makes queue rejection restore the exact previous operation. + if let Err(error) = enqueue(intent) { + state.operation = previous; + state.status = status; + state.error = Some(error.clone()); + state.config_revision.advance(); + return Err(error); } - state.operation = Operation::Connecting; - state.status = "Connecting…".into(); - state.error = None; Ok(()) } - pub fn connected(&self, path: String) { - let mut state = lock(&self.0); - state.operation = Operation::Discovering; - state.path = Some(path); - state.status = "Connected; waiting for controller heartbeat…".into(); - state.error = None; - state.config = None; - state.draft.clear(); - state.packets.clear(); - state.logs.clear(); - state.config_revision = state.config_revision.wrapping_add(1); - } - - pub fn disconnected(&self, error: Option) { - let mut state = lock(&self.0); - state.operation = Operation::Disconnected; - state.path = None; - state.status = "Disconnected.".into(); - state.error = error; - state.config = None; - state.draft.clear(); - state.packets.clear(); - state.logs.clear(); - state.config_revision = state.config_revision.wrapping_add(1); - } - - pub fn discovering(&self) { - let mut state = lock(&self.0); - state.operation = Operation::Discovering; - state.status = "Exporting configuration file…".into(); - state.error = None; - } - - pub(crate) fn configuration_event(&self, event: ClientEvent) { - let mut state = lock(&self.0); - match event { - ClientEvent::Configuration(config) => { - state.operation = Operation::Ready; - state.status = if config.restart_required { - "Saved. Restart the controller to activate the new configuration.".into() - } else { - "Configuration ready.".into() - }; - state.error = None; - state.config = Some(config); - state.draft.clear(); - state.config_revision = state.config_revision.wrapping_add(1); + fn reserve_intent( + state: &mut State, + context: ViewContext, + kind: IntentKind, + ) -> Result { + check_context(state, context)?; + match kind { + IntentKind::Connect if state.operation != Operation::Disconnected => { + return Err("disconnect the current serial link first".into()); } - ClientEvent::Failure(detail) => { - state.error = Some(detail); - state.operation = if state.config.is_some() { - Operation::Ready - } else { - Operation::Discovering - }; + IntentKind::Disconnect if state.operation == Operation::Disconnected => { + return Err("serial link is already disconnected".into()); } - ClientEvent::RediscoveryRequired(detail) => { - state.operation = Operation::Discovering; - state.status = "Waiting to rediscover configuration…".into(); - state.error = Some(detail); - state.config = None; - state.draft.clear(); - state.config_revision = state.config_revision.wrapping_add(1); + IntentKind::Save => { + if !editable(state) { + return Err("saved configuration is unknown or busy".into()); + } + if !state.invalid_edits.is_empty() { + return Err("correct invalid draft fields before saving".into()); + } + if state.draft.is_empty() { + return Err("configuration has no changed fields".into()); + } } - ClientEvent::DurableStateUnknown(detail) => { - state.operation = Operation::DurableStateUnknown; - state.status = - "Configuration state is unknown. Power-cycle the controller, then disconnect and reconnect." - .into(); - state.error = Some(detail); - state.config = None; - state.draft.clear(); - state.config_revision = state.config_revision.wrapping_add(1); + IntentKind::Reset if !resettable(state) => { + return Err("boot-active configuration is unavailable or busy".into()); } + _ => {} } + state.config_revision.advance(); + let intent = IntentToken { + context, + revision: state.config_revision, + kind, + }; + state.operation = Operation::Queued(intent); + state.status = "Configuration action queued…".into(); + Ok(intent) } - pub fn edit(&self, field_id: u16, value: &serde_json::Value) -> Result { - let field_id = - FieldId::try_new(field_id).ok_or_else(|| format!("unknown field id {field_id}"))?; + /// The serial worker claims an intent before touching its session. Only it + /// can replace the physical link. Queued saves cannot migrate to a new link. + pub(crate) fn claim_intent(&self, intent: IntentToken) -> Result, String> { let mut state = lock(&self.0); - if state.operation != Operation::Ready { - return Err("configuration is not ready for edits".into()); + if state.operation != Operation::Queued(intent) + || state.context.connection != intent.context.connection + || (intent.kind != IntentKind::Disconnect && state.context != intent.context) + { + return Err("stale queued action discarded; re-read required".into()); } - let field = state - .config - .as_ref() - .and_then(|config| config.fields.iter().find(|field| field.id == field_id)) - .ok_or_else(|| format!("unknown field id {}", field_id.get()))?; - let next = parse_scalar(field.kind, value)?; - validate_bounds(field, next)?; - if next == field.value { - state.draft.remove(&field_id); + state.operation = match intent.kind { + IntentKind::Connect => Operation::Connecting, + IntentKind::Disconnect => Operation::Disconnecting, + IntentKind::Save | IntentKind::Reset => Operation::Saving, + }; + state.status = match intent.kind { + IntentKind::Connect => "Connecting…", + IntentKind::Disconnect => "Disconnecting…", + IntentKind::Save => "Validating and saving configuration…", + IntentKind::Reset => "Resetting saved configuration to target factory values…", + } + .into(); + state.config_revision.advance(); + Ok(if intent.kind == IntentKind::Save { + state + .draft + .iter() + .map(|(field_id, value)| Edit { + field_id: *field_id, + value: *value, + }) + .collect() } else { - state.draft.insert(field_id, next); - } - state.error = None; - state.config_revision = state.config_revision.wrapping_add(1); - drop(state); - Ok(self.configuration()) + Vec::new() + }) } - pub(crate) fn begin_save(&self) -> Result, String> { + pub(crate) fn reject_intent(&self, intent: IntentToken, detail: String) { let mut state = lock(&self.0); - if state.operation != Operation::Ready { - return Err("configuration is busy".into()); + if state.operation != Operation::Queued(intent) { + return; } - state - .config - .as_ref() - .ok_or("configuration has not been discovered")?; - if state.draft.is_empty() { - return Err("configuration has no changed fields".into()); - } - let edits = state - .draft - .iter() - .map(|(field_id, value)| Edit { - field_id: *field_id, - value: *value, - }) - .collect(); - state.operation = Operation::Saving; - state.status = "Validating and saving configuration…".into(); - state.error = None; - Ok(edits) - } - - pub fn worker_failed(&self, detail: String) { - let mut state = lock(&self.0); state.error = Some(detail); + state.status = "Queued action discarded; re-read configuration before retrying.".into(); state.operation = if state.config.is_some() { Operation::Ready } else if state.path.is_some() { @@ -305,10 +420,286 @@ impl SharedState { } else { Operation::Disconnected }; + state.config_revision.advance(); + } + + pub(crate) fn link_cancelled(&self) -> bool { + matches!( + lock(&self.0).operation, + Operation::Queued(IntentToken { + kind: IntentKind::Disconnect, + .. + }) | Operation::Disconnecting + ) + } + + pub(crate) fn connection_opened(&self, intent: IntentToken, path: String) -> Option { + let mut state = lock(&self.0); + if state.operation != Operation::Connecting || state.context != intent.context { + return None; + } + Some(Self::connected_locked(&mut state, path)) + } + + #[cfg(test)] + pub fn connected(&self, path: String) -> Revision { + let mut state = lock(&self.0); + Self::connected_locked(&mut state, path) + } + + fn connected_locked(state: &mut State, path: String) -> Revision { + state.context.connection.advance(); + state.context.configuration.advance(); + state.operation = Operation::Discovering; + state.path = Some(path); + state.status = "Connected; waiting for controller heartbeat…".into(); + state.error = state.durable_uncertainty.clone(); + state.config = None; + state.draft.clear(); + state.invalid_edits.clear(); + state.packets.clear(); + state.logs.clear(); + state.config_revision.advance(); + state.context.connection + } + + pub fn disconnected(&self, error: Option) { + let mut state = lock(&self.0); + state.context.connection.advance(); + state.context.configuration.advance(); + state.operation = Operation::Disconnected; + state.path = None; + state.status = if state.durable_uncertainty.is_some() { + "Disconnected; saved configuration is unknown. Reconnect to re-read or reset it." + } else { + "Disconnected." + } + .into(); + state.error = match (error, &state.durable_uncertainty) { + (Some(error), Some(uncertainty)) => Some(format!("{error}; {uncertainty}")), + (error, uncertainty) => error.or_else(|| uncertainty.clone()), + }; + state.config = None; + state.draft.clear(); + state.invalid_edits.clear(); + state.packets.clear(); + state.logs.clear(); + state.config_revision.advance(); + } + + pub fn discovering(&self) { + let mut state = lock(&self.0); + if matches!( + state.operation, + Operation::Queued(IntentToken { + kind: IntentKind::Disconnect, + .. + }) | Operation::Disconnecting + ) { + return; + } + state.operation = Operation::Discovering; + state.status = "Reading saved and boot-active configuration…".into(); + state.config_revision.advance(); + } + + pub(crate) fn configuration_event(&self, event: ClientEvent) { + let mut state = lock(&self.0); + let disconnect = match state.operation { + Operation::Queued(intent) if intent.kind == IntentKind::Disconnect => Some(intent), + _ => None, + }; + state.context.configuration.advance(); + match event { + ClientEvent::Configuration(config) => { + state.operation = Operation::Ready; + state.status = if config.committed_error.is_some() { + "Saved configuration is unknown. Boot-active values are read-only; reset saved configuration to recover.".into() + } else if config.restart_required { + "Saved configuration differs from boot-active values. Restart the controller to activate it.".into() + } else { + "Configuration ready.".into() + }; + state.error = config.committed_error.clone(); + state.durable_uncertainty = config.committed_error.clone(); + state.config = Some(config); + state.draft.clear(); + state.invalid_edits.clear(); + state.config_revision.advance(); + } + ClientEvent::Failure(detail) => { + state.error = Some(detail); + state.status = "Configuration operation failed. Review the error and retry.".into(); + state.operation = if state.config.is_some() { + Operation::Ready + } else { + Operation::Discovering + }; + state.config_revision.advance(); + } + ClientEvent::RediscoveryRequired(detail) => { + state.operation = Operation::Discovering; + state.status = "Waiting to rediscover configuration…".into(); + state.error = Some(detail); + state.config = None; + state.draft.clear(); + state.invalid_edits.clear(); + state.config_revision.advance(); + } + ClientEvent::DurableStateUnknown(detail) => { + state.operation = Operation::DurableStateUnknown; + state.status = + "Saved configuration is unknown. Reconnect to re-read it, or reset saved configuration." + .into(); + state.error = Some(detail.clone()); + state.durable_uncertainty = Some(detail.clone()); + if let Some(config) = &mut state.config { + config.committed_error = Some(detail); + } + state.draft.clear(); + state.invalid_edits.clear(); + state.config_revision.advance(); + } + } + if let Some(intent) = disconnect { + state.operation = Operation::Queued(intent); + } + } + + pub fn edit_context( + &self, + context: ViewContext, + field_id: u16, + value: &serde_json::Value, + ) -> Result { + let field_id = + FieldId::try_new(field_id).ok_or_else(|| format!("unknown field id {field_id}"))?; + let mut state = lock(&self.0); + check_context(&state, context)?; + if !editable(&state) { + return Err("configuration is not ready for edits".into()); + } + let field = state + .config + .as_ref() + .and_then(|config| config.field(field_id)) + .ok_or_else(|| format!("unknown field id {}", field_id.get()))?; + let next = match parse_scalar(field.kind, value).and_then(|next| { + field.validate_value(next)?; + Ok(next) + }) { + Ok(next) => next, + Err(error) => { + state.context.draft.advance(); + state + .invalid_edits + .insert(field_id, (value.clone(), error.clone())); + state.config_revision.advance(); + return Ok(Self::view_snapshot(&state)); + } + }; + if next == field.value { + state.draft.remove(&field_id); + } else { + state.draft.insert(field_id, next); + } + state.invalid_edits.remove(&field_id); + state.context.draft.advance(); + state.error = None; + state.config_revision.advance(); + Ok(Self::view_snapshot(&state)) + } + + #[cfg(test)] + pub fn edit(&self, field_id: u16, value: &serde_json::Value) -> Result { + let view = self.edit_context(self.snapshot().context, field_id, value)?; + if let Some(error) = view + .config + .fields + .iter() + .find(|field| field.id == field_id) + .and_then(|field| field.error.clone()) + { + return Err(error); + } + Ok(view.config) + } + + #[cfg(test)] + pub fn begin_connect(&self) -> Result<(), String> { + let intent = self.begin_intent(self.snapshot().context, IntentKind::Connect)?; + self.claim_intent(intent).map(|_| ()) + } + + #[cfg(test)] + pub(crate) fn begin_save(&self) -> Result, String> { + let intent = self.begin_intent(self.snapshot().context, IntentKind::Save)?; + self.claim_intent(intent) + } + + #[cfg(test)] + pub(crate) fn begin_reset(&self) -> Result<(), String> { + let intent = self.begin_intent(self.snapshot().context, IntentKind::Reset)?; + self.claim_intent(intent).map(|_| ()) + } + + pub fn worker_failed(&self, detail: String) { + let mut state = lock(&self.0); + let disconnect = match state.operation { + Operation::Queued(intent) if intent.kind == IntentKind::Disconnect => Some(intent), + _ => None, + }; + state.error = Some(match &state.durable_uncertainty { + Some(uncertainty) => format!("{detail}; {uncertainty}"), + None => detail, + }); + state.status = "Configuration operation could not start.".into(); + state.config_revision.advance(); + state.operation = if state.config.is_some() { + Operation::Ready + } else if state.path.is_some() { + Operation::Discovering + } else { + Operation::Disconnected + }; + if let Some(intent) = disconnect { + state.operation = Operation::Queued(intent); + } } } -fn field_view(field: &RemoteField, draft: Option) -> FieldView { +fn check_context(state: &State, context: ViewContext) -> Result<(), String> { + if state.context != context { + return Err("stale view; re-read configuration before this action".into()); + } + Ok(()) +} + +fn editable(state: &State) -> bool { + state.operation == Operation::Ready + && state + .config + .as_ref() + .is_some_and(|config| config.committed_error.is_none()) +} + +fn resettable(state: &State) -> bool { + matches!( + state.operation, + Operation::Ready | Operation::DurableStateUnknown + ) && state.path.is_some() + && state + .config + .as_ref() + .is_some_and(|config| config.fields.iter().all(|field| field.active.is_some())) +} + +fn field_view( + field: &RemoteField, + committed_known: bool, + draft: Option, + invalid: Option<&(serde_json::Value, String)>, +) -> FieldView { let (group, tail) = field .key .rsplit_once('.') @@ -329,7 +720,24 @@ fn field_view(field: &RemoteField, draft: Option) -> FieldView { minimum: field.minimum.map(scalar_json), maximum: field.maximum.map(scalar_json), factory: scalar_json(field.factory), - value: scalar_json(draft.unwrap_or(field.value)), + value: invalid.map_or_else( + || { + scalar_json(draft.unwrap_or_else(|| { + if committed_known { + field.value + } else { + field.active.unwrap_or(field.value) + } + })) + }, + |(value, _)| value.clone(), + ), + active: field.active.map(scalar_json), + saved: committed_known.then(|| scalar_json(field.value)), + draft: invalid + .map(|(value, _)| value.clone()) + .or_else(|| draft.map(scalar_json)), + error: invalid.map(|(_, error)| error.clone()), help: format!("Stable field ID {}.", field.id.get()), options: field .options @@ -339,6 +747,8 @@ fn field_view(field: &RemoteField, draft: Option) -> FieldView { label: option.label.clone(), }) .collect(), + finite: field.finite, + nonzero: field.nonzero, } } @@ -372,50 +782,32 @@ fn parse_scalar(kind: ValueKind, value: &serde_json::Value) -> Result text - .parse::() - .map_err(|_| "expected a 32-bit decimal".to_owned()) - .and_then(|value| { - value - .is_finite() - .then_some(ConfigValue::F32(value)) - .ok_or_else(|| "decimal must be finite".into()) - }), - ValueKind::F64 => text - .parse::() - .map_err(|_| "expected a 64-bit decimal".to_owned()) - .and_then(|value| { - value - .is_finite() - .then_some(ConfigValue::F64(value)) - .ok_or_else(|| "decimal must be finite".into()) - }), + ValueKind::F32 => { + if let Some(bits) = text.strip_prefix("bits:0x") { + return parse_float_bits(bits, 8) + .map(|bits| ConfigValue::F32(f32::from_bits(bits as u32))); + } + text.parse::() + .map(ConfigValue::F32) + .map_err(|_| "expected a 32-bit decimal or bits:0x plus 8 hex digits".into()) + } + ValueKind::F64 => { + if let Some(bits) = text.strip_prefix("bits:0x") { + return parse_float_bits(bits, 16) + .map(|bits| ConfigValue::F64(f64::from_bits(bits))); + } + text.parse::() + .map(ConfigValue::F64) + .map_err(|_| "expected a 64-bit decimal or bits:0x plus 16 hex digits".into()) + } } } -fn validate_bounds(field: &RemoteField, value: ConfigValue) -> Result<(), String> { - if !field.options.is_empty() - && !matches!( - value, - ConfigValue::U8(value) - if field.options.iter().any(|option| option.value == value) - ) - { - return Err(format!("{} requires a named choice", field.key)); +fn parse_float_bits(bits: &str, digits: usize) -> Result { + if bits.len() != digits || !bits.bytes().all(|byte| byte.is_ascii_hexdigit()) { + return Err(format!("expected exactly {digits} hexadecimal float bits")); } - if field - .minimum - .is_some_and(|minimum| scalar_less_than(value, minimum)) - { - return Err(format!("{} is below its inclusive minimum", field.key)); - } - if field - .maximum - .is_some_and(|maximum| scalar_less_than(maximum, value)) - { - return Err(format!("{} is above its inclusive maximum", field.key)); - } - Ok(()) + u64::from_str_radix(bits, 16).map_err(|_| "invalid float bits".into()) } fn humanize(value: &str) -> String { @@ -449,14 +841,15 @@ fn lock(mutex: &Mutex) -> MutexGuard<'_, T> { } #[cfg(test)] -mod tests { +pub(crate) mod tests { use super::*; use crate::config_client::RemoteEnumOption; - fn unknown_board() -> RemoteConfig { + pub(crate) fn unknown_board() -> RemoteConfig { RemoteConfig { schema_id: *b"UNKNOWN-SCHEMA01", restart_required: false, + committed_error: None, fields: vec![RemoteField { id: FieldId::new(401), key: "future.sensor.rate_hz".into(), @@ -466,11 +859,75 @@ mod tests { maximum: Some(ConfigValue::U32(100)), factory: ConfigValue::U32(20), value: ConfigValue::U32(25), + active: Some(ConfigValue::U32(25)), options: Vec::new(), + finite: false, + nonzero: false, }], } } + #[test] + fn revisions_are_lossless_and_queue_rejection_preserves_an_inflight_save() { + let revision = Revision(u128::from(u64::MAX) + 1); + let encoded = serde_json::to_value(revision).unwrap(); + assert_eq!(encoded, serde_json::json!("18446744073709551616")); + assert_eq!( + serde_json::from_value::(encoded).unwrap(), + revision + ); + assert!(serde_json::from_value::(serde_json::json!(5)).is_err()); + let state = SharedState::default(); + state.connected("A".into()); + state.configuration_event(ClientEvent::Configuration(unknown_board())); + let old_draft = state.snapshot().context; + state + .edit_context(old_draft, 401, &serde_json::json!(30)) + .unwrap(); + assert!( + state + .edit_context(old_draft, 401, &serde_json::json!(40)) + .is_err() + ); + assert!(state.begin_intent(old_draft, IntentKind::Save).is_err()); + state.begin_save().unwrap(); + assert!( + state + .queue_intent(state.snapshot().context, IntentKind::Disconnect, |_| Err( + "queue full".into() + )) + .is_err() + ); + assert!(!state.snapshot().can_edit); + assert!(!state.snapshot().can_reset); + assert_eq!( + state.configuration().fields[0].draft, + Some(serde_json::json!(30)) + ); + } + + #[test] + fn disconnect_intent_survives_discovery_and_cancels_an_open_in_progress() { + let state = SharedState::default(); + let connect = state + .begin_intent(state.snapshot().context, IntentKind::Connect) + .unwrap(); + state.claim_intent(connect).unwrap(); + let disconnect = state + .begin_intent(state.snapshot().context, IntentKind::Disconnect) + .unwrap(); + assert!( + state + .connection_opened(connect, "cancelled".into()) + .is_none() + ); + state.discovering(); + state.configuration_event(ClientEvent::RediscoveryRequired("readback failed".into())); + state.claim_intent(disconnect).unwrap(); + assert!(state.link_cancelled()); + assert!(state.snapshot().path.is_none()); + } + #[test] fn unknown_schema_edits_by_stable_field_id_without_a_catalog() { let state = SharedState::default(); @@ -507,6 +964,91 @@ mod tests { assert!(state.edit(401, &serde_json::json!(101)).is_err()); } + #[test] + fn active_saved_and_draft_are_distinct_and_invalid_visible_edits_block_save() { + let state = SharedState::default(); + state.connected("test".into()); + let mut config = unknown_board(); + config.fields[0].active = Some(ConfigValue::U32(20)); + config.restart_required = true; + state.configuration_event(ClientEvent::Configuration(config)); + state.edit(401, &serde_json::json!(30)).unwrap(); + assert!(state.snapshot().can_save); + let view = state.configuration(); + assert_eq!(view.fields[0].active, Some(serde_json::json!(20))); + assert_eq!(view.fields[0].saved, Some(serde_json::json!(25))); + assert_eq!(view.fields[0].draft, Some(serde_json::json!(30))); + for invalid in [serde_json::json!(101), serde_json::json!("not a number")] { + assert!(state.edit(401, &invalid).is_err()); + let view = state.configuration(); + assert_eq!(view.fields[0].value, invalid); + assert_eq!(view.fields[0].draft, Some(invalid)); + assert!(view.fields[0].error.is_some()); + assert!(!view.validation.is_empty()); + assert!(!state.snapshot().can_save); + assert!(state.begin_save().is_err()); + } + state.edit(401, &serde_json::json!(35)).unwrap(); + assert!(state.configuration().fields[0].error.is_none()); + assert_eq!(state.begin_save().unwrap()[0].value, ConfigValue::U32(35)); + } + + #[test] + fn unknown_saved_is_read_only_but_factory_reset_remains_available() { + let state = SharedState::default(); + state.connected("test".into()); + let mut config = unknown_board(); + config.committed_error = Some("StorageFailure".into()); + state.configuration_event(ClientEvent::Configuration(config)); + let view = state.configuration(); + assert_eq!(view.state, "committed unknown"); + assert!(!view.committed_known); + assert_eq!(view.fields[0].active, Some(serde_json::json!(25))); + assert!(view.fields[0].saved.is_none()); + assert!(view.fields[0].draft.is_none()); + assert!(!state.snapshot().can_edit); + assert!(!state.snapshot().can_save); + assert!(state.snapshot().can_reset); + assert!(state.edit(401, &serde_json::json!(30)).is_err()); + assert!(state.begin_save().is_err()); + state.begin_reset().unwrap(); + assert!(!state.snapshot().can_reset); + assert!(state.begin_reset().is_err()); + state.worker_failed("queue full".into()); + assert!(state.snapshot().can_reset); + assert!(!state.snapshot().can_save); + assert!(state.snapshot().durable_state_unknown); + assert!(state.snapshot().error.unwrap().contains("StorageFailure")); + } + + #[test] + fn stale_draft_is_cleared_before_rediscovery_and_failed_validation_keeps_draft() { + let state = SharedState::default(); + state.connected("test".into()); + state.configuration_event(ClientEvent::Configuration(unknown_board())); + state.edit(401, &serde_json::json!(30)).unwrap(); + state.begin_save().unwrap(); + state.configuration_event(ClientEvent::Failure( + "target cross-field validation failed".into(), + )); + assert!(state.snapshot().can_save); + assert_eq!( + state.configuration().fields[0].draft, + Some(serde_json::json!(30)) + ); + state.begin_save().unwrap(); + state.configuration_event(ClientEvent::RediscoveryRequired( + "RequestConflict; re-read required".into(), + )); + assert!(!state.snapshot().can_save); + assert!(!state.snapshot().has_config); + state.discovering(); + assert!(state.snapshot().error.unwrap().contains("re-read required")); + state.configuration_event(ClientEvent::Configuration(unknown_board())); + assert!(state.configuration().fields[0].draft.is_none()); + assert!(!state.snapshot().can_save); + } + #[test] fn enum_choices_are_projected_and_validated_from_remote_metadata() { let mut config = unknown_board(); @@ -536,4 +1078,77 @@ mod tests { assert!(state.edit(401, &serde_json::json!(20)).is_ok()); assert!(state.edit(401, &serde_json::json!(30)).is_err()); } + + #[test] + fn float_json_and_drafts_preserve_bits_and_apply_only_advertised_rules() { + for value in [ + ConfigValue::F32(0.0), + ConfigValue::F32(-0.0), + ConfigValue::F32(f32::INFINITY), + ConfigValue::F32(f32::NEG_INFINITY), + ConfigValue::F32(f32::from_bits(0x7fc0_0042)), + ConfigValue::F32(f32::from_bits(0xff80_0042)), + ConfigValue::F64(0.0), + ConfigValue::F64(-0.0), + ConfigValue::F64(f64::INFINITY), + ConfigValue::F64(f64::NEG_INFINITY), + ConfigValue::F64(f64::from_bits(0x7ff8_0000_0000_0042)), + ConfigValue::F64(f64::from_bits(0xfff0_0000_0000_0042)), + ConfigValue::F64(f64::MAX), + ConfigValue::F64(f64::MIN_POSITIVE), + ] { + let json = scalar_json(value); + assert!(json.is_string()); + assert_eq!(parse_scalar(value.kind(), &json).unwrap(), value); + let mut config = unknown_board(); + let field = &mut config.fields[0]; + field.kind = value.kind(); + field.minimum = None; + field.maximum = None; + field.value = value; + field.factory = value; + field.active = Some(value); + let state = SharedState::default(); + state.connected("test".into()); + state.configuration_event(ClientEvent::Configuration(config)); + state.edit(401, &json).unwrap(); + assert!(!state.snapshot().can_save, "same bits are not an edit"); + let changed = if value == ConfigValue::F32(0.0) || value == ConfigValue::F64(0.0) { + "-0" + } else { + "0" + }; + state.edit(401, &serde_json::json!(changed)).unwrap(); + assert!(state.snapshot().can_save); + } + let mut field = unknown_board().fields.remove(0); + field.kind = ValueKind::F64; + field.minimum = None; + field.maximum = None; + field.nonzero = true; + assert!(field.validate_value(ConfigValue::F64(-0.0)).is_err()); + assert!(field.validate_value(ConfigValue::F64(0.0)).is_err()); + assert!(field.validate_value(ConfigValue::F64(f64::NAN)).is_ok()); + assert!( + field + .validate_value(ConfigValue::F64(f64::INFINITY)) + .is_ok() + ); + field.minimum = Some(ConfigValue::F64(0.0)); + assert!(field.validate_value(ConfigValue::F64(f64::NAN)).is_err()); + assert!( + field + .validate_value(ConfigValue::F64(f64::INFINITY)) + .is_ok() + ); + field.finite = true; + assert!( + field + .validate_value(ConfigValue::F64(f64::INFINITY)) + .is_err() + ); + assert!(field.validate_value(ConfigValue::F64(1.0)).is_ok()); + let view = field_view(&field, true, None, None); + assert!(view.finite && view.nonzero); + } } diff --git a/firmware/tools/configurator/web/app.js b/firmware/tools/configurator/web/app.js index 7c97ea3..119e904 100644 --- a/firmware/tools/configurator/web/app.js +++ b/firmware/tools/configurator/web/app.js @@ -1,14 +1,27 @@ const q = selector => document.querySelector(selector); -const pendingActions = new Set(); let latestState = null; let latestConfig = null; -let configRevision = -1; let editorStructure = ''; const expandedPackets = new Set(); const packetRows = new Map(); let logSession = null; let latestLogId = -1; +const views = new ViewSession({ + read: () => api('/api/view'), + send: post, + render: view => { renderState(view.state); renderConfig(view.config); }, + error: showError, + permissions: renderPermissions, + edited: (before, after) => { + for (const control of q('#draft-fields').elements) { + if (control.dataset.dirty === 'true' + && sameContext(control.intentContext ?? control.viewContext, before)) { + control.intentContext = after; + } + } + }, +}); function setText(element, value) { const text = value ?? ''; @@ -30,7 +43,9 @@ async function api(path, options = {}) { const value = await responseValue(response); if (!response.ok) { const detail = typeof value === 'object' && value !== null ? value.error : value; - throw Error(detail || `Request failed with HTTP ${response.status}`); + const error = Error(detail || `Request failed with HTTP ${response.status}`); + error.view = value?.view; + throw error; } return value; } @@ -47,41 +62,30 @@ function showError(message) { setText(q('#connect-error'), message); } -async function runAction(id, action) { - pendingActions.add(id); - renderPermissions(); - try { - const value = await action(); - showError(''); - return value; - } catch (error) { - showError(error.message); - return null; - } finally { - pendingActions.delete(id); - renderPermissions(); - } -} - q('#connect-form').addEventListener('submit', event => { event.preventDefault(); - runAction('connect', () => post('/api/connect', {path: q('#path').value})); + views.mutate('/api/connect', {path: q('#path').value}, q('#connect-serial').viewContext); }); q('#disconnect').addEventListener('click', () => { - runAction('disconnect', () => post('/api/disconnect')); + views.mutate('/api/disconnect', {}, q('#disconnect').viewContext); }); q('#draft-fields').addEventListener('submit', event => event.preventDefault()); q('#save').addEventListener('click', () => { - runAction('save', () => post('/api/config/save')); + views.mutate('/api/config/save', {}, q('#save').viewContext); +}); + +q('#reset-saved').addEventListener('click', () => { + views.mutate('/api/config/reset', {}, q('#reset-saved').viewContext); }); function fieldStructure(field) { return [ field.id, field.key, field.label, field.group, field.unit, field.kind, field.minimum, field.maximum, field.factory, field.help, field.options, + field.finite, field.nonzero, ]; } @@ -104,24 +108,37 @@ function fieldControl(field) { if (field.kind === 'bool') { control.type = 'checkbox'; } else { - control.type = 'number'; + // Text retains signed zero and IEEE bit strings for nonfinite values. + control.type = field.kind === 'decimal' ? 'text' : 'number'; control.step = field.kind === 'decimal' ? 'any' : '1'; control.inputMode = field.kind === 'decimal' ? 'decimal' : 'numeric'; if (field.minimum !== null) control.min = String(field.minimum); if (field.maximum !== null) control.max = String(field.maximum); } } + control.addEventListener('input', () => { + if (control.dataset.dirty !== 'true') control.intentContext = control.viewContext; + control.dataset.dirty = 'true'; + renderPermissions(); + }); control.addEventListener('change', async () => { let value = control.value; if (field.kind === 'bool') { value = field.options.length === 0 ? control.checked : control.value === 'true'; } - const action = `edit:${field.id}`; - const config = await runAction(action, () => post('/api/config/edit', { + control.dataset.dirty = 'true'; + const displayed = control.type === 'checkbox' ? control.checked : control.value; + await views.mutate('/api/config/edit', { field_id: field.id, value, - })); - if (config !== null) renderConfig(config); + }, control.intentContext ?? control.viewContext); + if (displayed === (control.type === 'checkbox' ? control.checked : control.value) + && !views.editing(field.id)) { + delete control.dataset.dirty; + delete control.intentContext; + } + if (latestConfig) syncEditorValues(latestConfig.fields); + renderPermissions(); }); return control; } @@ -148,11 +165,19 @@ function buildEditor(fields) { key.textContent = `${field.key} [${field.id}]`; const control = fieldControl(field); label.append(title, key, control); + const values = document.createElement('small'); + values.id = `values-${field.id}`; + const error = document.createElement('small'); + error.id = `error-${field.id}`; + error.className = 'error'; + error.setAttribute('role', 'alert'); + label.append(values, error); const bounds = field.minimum !== null || field.maximum !== null ? `Inclusive bounds: ${field.minimum ?? 'unbounded'} to ${field.maximum ?? 'unbounded'}.` : ''; - const helpText = [field.help, `Factory: ${field.factory}.`, bounds].filter(Boolean).join(' '); + const rules = [field.finite ? 'Must be finite.' : '', field.nonzero ? 'Must be nonzero.' : ''].filter(Boolean).join(' '); + const helpText = [field.help, `Factory: ${field.factory}.`, bounds, rules].filter(Boolean).join(' '); if (helpText) { const help = document.createElement('small'); help.id = `help-${field.id}`; @@ -171,9 +196,18 @@ function syncEditorValues(fields) { for (const field of fields) { const control = q(`[data-field-id="${field.id}"]`); if (!control) continue; + const display = value => { + if (value === null) return 'unknown'; + return field.options.find(option => String(option.value) === String(value))?.label ?? String(value); + }; + setText(q(`#values-${field.id}`), `Boot-active: ${display(field.active)} · Saved: ${display(field.saved)} · Draft: ${field.draft === null ? 'unchanged' : display(field.draft)}`); + setText(q(`#error-${field.id}`), field.error ?? ''); + control.setAttribute('aria-invalid', String(field.error !== null)); + if (views.editing(field.id) || control.dataset.dirty === 'true') continue; + control.viewContext = latestState.context; if (field.kind === 'bool' && field.options.length === 0) { - if (document.activeElement !== control) control.checked = field.value; - } else if (document.activeElement !== control && control.value !== String(field.value)) { + control.checked = field.value; + } else if (control.value !== String(field.value)) { control.value = String(field.value); } } @@ -191,25 +225,32 @@ function renderConfig(config) { q('#restart-guidance').hidden = config.state !== 'restart required'; renderValidation(config.validation); - const structure = JSON.stringify(config.fields.map(fieldStructure)); + const structure = JSON.stringify([ + latestState.context.instance, latestState.context.connection, latestState.context.configuration, + config.fields.map(fieldStructure), + ]); if (structure !== editorStructure) { editorStructure = structure; buildEditor(config.fields); } syncEditorValues(config.fields); + q('#save').viewContext = latestState.context; + q('#reset-saved').viewContext = latestState.context; renderPermissions(); } function renderPermissions() { if (latestState === null) return; - const busy = pendingActions.size !== 0; + const busy = views.busy; + const dirty = [...q('#draft-fields').elements].some(control => control.dataset.dirty === 'true'); q('#path').disabled = latestState.can_disconnect || busy; q('#connect-serial').disabled = latestState.can_disconnect || busy; q('#disconnect').hidden = !latestState.can_disconnect; q('#disconnect').disabled = busy; - q('#save').disabled = !latestState.can_save || busy; + q('#save').disabled = !latestState.can_save || busy || dirty; + q('#reset-saved').disabled = !latestState.can_reset || busy; for (const control of q('#draft-fields').elements) { - control.disabled = !latestState.can_edit || busy; + control.disabled = !latestState.can_edit || (busy && !views.onlyEdits); } } @@ -406,24 +447,23 @@ function renderState(state) { q('#configuration').hidden = !state.has_config; q('#packet-monitor').hidden = !state.can_disconnect; q('#log-monitor').hidden = !state.can_disconnect; + q('#connect-serial').viewContext = state.context; + q('#disconnect').viewContext = state.context; renderPermissions(); } async function refresh() { try { - const [state, packets, logs] = await Promise.all([ - api('/api/state'), + const context = views.view?.state.context; + const [, packets, logs] = await Promise.all([ + views.refresh(), api('/api/packets'), api('/api/logs'), ]); - renderState(state); - renderPackets(packets); - renderLogs(logs); - if (state.config_revision !== configRevision) { - const config = await api('/api/config'); - renderConfig(config); - configRevision = state.config_revision; - q('#configuration').hidden = latestConfig.schema_id === null; + if (context?.connection === views.view?.state.context.connection + && context?.instance === views.view?.state.context.instance) { + renderPackets(packets); + renderLogs(logs); } } catch (error) { showError(error.message); diff --git a/firmware/tools/configurator/web/index.html b/firmware/tools/configurator/web/index.html index da21737..63d6e1b 100644 --- a/firmware/tools/configurator/web/index.html +++ b/firmware/tools/configurator/web/index.html @@ -35,11 +35,15 @@

+

Inputs edit the saved draft. Boot-active values stay in use until restart. + USB remains available for configuration and recovery.

    +
    @@ -69,6 +73,7 @@ + diff --git a/firmware/tools/configurator/web/view.js b/firmware/tools/configurator/web/view.js new file mode 100644 index 0000000..8539acf --- /dev/null +++ b/firmware/tools/configurator/web/view.js @@ -0,0 +1,102 @@ +// Local view ordering and intent binding. Contexts are not credentials. +function sameContext(left, right) { + return ['instance', 'connection', 'configuration', 'draft'].every(key => left?.[key] === right?.[key]); +} + +class ViewSession { + constructor({read, send, render, error, permissions, edited = () => {}}) { + Object.assign(this, {read, send, render, error, permissions, edited}); + this.view = null; + this.queue = []; + this.ticket = 0; + this.acceptedTicket = 0; + this.retired = new Set(); + this.message = ''; + } + + get busy() { return this.queue.length !== 0; } + get onlyEdits() { return this.queue.every(entry => entry.path === '/api/config/edit'); } + editing(id) { + return this.queue.some(entry => entry.body.field_id === id + && entry.context.instance === this.view?.state.context.instance + && entry.context.connection === this.view?.state.context.connection + && entry.context.configuration === this.view?.state.context.configuration); + } + + accept(view, ticket) { + const current = this.view?.state; + const next = view.state; + if (current) { + if (current.context.instance === next.context.instance) { + if (BigInt(next.config_revision) < BigInt(current.config_revision)) return false; + } else { + if (ticket < this.acceptedTicket || this.retired.has(next.context.instance)) return false; + this.retired.add(current.context.instance); + } + } + this.acceptedTicket = Math.max(ticket, this.acceptedTicket); + this.view = view; + this.render(view); + if (this.message) this.error(this.message); + return true; + } + + async refresh() { + const ticket = ++this.ticket; + const view = await this.read(); + return this.accept(view, ticket); + } + + mutate(path, body = {}, context = this.view?.state.context) { + if (!this.view) return Promise.resolve(null); + return new Promise(resolve => { + this.queue.push({path, body, context, resolve}); + this.permissions(); + if (this.queue.length === 1) this.drain(); + }); + } + + async drain() { + while (this.queue.length) { + const entry = this.queue[0]; + const ticket = ++this.ticket; + let result = null; + try { + if (!sameContext(entry.context, this.view.state.context)) { + const error = Error('View changed; queued action discarded. Review the current configuration.'); + error.staleIntent = true; + throw error; + } + const view = await this.send(entry.path, {...entry.body, context: entry.context}); + if (this.accept(view, ticket)) { + this.message = ''; + result = view; + // Only our own accepted edits can advance already queued edits. + // A poll, another editor, a reset, or a new connection cannot rebase intent. + if (entry.path === '/api/config/edit' + && view.state.context.instance === entry.context.instance + && view.state.context.connection === entry.context.connection + && view.state.context.configuration === entry.context.configuration) { + for (const queued of this.queue.slice(1)) { + if (sameContext(queued.context, entry.context)) queued.context = view.state.context; + } + this.edited(entry.context, view.state.context); + } + } + } catch (error) { + const relevant = error.view + ? this.accept(error.view, ticket) + : error.staleIntent || sameContext(entry.context, this.view.state.context); + if (relevant) this.message = error.message; + } finally { + this.queue.shift(); + this.render(this.view); + if (this.message) this.error(this.message); + this.permissions(); + entry.resolve(result); + } + } + } +} + +if (typeof module !== 'undefined') module.exports = {ViewSession, sameContext}; diff --git a/firmware/tools/configurator/web/view.test.cjs b/firmware/tools/configurator/web/view.test.cjs new file mode 100644 index 0000000..31a8aa9 --- /dev/null +++ b/firmware/tools/configurator/web/view.test.cjs @@ -0,0 +1,160 @@ +const {test} = require('node:test'); +const assert = require('node:assert/strict'); +const {ViewSession, sameContext} = require('./view.js'); + +function view(revision, connection = '1', draft = '0', value = 20, instance = 'host') { + return { + state: {config_revision: String(revision), context: {instance, connection, configuration: '1', draft}}, + config: {fields: [{id: 401, value}]}, + }; +} +function deferred() { + let resolve, reject; + const promise = new Promise((yes, no) => { resolve = yes; reject = no; }); + return {promise, resolve, reject}; +} +function harness(send = async () => { throw Error('unexpected mutation'); }, edited = () => {}) { + const reads = [], rendered = [], errors = []; + const session = new ViewSession({ + read: () => { const next = deferred(); reads.push(next); return next.promise; }, + send, edited, render: snapshot => rendered.push(snapshot), error: message => errors.push(message), permissions() {}, + }); + return {session, reads, rendered, errors}; +} + +test('late A view cannot replace displayed B or supply a reset context', async () => { + const sent = []; + const h = harness(async (path, body) => { sent.push({path, body}); return view(4, '2'); }); + h.session.accept(view(1), 0); + const old = h.session.refresh(); + const fresh = h.session.refresh(); + h.reads[1].resolve(view(3, '2')); + await fresh; + h.reads[0].resolve(view(2)); + assert.equal(await old, false); + assert.equal(h.rendered.at(-1).state.context.connection, '2'); + await h.session.mutate('/api/config/reset'); + assert.equal(sent[0].body.context.connection, '2'); +}); + +test('an A action already queued is discarded rather than rebound to B', async () => { + const first = deferred(), sent = []; + const h = harness((path, body) => { sent.push({path, body}); return first.promise; }); + h.session.accept(view(1), 0); + const edit = h.session.mutate('/api/config/edit', {field_id: 401, value: 30}); + const reset = h.session.mutate('/api/config/reset'); + const fresh = h.session.refresh(); + h.reads[0].resolve(view(5, '2')); + await fresh; + first.resolve(view(2, '1', '1', 30)); + await Promise.all([edit, reset]); + assert.equal(sent.length, 1); + assert.equal(h.rendered.at(-1).state.context.connection, '2'); + assert.match(h.errors.at(-1), /queued action discarded/); +}); + +test('an old input change event retains the context of its displayed value', async () => { + const h = harness(); + const a = view(1); + h.session.accept(a, 0); + h.session.accept(view(3, '2'), 1); + assert.equal(await h.session.mutate('/api/config/edit', {field_id: 401, value: 30}, a.state.context), null); + assert.match(h.errors.at(-1), /queued action discarded/); + assert.equal(h.rendered.at(-1).state.context.connection, '2'); +}); + +test('rapid same-input edits serialize and only our accepted draft advances queued context', async () => { + const replies = [deferred(), deferred(), deferred()], sent = []; + let displayedContext = view('9007199254740993').state.context; + const h = harness((path, body) => { sent.push({path, body}); return replies[sent.length - 1].promise; }, + (before, after) => { if (sameContext(displayedContext, before)) displayedContext = after; }); + h.session.accept(view('9007199254740993'), 0); + const first = h.session.mutate('/api/config/edit', {field_id: 401, value: 30}, displayedContext); + const second = h.session.mutate('/api/config/edit', {field_id: 401, value: 40}, displayedContext); + assert.equal(sent.length, 1); + assert.equal(h.session.editing(401), true); + replies[0].resolve(view('9007199254740994', '1', '1', 30)); + await first; + assert.equal(sent.length, 2); + assert.equal(sent[1].body.context.draft, '1'); + assert.equal(sent[1].body.value, 40); + assert.equal(h.session.editing(401), true); + const third = h.session.mutate('/api/config/edit', {field_id: 401, value: 50}, displayedContext); + replies[1].resolve(view('9007199254740995', '1', '2', 40)); + await second; + assert.equal(sent[2].body.context.draft, '2'); + replies[2].resolve(view('9007199254740996', '1', '3', 50)); + await third; + assert.equal(h.session.editing(401), false); + assert.equal(h.rendered.at(-1).config.fields[0].value, 50); + assert.equal(h.session.accept(view('9007199254740994', '1', '1', 30), 100), false); +}); + +test('another editors draft cannot silently rebase queued local edits', async () => { + const reply = deferred(), sent = []; + const h = harness((path, body) => { sent.push(body); return reply.promise; }); + h.session.accept(view(1), 0); + const first = h.session.mutate('/api/config/edit', {field_id: 401, value: 30}); + const next = h.session.mutate('/api/config/edit', {field_id: 401, value: 40}); + const poll = h.session.refresh(); + h.reads[0].resolve(view(4, '1', '3', 50)); + await poll; + reply.resolve(view(2, '1', '1', 30)); + await Promise.all([first, next]); + assert.equal(sent.length, 1); + assert.equal(h.rendered.at(-1).config.fields[0].value, 50); +}); + +test('failed refresh can retry and prior server-instance responses stay retired', async () => { + const h = harness(); + const first = h.session.refresh(); + h.reads[0].reject(Error('HTTP failure')); + await assert.rejects(first, /HTTP failure/); + const retry = h.session.refresh(); + h.reads[1].resolve(view(100)); + assert.equal(await retry, true); + const prior = h.session.refresh(); + const current = h.session.refresh(); + h.reads[3].resolve(view(1, '1', '0', 60, 'new-host')); + await current; + h.reads[2].resolve(view(101)); + assert.equal(await prior, false); + assert.equal(h.rendered.at(-1).config.fields[0].value, 60); +}); + +test('decimal controls retain signed zero and nonfinite bit strings when submitting edits', async () => { + const vm = require('node:vm'); + const fs = require('node:fs'); + const sent = []; + function element() { + return { + dataset: {}, value: '', events: {}, + addEventListener(name, callback) { this.events[name] = callback; }, + }; + } + const pending = () => new Promise(() => {}); + const context = vm.createContext({ + document: {querySelector: element, createElement: element}, + fetch: pending, + ViewSession: class { + refresh() { return pending(); } + editing() { return false; } + async mutate(path, body) { sent.push({path, body}); } + }, + }); + vm.runInContext(fs.readFileSync(`${__dirname}/app.js`, 'utf8'), context); + const field = {id: 401, kind: 'decimal', options: [], minimum: null, maximum: null}; + context.field = field; + const control = vm.runInContext('fieldControl(field)', context); + assert.equal(control.type, 'text'); + for (const value of ['-0', 'bits:0xfff0000000000042', 'bits:0x7f800000']) { + control.value = value; + await control.events.change(); + const serialized = JSON.parse(JSON.stringify(sent.at(-1))); + assert.equal(serialized.body.value, value); + assert.equal(serialized.path, '/api/config/edit'); + } + const original = vm.runInContext('JSON.stringify(fieldStructure(field))', context); + field.finite = true; + assert.notEqual(vm.runInContext('JSON.stringify(fieldStructure(field))', context), original); +}); diff --git a/firmware/vendor/synopsys-usb-otg/Cargo.toml b/firmware/vendor/synopsys-usb-otg/Cargo.toml new file mode 100644 index 0000000..77ed78f --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/Cargo.toml @@ -0,0 +1,26 @@ +[package] +name = "synopsys-usb-otg" +version = "0.4.0" +authors = ["Vadim Kaushan "] +description = "'usb-device' implementation for Synopsys USB OTG IP cores" +edition = "2018" +license = "MIT" +repository = "https://github.com/stm32-rs/synopsys-usb-otg" +readme = "README.md" +keywords = ["no-std", "embedded", "usb"] + +[dependencies] +critical-section = "1.2" +riscv = { version = "0.6.0", optional = true } +cortex-m = { version = "0.7.0", optional = true } +embedded-hal = "0.2.4" +vcell = "0.1.0" +usb-device = "0.3" + +[package.metadata.docs.rs] +features = ['cortex-m', 'fs'] + +[features] +hs = [] +fs = [] +xcvrdly = [] diff --git a/firmware/vendor/synopsys-usb-otg/KAPTAN.md b/firmware/vendor/synopsys-usb-otg/KAPTAN.md new file mode 100644 index 0000000..13784f5 --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/KAPTAN.md @@ -0,0 +1,13 @@ +Based on the crates.io synopsys-usb-otg 0.4.0 package, SHA-256 +`e948d523b316939545d8b21a48c27aef150ce25321b9f95ff7978647a806a6fe`. +Upstream source and MIT license are retained. + +The local change serializes USB operations without masking unrelated interrupts +while hardware completes. F722 full-speed reset and FIFO waits have a clock deadline and +a finite polling limit. Failure permanently aborts the peripheral until restart. +The platform supplies the clock, limits and bounded abort operation. + +From `firmware/`, run `cargo test -p platform-stm32f7 +-p synopsys-usb-otg --lib --locked --offline`. These tests exercise the +production bus against register storage; they do not measure hardware. Upstream +generated RAL doctests refer to another crate and are not included in this gate. diff --git a/firmware/vendor/synopsys-usb-otg/LICENSE b/firmware/vendor/synopsys-usb-otg/LICENSE new file mode 100644 index 0000000..62c9ea0 --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2018-2020 Matti Virkkunen, Vadim Kaushan + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. \ No newline at end of file diff --git a/firmware/vendor/synopsys-usb-otg/README.md b/firmware/vendor/synopsys-usb-otg/README.md new file mode 100644 index 0000000..967387d --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/README.md @@ -0,0 +1,43 @@ +[![crates.io](https://img.shields.io/crates/d/synopsys-usb-otg.svg)](https://crates.io/crates/synopsys-usb-otg) +[![crates.io](https://img.shields.io/crates/v/synopsys-usb-otg.svg)](https://crates.io/crates/synopsys-usb-otg) +[![Build Status](https://travis-ci.com/stm32-rs/synopsys-usb-otg.svg?branch=master)](https://travis-ci.com/stm32-rs/synopsys-usb-otg) + +# `synopsys-usb-otg` + +> [usb-device](https://github.com/mvirkkunen/usb-device) implementation for Synopsys USB OTG IP cores. + +This project is a successor to the [great work](https://github.com/mvirkkunen/stm32f103xx-usb) +started by [@mvirkkunen](https://github.com/mvirkkunen). + +## Supported microcontrollers + +* `STM32F429xx` (OTG_FS and OTG_HS in FS mode) +* `STM32F401xx` +* `STM32F446xx` (OTG_FS and OTG_HS in FS mode) +* `STM32F723xx` (OTG_FS and OTG_HS with internal HS PHY) +* `STM32H7xxxx` (OTG1_HS and OTG2_HS in FS mode, and OTG1_HS with external HS PHY) +* And others... + + +## Usage + +This driver is intended for use through a device hal library. +Such hal library should implement `UsbPeripheral` for the corresponding USB peripheral object. +This trait declares all the peripheral properties that may vary from one device family to the other. +Additionally, hal should pass `fs` of `hs` feature to the `synopsys-usb-otg` library to +define a peripheral type: +* `fs` - for FullSpeed peripherals +* `hs` - for HighSpeed peripherals + +Only one peripheral type can be selected at the moment. + +### ULPI Transciever Delay + +Some ULPI PHYs like the Microchip USB334x series require a delay between the ULPI register write that initiates +the HS Chirp and the subsequent transmit command, otherwise the HS Chirp does not get executed and the deivce +enumerates in FS mode. Some USB Link IP like those in the STM32H7 series support adding this delay to work with the +affected PHYs. Enable the `xcvrdly` feature to add this delay. + +## Examples + +See the [usb-otg-workspace](https://github.com/Disasm/usb-otg-workspace) repo for different device-specific examples. diff --git a/firmware/vendor/synopsys-usb-otg/src/bus.rs b/firmware/vendor/synopsys-usb-otg/src/bus.rs new file mode 100644 index 0000000..506341e --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/bus.rs @@ -0,0 +1,941 @@ +use crate::ral::{ + modify_reg, otg_device, otg_global, otg_global_dieptxfx, otg_pwrclk, read_reg, write_reg, +}; +use crate::transition::{EndpointConfig, EndpointDescriptor}; +use core::{cell::Cell, marker::PhantomData}; +use embedded_hal::blocking::delay::DelayMs; +use usb_device::bus::{PollResult, UsbBusAllocator}; +use usb_device::endpoint::{EndpointAddress, EndpointType}; +use usb_device::{Result, UsbDirection, UsbError}; + +use crate::endpoint::{EndpointIn, EndpointOut}; +use crate::endpoint_memory::{EndpointBufferState, EndpointMemoryAllocator}; +use crate::target::UsbRegisters; +use crate::{PhyType, UsbPeripheral}; +use critical_section::Mutex; + +/// USB peripheral driver for STM32 microcontrollers. +pub struct UsbBus { + peripheral: USB, + regs: UsbRegisters, + state: Mutex>, + allocator: EndpointAllocator, +} + +#[derive(Clone, Copy, PartialEq)] +enum BusState { + Ready, + Busy, + FaultPending, + Offline, +} + +// This private guard never leaves access(). The short state critical sections +// exclude other USB callers. Hardware waits hold only this operation ownership. +struct Operation<'a, USB: UsbPeripheral> { + bus: &'a UsbBus, + restore_interrupt: bool, + active: bool, +} + +impl Operation<'_, USB> { + fn finish(mut self) -> bool { + let healthy = critical_section::with(|cs| { + let state = self.bus.state.borrow(cs); + if state.get() == BusState::Busy { + modify_reg!(otg_global, self.bus.regs.global(), GAHBCFG, + GINT: self.restore_interrupt as u32); + // Complete the mask write before another caller takes ownership. + let _ = read_reg!(otg_global, self.bus.regs.global(), GAHBCFG); + state.set(BusState::Ready); + true + } else { + false + } + }); + if !healthy { + self.bus.abort(); + } + self.active = false; + healthy + } +} + +impl Drop for Operation<'_, USB> { + fn drop(&mut self) { + if self.active { + // An unwound operation may have changed hardware only partially. + self.bus.disable(); + self.bus.abort(); + } + } +} + +impl UsbBus { + /// Constructs a new USB peripheral driver. + pub fn new(peripheral: USB, ep_memory: &'static mut [u32]) -> UsbBusAllocator { + let bus = UsbBus { + peripheral, + regs: UsbRegisters::new::(), + state: Mutex::new(Cell::new(BusState::Ready)), + allocator: EndpointAllocator::new(ep_memory), + }; + + UsbBusAllocator::new(bus) + } + + pub fn free(self) -> USB { + self.peripheral + } + + /// Reports a terminal fault, including an abort awaiting the active owner. + pub fn is_offline(&self) -> bool { + critical_section::with(|cs| { + matches!( + self.state.borrow(cs).get(), + BusState::FaultPending | BusState::Offline + ) + }) + } + + /// Permanently disables USB. Only reconstruction after reset can reconnect it. + pub fn disable(&self) { + let abort = critical_section::with(|cs| { + let state = self.state.borrow(cs); + match state.get() { + BusState::Ready => { + state.set(BusState::FaultPending); + true + } + BusState::Busy => { + state.set(BusState::FaultPending); + false + } + BusState::FaultPending | BusState::Offline => false, + } + }); + if abort { + self.abort(); + } + } + + fn abort(&self) { + // No USB MMIO is allowed while a reset or flush might still be active. + self.peripheral.abort(); + critical_section::with(|cs| self.state.borrow(cs).set(BusState::Offline)); + } + + fn access(&self, f: impl FnOnce(&mut Operation<'_, USB>) -> R) -> Result { + let restore_interrupt = critical_section::with(|cs| { + let state = self.state.borrow(cs); + match state.get() { + BusState::Ready => { + state.set(BusState::Busy); + let enabled = read_reg!(otg_global, self.regs.global(), GAHBCFG, GINT) != 0; + modify_reg!(otg_global, self.regs.global(), GAHBCFG, GINT: 0); + // Read back this same peripheral before releasing exclusion. + let _ = read_reg!(otg_global, self.regs.global(), GAHBCFG); + Ok(enabled) + } + BusState::Busy => Err(UsbError::WouldBlock), + BusState::FaultPending | BusState::Offline => Err(UsbError::InvalidState), + } + })?; + let mut operation = Operation { + bus: self, + restore_interrupt, + active: true, + }; + let result = f(&mut operation); + if operation.finish() { + Ok(result) + } else { + Err(UsbError::InvalidState) + } + } + + fn mutate(&self, f: impl FnOnce(&mut Operation<'_, USB>)) { + if self.access(f) == Err(UsbError::WouldBlock) { + // These trait methods cannot return a retry. A discarded reset or + // address change would leave the USB device and hardware incoherent. + self.disable(); + } + } + + fn wait(&self, ready: impl Fn() -> bool) -> bool { + let start = self.peripheral.now_us(); + for _ in 0..USB::WAIT_POLL_LIMIT { + let now = self.peripheral.now_us(); + if self.is_offline() || now < start || now - start >= u64::from(USB::WAIT_TIMEOUT_US) { + break; + } + if ready() { + return true; + } + } + self.disable(); + false + } + + fn configure_all(&self) -> bool { + let regs = &self.regs; + + // Rx FIFO + // This calculation doesn't correspond to one in a Reference Manual. + // In fact, the required number of words is higher than indicated in RM. + // The following numbers are pessimistic and were figured out empirically. + let rx_fifo_size = if USB::HIGH_SPEED { + self.allocator.memory_allocator.total_rx_buffer_size_words() + 30 + } else { + // F429 requires 35+ words for the (EP0[8] + EP2[64]) setup + // F446 requires 39+ words for the same setup + self.allocator.memory_allocator.total_rx_buffer_size_words() + 30 + }; + write_reg!(otg_global, regs.global(), GRXFSIZ, rx_fifo_size as u32); + let mut fifo_top = rx_fifo_size; + + // Tx FIFO #0 + let fifo_size = self.allocator.memory_allocator.tx_fifo_size_words(0); + + #[cfg(feature = "fs")] + write_reg!(otg_global, regs.global(), DIEPTXF0, + TX0FD: fifo_size as u32, + TX0FSA: fifo_top as u32 + ); + #[cfg(feature = "hs")] + write_reg!(otg_global, regs.global(), GNPTXFSIZ, + TX0FD: fifo_size as u32, + TX0FSA: fifo_top as u32 + ); + + fifo_top += fifo_size; + + // Tx FIFOs + for i in 1..USB::ENDPOINT_COUNT { + let fifo_size = self.allocator.memory_allocator.tx_fifo_size_words(i); + + let dieptxfx = regs.dieptxfx(i); + write_reg!(otg_global_dieptxfx, dieptxfx, DIEPTXFx, + INEPTXFD: fifo_size as u32, + INEPTXSA: fifo_top as u32 + ); + + fifo_top += fifo_size; + } + + assert!(fifo_top as usize <= USB::FIFO_DEPTH_WORDS); + + // Flush Rx & Tx FIFOs + if !self.wait(|| read_reg!(otg_global, regs.global(), GRSTCTL, AHBIDL) != 0) { + return false; + } + modify_reg!(otg_global, regs.global(), GRSTCTL, RXFFLSH: 1, TXFFLSH: 1, TXFNUM: 0x10); + if !self.wait(|| read_reg!(otg_global, regs.global(), GRSTCTL, RXFFLSH, TXFFLSH) == (0, 0)) + { + return false; + } + + for ep in &self.allocator.endpoints_in { + if let Some(ep) = ep { + // enabling EP TX interrupt + modify_reg!(otg_device, regs.device(), DAINTMSK, |v| v + | (0x0001 << ep.address().index())); + + ep.configure(); + } + } + + for ep in &self.allocator.endpoints_out { + if let Some(ep) = ep { + if ep.address().index() == 0 { + // enabling RX interrupt from EP0 + modify_reg!(otg_device, regs.device(), DAINTMSK, |v| v | 0x00010000); + } + + ep.configure(); + } + } + true + } + + fn deconfigure_all(&self) { + let regs = &self.regs; + + // disable interrupts + modify_reg!(otg_device, regs.device(), DAINTMSK, IEPM: 0, OEPM: 0); + + for ep in &self.allocator.endpoints_in { + if let Some(ep) = ep { + ep.deconfigure(); + } + } + + for ep in &self.allocator.endpoints_out { + if let Some(ep) = ep { + ep.deconfigure(); + } + } + } + + pub fn force_reset(&self, delay: &mut impl DelayMs) -> Result<()> { + self.access(|_| { + let regs = &self.regs; + write_reg!(otg_device, regs.device(), DCTL, SDIS: 1); // Soft disconnect + delay.delay_ms(3); + if self.is_offline() { + return; + } + write_reg!(otg_device, regs.device(), DCTL, SDIS: 0); // Soft connect + delay.delay_ms(3); + }) + } + + #[cfg(feature = "hs")] + /// Reads from a ULPI register in an external ULPI PHY. + /// + /// Other USB operations are excluded for the duration of this call. + /// + /// **Panics:** if `phy_type` is not `PhyType::ExternalHighSpeed`. + /// + /// # Example + /// + /// ``` + /// # use synopsys_usb_otg::{UsbPeripheral, UsbBus}; + /// fn read_usb_vid_pid(bus: &UsbBus) -> (u16, u16) { + /// let mut vid: u16 = bus.ulpi_read(0x00) as u16; + /// vid |= (bus.ulpi_read(0x01) as u16) << 8; + /// let mut pid: u16 = bus.ulpi_read(0x02) as u16; + /// pid |= (bus.ulpi_read(0x03) as u16) << 8; + /// (vid, pid) + /// } + /// ``` + pub fn ulpi_read(&self, addr: u8) -> core::result::Result { + if self.peripheral.phy_type() != PhyType::ExternalHighSpeed { + panic!("ulpi_read is only supported with external ULPI PHYs"); + } + + self.access(|_| { + let regs = &self.regs; + + // Begin ULPI register read transaction + modify_reg!(otg_global, regs.global(), PHYCR, + NEW: 1, + RW: 0, + ADDR: addr as u32 + ); + + // ULPI transactions should take less than 1us. 1000 iterations should be enough even for fast MCUs. + let mut timeout = 1000; + while timeout > 0 { + // Wait for transaction to complete + if read_reg!(otg_global, regs.global(), PHYCR, DONE) != 0 { + // Read transaction data + return Ok((read_reg!(otg_global, regs.global(), PHYCR, DATA) & 0xFF) as u8); + } + timeout -= 1; + } + Err(UlpiError::Timeout) + }) + .map_err(|_| UlpiError::Timeout)? + } + + #[cfg(feature = "hs")] + /// Writes to a ULPI register in an external ULPI PHY. + /// + /// Other USB operations are excluded for the duration of this call. + /// + /// **Panics:** if `phy_type` is not `PhyType::ExternalHighSpeed`. + pub fn ulpi_write(&self, addr: u8, data: u8) -> core::result::Result<(), UlpiError> { + if self.peripheral.phy_type() != PhyType::ExternalHighSpeed { + panic!("ulpi_write is only supported with external ULPI PHYs"); + } + + self.access(|_| { + let regs = &self.regs; + + // Begin ULPI register write transaction + modify_reg!(otg_global, regs.global(), PHYCR, + NEW: 1, + RW: 1, + ADDR: addr as u32, + DATA: data as u32 + ); + + // ULPI transactions should take less than 1us. 1000 iterations should be enough even for fast MCUs. + let mut timeout = 1000; + while timeout > 0 { + // Wait for transaction to complete + if read_reg!(otg_global, regs.global(), PHYCR, DONE) != 0 { + return Ok(()); + } + timeout -= 1; + } + Err(UlpiError::Timeout) + }) + .map_err(|_| UlpiError::Timeout)? + } +} + +#[cfg(feature = "hs")] +#[derive(Debug)] +/// Errors that can occur while interfacing with a ULPI PHY. +pub enum UlpiError { + /// The action has timed out. + Timeout, +} + +pub(crate) struct EndpointAllocator { + bitmap_in: u8, + bitmap_out: u8, + endpoints_in: [Option; 9], + endpoints_out: [Option; 9], + memory_allocator: EndpointMemoryAllocator, + _marker: PhantomData, +} + +impl EndpointAllocator { + fn new(memory: &'static mut [u32]) -> Self { + assert!(USB::ENDPOINT_COUNT <= 9); + Self { + bitmap_in: 0, + bitmap_out: 0, + // [None; 9] requires Copy + endpoints_in: [None, None, None, None, None, None, None, None, None], + endpoints_out: [None, None, None, None, None, None, None, None, None], + memory_allocator: EndpointMemoryAllocator::new(memory), + _marker: PhantomData, + } + } + + fn alloc_number(bitmap: &mut u8, number: Option) -> Result { + if let Some(number) = number { + if number as usize >= USB::ENDPOINT_COUNT { + return Err(UsbError::InvalidEndpoint); + } + if *bitmap & (1 << number) == 0 { + *bitmap |= 1 << number; + Ok(number) + } else { + Err(UsbError::InvalidEndpoint) + } + } else { + // Skip EP0 + for number in 1..USB::ENDPOINT_COUNT { + if *bitmap & (1 << number) == 0 { + *bitmap |= 1 << number; + return Ok(number as u8); + } + } + Err(UsbError::EndpointOverflow) + } + } + + fn alloc( + bitmap: &mut u8, + config: &EndpointConfig, + direction: UsbDirection, + ) -> Result { + let number = Self::alloc_number(bitmap, config.number)?; + let address = EndpointAddress::from_parts(number as usize, direction); + Ok(EndpointDescriptor { + address, + ep_type: config.ep_type, + max_packet_size: config.max_packet_size, + interval: config.interval, + }) + } + + fn alloc_in(&mut self, config: &EndpointConfig) -> Result { + let descr = Self::alloc(&mut self.bitmap_in, config, UsbDirection::In)?; + + self.memory_allocator + .allocate_tx_buffer(descr.address.index() as u8, descr.max_packet_size as usize)?; + let ep = EndpointIn::new::(descr); + + Ok(ep) + } + + fn alloc_out(&mut self, config: &EndpointConfig) -> Result { + let descr = Self::alloc(&mut self.bitmap_out, config, UsbDirection::Out)?; + + let buffer = self + .memory_allocator + .allocate_rx_buffer(descr.max_packet_size as usize)?; + let ep = EndpointOut::new::(descr, buffer); + + Ok(ep) + } + + fn alloc_ep( + &mut self, + ep_dir: UsbDirection, + ep_addr: Option, + ep_type: EndpointType, + max_packet_size: u16, + interval: u8, + ) -> Result { + let ep_type = unsafe { core::mem::transmute(ep_type) }; + let number = ep_addr.map(|a| a.index() as u8); + + let config = EndpointConfig { + ep_type, + max_packet_size, + interval, + number, + pair_of: None, + }; + match ep_dir { + UsbDirection::Out => { + let ep = self.alloc_out(&config)?; + let address = ep.address(); + self.endpoints_out[address.index()] = Some(ep); + Ok(address) + } + UsbDirection::In => { + let ep = self.alloc_in(&config)?; + let address = ep.address(); + self.endpoints_in[address.index()] = Some(ep); + Ok(address) + } + } + } +} + +impl usb_device::bus::UsbBus for UsbBus { + fn alloc_ep( + &mut self, + ep_dir: UsbDirection, + ep_addr: Option, + ep_type: EndpointType, + max_packet_size: u16, + interval: u8, + ) -> Result { + self.allocator + .alloc_ep(ep_dir, ep_addr, ep_type, max_packet_size, interval) + } + + fn enable(&mut self) { + if self.is_offline() { + return; + } + // Enable USB_OTG in RCC + USB::enable(); + + let _ = self.access(|operation| { + let regs = &self.regs; + + let core_id = read_reg!(otg_global, regs.global(), CID); + + // Wait for AHB ready + if !self.wait(|| read_reg!(otg_global, regs.global(), GRSTCTL, AHBIDL) != 0) { + return; + } + + // Configure OTG as device + #[cfg(feature = "fs")] + modify_reg!(otg_global, regs.global(), GUSBCFG, + SRPCAP: 0, // SRP capability is not enabled + FDMOD: 1 // Force device mode + ); + #[cfg(feature = "hs")] + modify_reg!(otg_global, regs.global(), GUSBCFG, + SRPCAP: 0, // SRP capability is not enabled + TOCAL: 0x1, + FDMOD: 1 // Force device mode + ); + + // Configure USB PHY + #[cfg(feature = "hs")] + match self.peripheral.phy_type() { + PhyType::InternalFullSpeed => { + // Select FS Embedded PHY + modify_reg!(otg_global, regs.global(), GUSBCFG, PHYSEL: 1); + } + PhyType::InternalHighSpeed => { + // Turn off PHY + modify_reg!(otg_global, regs.global(), GCCFG, PWRDWN: 0); + + // Init The UTMI Interface + modify_reg!(otg_global, regs.global(), GUSBCFG, + TSDPS: 0, + ULPIFSLS: 0, + PHYSEL: 0 // ULPI or UTMI + ); + + // Select VBUS source + modify_reg!(otg_global, regs.global(), GUSBCFG, + ULPIEVBUSD: 0, + ULPIEVBUSI: 0 + ); + + // Select UTMI Interace + //modify_reg!(otg_global, regs.global(), GUSBCFG, ULPISEL: 0); + modify_reg!(otg_global, regs.global(), GUSBCFG, |r| r & !(1 << 4)); + + // This is a secret bit from ST that is not mentioned anywhere except + // the driver code shipped with STM32CubeIDE. + //modify_reg!(otg_global, regs.global(), GCCFG, PHYHSEN: 1); + modify_reg!(otg_global, regs.global(), GCCFG, |r| r | (1 << 23)); + + self.peripheral.setup_internal_hs_phy(); + } + PhyType::ExternalHighSpeed => { + // Turn off embedded PHY + modify_reg!(otg_global, regs.global(), GCCFG, PWRDWN: 0); + + // Init The ULPI Interface + modify_reg!(otg_global, regs.global(), GUSBCFG, + TSDPS: 0, + ULPIFSLS: 0, + PHYSEL: 0 // ULPI or UTMI + ); + + // Select VBUS source + modify_reg!(otg_global, regs.global(), GUSBCFG, + ULPIEVBUSD: 0, + ULPIEVBUSI: 0 + ); + } + } + + // Perform core soft-reset + if !self.wait(|| read_reg!(otg_global, regs.global(), GRSTCTL, AHBIDL) != 0) { + return; + } + modify_reg!(otg_global, regs.global(), GRSTCTL, CSRST: 1); + if !self.wait(|| read_reg!(otg_global, regs.global(), GRSTCTL, CSRST) == 0) { + return; + } + + if self.peripheral.phy_type() == PhyType::InternalFullSpeed { + // Activate the USB Transceiver + modify_reg!(otg_global, regs.global(), GCCFG, PWRDWN: 1); + } + + // Configuring Vbus sense and SOF output + match core_id { + 0x0000_1200 | 0x0000_1100 => { + // F429-like chips have the GCCFG.NOVBUSSENS bit + + //modify_reg!(otg_global, regs.global, GCCFG, NOVBUSSENS: 1); + modify_reg!(otg_global, regs.global(), GCCFG, |r| r | (1 << 21)); + + modify_reg!(otg_global, regs.global(), GCCFG, VBUSASEN: 0, VBUSBSEN: 0, SOFOUTEN: 0); + } + 0x0000_2000 | 0x0000_2100 | 0x0000_2300 | 0x0000_3000 | 0x0000_3100 => { + // F446-like chips have the GCCFG.VBDEN bit with the opposite meaning + + //modify_reg!(otg_global, regs.global, GCCFG, VBDEN: 0); + modify_reg!(otg_global, regs.global(), GCCFG, |r| r & !(1 << 21)); + + // Force B-peripheral session + //modify_reg!(otg_global, regs.global, GOTGCTL, BVALOEN: 1, BVALOVAL: 1); + modify_reg!(otg_global, regs.global(), GOTGCTL, |r| r | (0b11 << 6)); + } + _ => {} + } + + // Enable PHY clock + write_reg!(otg_pwrclk, regs.pwrclk(), PCGCCTL, 0); + + // Soft disconnect device + modify_reg!(otg_device, regs.device(), DCTL, SDIS: 1); + + // Setup USB speed and frame interval + let speed = match (USB::HIGH_SPEED, self.peripheral.phy_type()) { + (false, _) => 0b11, + (true, PhyType::InternalFullSpeed) => 0b11, + (true, PhyType::InternalHighSpeed) => 0b00, + (true, PhyType::ExternalHighSpeed) => 0b00, + }; + modify_reg!(otg_device, regs.device(), DCFG, + PFIVL: 0b00, + DSPD: speed + ); + #[cfg(feature = "xcvrdly")] + modify_reg!(otg_device, regs.device(), DCFG, XCVRDLY: 1); + + // unmask EP interrupts + write_reg!(otg_device, regs.device(), DIEPMSK, XFRCM: 1); + + // unmask core interrupts + write_reg!(otg_global, regs.global(), GINTMSK, + USBRST: 1, ENUMDNEM: 1, + USBSUSPM: 1, WUIM: 1, + IEPINT: 1, RXFLVLM: 1 + ); + + // clear pending interrupts + write_reg!(otg_global, regs.global(), GINTSTS, 0xffffffff); + + // Publish USB interrupts only when the operation finishes healthy. + operation.restore_interrupt = true; + + // connect(true) + modify_reg!(otg_device, regs.device(), DCTL, SDIS: 0); + }); + } + + fn reset(&self) { + self.mutate(|_| { + if self.configure_all() { + modify_reg!(otg_device, self.regs.device(), DCFG, DAD: 0); + } + }); + } + + fn set_device_address(&self, addr: u8) { + self.mutate(|_| { + modify_reg!(otg_device, self.regs.device(), DCFG, DAD: addr as u32); + }); + } + + fn write(&self, ep_addr: EndpointAddress, buf: &[u8]) -> Result { + if !ep_addr.is_in() || ep_addr.index() >= USB::ENDPOINT_COUNT { + return Err(UsbError::InvalidEndpoint); + } + self.access(|_| { + if let Some(ep) = &self.allocator.endpoints_in[ep_addr.index()] { + ep.write(buf).map(|_| buf.len()) + } else { + Err(UsbError::InvalidEndpoint) + } + })? + } + + fn read(&self, ep_addr: EndpointAddress, buf: &mut [u8]) -> Result { + if !ep_addr.is_out() || ep_addr.index() >= USB::ENDPOINT_COUNT { + return Err(UsbError::InvalidEndpoint); + } + + self.access(|_| { + if let Some(ep) = &self.allocator.endpoints_out[ep_addr.index()] { + ep.read(buf) + } else { + Err(UsbError::InvalidEndpoint) + } + })? + } + + fn set_stalled(&self, ep_addr: EndpointAddress, stalled: bool) { + if ep_addr.index() >= USB::ENDPOINT_COUNT { + return; + } + + self.mutate(|_| crate::endpoint::set_stalled(self.regs, ep_addr, stalled)); + } + + fn is_stalled(&self, ep_addr: EndpointAddress) -> bool { + if ep_addr.index() >= USB::ENDPOINT_COUNT { + return true; + } + + self.access(|_| crate::endpoint::is_stalled(self.regs, ep_addr)) + .unwrap_or(true) + } + + fn suspend(&self) { + // Nothing to do here? + } + + fn resume(&self) { + // Nothing to do here? + } + + fn poll(&self) -> PollResult { + self.access(|_| { + let regs = &self.regs; + + let core_id = read_reg!(otg_global, regs.global(), CID); + + let (wakeup, suspend, enum_done, reset, iep, rxflvl) = read_reg!( + otg_global, + regs.global(), + GINTSTS, + WKUPINT, + USBSUSP, + ENUMDNE, + USBRST, + IEPINT, + RXFLVL + ); + + if reset != 0 { + write_reg!(otg_global, regs.global(), GINTSTS, USBRST: 1); + + self.deconfigure_all(); + + // Flush RX + if !self.wait(|| read_reg!(otg_global, regs.global(), GRSTCTL, AHBIDL) != 0) { + return PollResult::None; + } + modify_reg!(otg_global, regs.global(), GRSTCTL, RXFFLSH: 1); + if !self.wait(|| read_reg!(otg_global, regs.global(), GRSTCTL, RXFFLSH) == 0) { + return PollResult::None; + } + } + + if enum_done != 0 { + write_reg!(otg_global, regs.global(), GINTSTS, ENUMDNE: 1); + + let speed = read_reg!(otg_device, regs.device(), DSTS, ENUMSPD); + + // Compute and update TRDT + let trdt; + match speed { + 0b00 => { + // High speed + + // From RM0431 (F72xx), RM0090 (F429), RM0390 (F446) + if self.peripheral.ahb_frequency_hz() >= 30_000_000 { + trdt = 0x9; + } else { + panic!("AHB frequency is too low") + } + } + 0b01 | 0b11 => { + // Full speed + + // From RM0431 (F72xx), RM0090 (F429) + trdt = match self.peripheral.ahb_frequency_hz() { + 0..=14_199_999 => panic!("AHB frequency is too low"), + 14_200_000..=14_999_999 => 0xF, + 15_000_000..=15_999_999 => 0xE, + 16_000_000..=17_199_999 => 0xD, + 17_200_000..=18_499_999 => 0xC, + 18_500_000..=19_999_999 => 0xB, + 20_000_000..=21_799_999 => 0xA, + 21_800_000..=23_999_999 => 0x9, + 24_000_000..=27_499_999 => 0x8, + 27_500_000..=31_999_999 => 0x7, // 27.7..32 in code from CubeIDE + 32_000_000..=u32::MAX => 0x6, + }; + } + _ => unimplemented!(), + } + modify_reg!(otg_global, regs.global(), GUSBCFG, TRDT: trdt); + + PollResult::Reset + } else if wakeup != 0 { + // Clear the interrupt + write_reg!(otg_global, regs.global(), GINTSTS, WKUPINT: 1); + + PollResult::Resume + } else if suspend != 0 { + write_reg!(otg_global, regs.global(), GINTSTS, USBSUSP: 1); + + PollResult::Suspend + } else { + let mut ep_out = 0; + let mut ep_in_complete = 0; + let mut ep_setup = 0; + + use crate::ral::{endpoint_in, endpoint_out}; + + // RXFLVL & IEPINT flags are read-only, there is no need to clear them + if rxflvl != 0 { + let (epnum, data_size, status) = + read_reg!(otg_global, regs.global(), GRXSTSR, EPNUM, BCNT, PKTSTS); + match status { + 0x02 => { + // OUT received + ep_out |= 1 << epnum; + } + 0x06 => { + // SETUP received + // flushing TX if something stuck in control endpoint + let ep = regs.endpoint_in(epnum as usize); + if read_reg!(endpoint_in, ep, DIEPTSIZ, PKTCNT) != 0 { + if !self.wait(|| read_reg!(otg_global, regs.global(), GRSTCTL, AHBIDL) != 0) { + return PollResult::None; + } + modify_reg!(otg_global, regs.global(), GRSTCTL, TXFNUM: epnum, TXFFLSH: 1); + if !self.wait(|| read_reg!(otg_global, regs.global(), GRSTCTL, TXFFLSH) == 0) { + return PollResult::None; + } + } + ep_setup |= 1 << epnum; + } + 0x03 | 0x04 => { + // OUT completed | SETUP completed + // Re-enable the endpoint, F429-like chips only + if core_id == 0x0000_1200 || core_id == 0x0000_1100 { + let ep = regs.endpoint_out(epnum as usize); + modify_reg!(endpoint_out, ep, DOEPCTL, CNAK: 1, EPENA: 1); + } + read_reg!(otg_global, regs.global(), GRXSTSP); // pop GRXSTSP + } + _ => { + read_reg!(otg_global, regs.global(), GRXSTSP); // pop GRXSTSP + } + } + + if status == 0x02 || status == 0x06 { + if let Some(ep) = &self.allocator.endpoints_out[epnum as usize] { + critical_section::with(|cs| { + let mut buffer = ep.buffer.borrow(cs).borrow_mut(); + if buffer.state() == EndpointBufferState::Empty { + read_reg!(otg_global, regs.global(), GRXSTSP); // pop GRXSTSP + + let is_setup = status == 0x06; + buffer + .fill_from_fifo(*regs, data_size as u16, is_setup) + .ok(); + + // Re-enable the endpoint, F446-like chips only + if core_id == 0x0000_2000 + || core_id == 0x0000_2100 + || core_id == 0x0000_2300 + || core_id == 0x0000_3000 + || core_id == 0x0000_3100 + { + let ep = regs.endpoint_out(epnum as usize); + modify_reg!(endpoint_out, ep, DOEPCTL, CNAK: 1, EPENA: 1); + } + } + }); + } + } + } + + if iep != 0 { + for ep in &self.allocator.endpoints_in { + if let Some(ep) = ep { + let ep_regs = regs.endpoint_in(ep.address().index()); + if read_reg!(endpoint_in, ep_regs, DIEPINT, XFRC) != 0 { + write_reg!(endpoint_in, ep_regs, DIEPINT, XFRC: 1); + ep_in_complete |= 1 << ep.address().index(); + } + } + } + } + + for ep in &self.allocator.endpoints_out { + if let Some(ep) = ep { + match ep.buffer_state() { + EndpointBufferState::DataOut => { + ep_out |= 1 << ep.address().index(); + } + EndpointBufferState::DataSetup => { + ep_setup |= 1 << ep.address().index(); + } + EndpointBufferState::Empty => {} + } + } + } + + if (ep_in_complete | ep_out | ep_setup) != 0 { + PollResult::Data { + ep_out, + ep_in_complete, + ep_setup, + } + } else { + PollResult::None + } + } + }).unwrap_or(PollResult::None) + } + + const QUIRK_SET_ADDRESS_BEFORE_STATUS: bool = true; +} diff --git a/firmware/vendor/synopsys-usb-otg/src/endpoint.rs b/firmware/vendor/synopsys-usb-otg/src/endpoint.rs new file mode 100644 index 0000000..6708fd8 --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/endpoint.rs @@ -0,0 +1,242 @@ +use crate::endpoint_memory::{EndpointBuffer, EndpointBufferState}; +use crate::ral::{endpoint0_out, endpoint_in, endpoint_out, modify_reg, read_reg, write_reg}; +use crate::target::{fifo_write, UsbRegisters}; +use crate::transition::EndpointDescriptor; +use crate::UsbPeripheral; +use core::cell::RefCell; +use core::ops::{Deref, DerefMut}; +use critical_section::Mutex; +use usb_device::endpoint::EndpointAddress; +use usb_device::{Result, UsbDirection, UsbError}; + +pub fn set_stalled(usb: UsbRegisters, address: EndpointAddress, stalled: bool) { + critical_section::with(|_| match address.direction() { + UsbDirection::Out => { + let ep = usb.endpoint_out(address.index() as usize); + modify_reg!(endpoint_out, ep, DOEPCTL, STALL: stalled as u32); + } + UsbDirection::In => { + let ep = usb.endpoint_in(address.index() as usize); + modify_reg!(endpoint_in, ep, DIEPCTL, STALL: stalled as u32); + } + }) +} + +pub fn is_stalled(usb: UsbRegisters, address: EndpointAddress) -> bool { + let stall = match address.direction() { + UsbDirection::Out => { + let ep = usb.endpoint_out(address.index()); + read_reg!(endpoint_out, ep, DOEPCTL, STALL) + } + UsbDirection::In => { + let ep = usb.endpoint_in(address.index()); + read_reg!(endpoint_in, ep, DIEPCTL, STALL) + } + }; + stall != 0 +} + +/// Arbitrates access to the endpoint-specific registers and packet buffer memory. +pub struct Endpoint { + descriptor: EndpointDescriptor, + usb: UsbRegisters, +} + +impl Endpoint { + pub fn new(descriptor: EndpointDescriptor) -> Endpoint { + Endpoint { + descriptor, + usb: UsbRegisters::new::(), + } + } + + pub fn address(&self) -> EndpointAddress { + self.descriptor.address + } + + #[inline(always)] + fn index(&self) -> u8 { + self.descriptor.address.index() as u8 + } +} + +pub struct EndpointIn { + common: Endpoint, +} + +impl EndpointIn { + pub fn new(descriptor: EndpointDescriptor) -> EndpointIn { + EndpointIn { + common: Endpoint::new::(descriptor), + } + } + + pub fn configure(&self) { + if self.index() == 0 { + let mpsiz = match self.descriptor.max_packet_size { + 8 => 0b11, + 16 => 0b10, + 32 => 0b01, + 64 => 0b00, + other => panic!("Unsupported EP0 size: {}", other), + }; + + let regs = self.usb.endpoint_in(self.index() as usize); + write_reg!(endpoint_in, regs, DIEPCTL, MPSIZ: mpsiz as u32, SNAK: 1); + write_reg!(endpoint_in, regs, DIEPTSIZ, PKTCNT: 0, XFRSIZ: self.descriptor.max_packet_size as u32); + } else { + let regs = self.usb.endpoint_in(self.index() as usize); + write_reg!(endpoint_in, regs, DIEPCTL, + SNAK: 1, + USBAEP: 1, + EPTYP: self.descriptor.ep_type.to_bm_attributes() as u32, + SD0PID_SEVNFRM: 1, + TXFNUM: self.index() as u32, + MPSIZ: self.descriptor.max_packet_size as u32 + ); + } + } + + pub fn deconfigure(&self) { + let regs = self.usb.endpoint_in(self.index() as usize); + + // deactivating endpoint + modify_reg!(endpoint_in, regs, DIEPCTL, USBAEP: 0); + + // TODO: flushing FIFO + + // disabling endpoint + if read_reg!(endpoint_in, regs, DIEPCTL, EPENA) != 0 && self.index() != 0 { + modify_reg!(endpoint_in, regs, DIEPCTL, EPDIS: 1) + } + + // clean EP interrupts + write_reg!(endpoint_in, regs, DIEPINT, 0xff); + + // TODO: deconfiguring TX FIFO + } + + pub fn write(&self, buf: &[u8]) -> Result<()> { + let ep = self.usb.endpoint_in(self.index() as usize); + if self.index() != 0 && read_reg!(endpoint_in, ep, DIEPCTL, EPENA) != 0 { + return Err(UsbError::WouldBlock); + } + + if buf.len() > self.descriptor.max_packet_size as usize { + return Err(UsbError::BufferOverflow); + } + + if !buf.is_empty() { + // Check for FIFO free space + let size_words = (buf.len() + 3) / 4; + if size_words > read_reg!(endpoint_in, ep, DTXFSTS, INEPTFSAV) as usize { + return Err(UsbError::WouldBlock); + } + } + + #[cfg(feature = "fs")] + write_reg!(endpoint_in, ep, DIEPTSIZ, PKTCNT: 1, XFRSIZ: buf.len() as u32); + #[cfg(feature = "hs")] + write_reg!(endpoint_in, ep, DIEPTSIZ, MCNT: 1, PKTCNT: 1, XFRSIZ: buf.len() as u32); + + modify_reg!(endpoint_in, ep, DIEPCTL, CNAK: 1, EPENA: 1); + + fifo_write(self.usb, self.index(), buf); + + Ok(()) + } +} + +pub struct EndpointOut { + common: Endpoint, + pub(crate) buffer: Mutex>, +} + +impl EndpointOut { + pub fn new( + descriptor: EndpointDescriptor, + buffer: EndpointBuffer, + ) -> EndpointOut { + EndpointOut { + common: Endpoint::new::(descriptor), + buffer: Mutex::new(RefCell::new(buffer)), + } + } + + pub fn configure(&self) { + if self.index() == 0 { + let mpsiz = match self.descriptor.max_packet_size { + 8 => 0b11, + 16 => 0b10, + 32 => 0b01, + 64 => 0b00, + other => panic!("Unsupported EP0 size: {}", other), + }; + + let regs = self.usb.endpoint0_out(); + write_reg!(endpoint0_out, regs, DOEPTSIZ0, STUPCNT: 1, PKTCNT: 1, XFRSIZ: self.descriptor.max_packet_size as u32); + modify_reg!(endpoint0_out, regs, DOEPCTL0, MPSIZ: mpsiz as u32, EPENA: 1, CNAK: 1); + } else { + let regs = self.usb.endpoint_out(self.index() as usize); + write_reg!(endpoint_out, regs, DOEPCTL, + SD0PID_SEVNFRM: 1, + CNAK: 1, + EPENA: 1, + USBAEP: 1, + EPTYP: self.descriptor.ep_type.to_bm_attributes() as u32, + MPSIZ: self.descriptor.max_packet_size as u32 + ); + } + } + + pub fn deconfigure(&self) { + let regs = self.usb.endpoint_out(self.index() as usize); + + // deactivating endpoint + modify_reg!(endpoint_out, regs, DOEPCTL, USBAEP: 0); + + // disabling endpoint + if read_reg!(endpoint_out, regs, DOEPCTL, EPENA) != 0 && self.index() != 0 { + modify_reg!(endpoint_out, regs, DOEPCTL, EPDIS: 1) + } + + // clean EP interrupts + write_reg!(endpoint_out, regs, DOEPINT, 0xff); + } + + pub fn read(&self, buf: &mut [u8]) -> Result { + critical_section::with(|cs| self.buffer.borrow(cs).borrow_mut().read_packet(buf)) + } + + pub fn buffer_state(&self) -> EndpointBufferState { + critical_section::with(|cs| self.buffer.borrow(cs).borrow().state()) + } +} + +impl Deref for EndpointIn { + type Target = Endpoint; + + fn deref(&self) -> &Self::Target { + &self.common + } +} + +impl DerefMut for EndpointIn { + fn deref_mut(&mut self) -> &mut Self::Target { + &mut self.common + } +} + +impl Deref for EndpointOut { + type Target = Endpoint; + + fn deref(&self) -> &Self::Target { + &self.common + } +} + +impl DerefMut for EndpointOut { + fn deref_mut(&mut self) -> &mut Self::Target { + &mut self.common + } +} diff --git a/firmware/vendor/synopsys-usb-otg/src/endpoint_memory.rs b/firmware/vendor/synopsys-usb-otg/src/endpoint_memory.rs new file mode 100644 index 0000000..8614aac --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/endpoint_memory.rs @@ -0,0 +1,185 @@ +#![allow(dead_code)] +use crate::target::{fifo_read_into, UsbRegisters}; +use crate::UsbPeripheral; +use core::marker::PhantomData; +use core::slice; +use usb_device::{Result, UsbError}; +use vcell::VolatileCell; + +#[derive(Eq, PartialEq)] +pub enum EndpointBufferState { + Empty, + DataOut, + DataSetup, +} + +pub struct EndpointBuffer { + buffer: &'static mut [VolatileCell], + data_size: u16, + has_data: bool, + is_setup: bool, +} + +impl EndpointBuffer { + pub fn new(buffer: &'static mut [u32]) -> Self { + Self { + buffer: unsafe { &mut *(buffer as *mut [u32] as *mut [VolatileCell]) }, + data_size: 0, + has_data: false, + is_setup: false, + } + } + + pub fn read_packet(&mut self, mut buf: &mut [u8]) -> Result { + if !self.has_data { + return Err(UsbError::WouldBlock); + } + + let data_size = self.data_size as usize; + + if buf.len() < data_size { + return Err(UsbError::BufferOverflow); + } + + let mut index = 0; + let mut current_size = data_size; + while current_size >= 4 { + let word = self.buffer[index].get(); + index += 1; + + let bytes = word.to_ne_bytes(); + buf[..4].copy_from_slice(&bytes); + buf = &mut buf[4..]; + + current_size -= 4; + } + if current_size > 0 { + let word = self.buffer[index].get(); + let bytes = word.to_ne_bytes(); + buf[..current_size].copy_from_slice(&bytes[..current_size]); + } + + self.has_data = false; + + Ok(data_size) + } + + pub fn fill_from_fifo( + &mut self, + usb: UsbRegisters, + data_size: u16, + is_setup: bool, + ) -> Result<()> { + if self.has_data { + return Err(UsbError::WouldBlock); + } + + if data_size as usize > self.capacity() { + return Err(UsbError::BufferOverflow); + } + + let words = (data_size as usize + 3) / 4; + fifo_read_into(usb, &self.buffer[..words]); + + self.is_setup = is_setup; + self.data_size = data_size; + self.has_data = true; + + Ok(()) + } + + pub fn state(&self) -> EndpointBufferState { + if self.has_data { + if self.is_setup { + EndpointBufferState::DataSetup + } else { + EndpointBufferState::DataOut + } + } else { + EndpointBufferState::Empty + } + } + + pub fn capacity(&self) -> usize { + self.buffer.len() * 4 + } +} + +impl Default for EndpointBuffer { + fn default() -> Self { + EndpointBuffer::new(&mut []) + } +} + +pub struct EndpointMemoryAllocator { + next_free_offset: usize, + max_size_words: usize, + memory: &'static mut [u32], + tx_fifo_size_words: [u16; 9], + _marker: PhantomData, +} + +impl EndpointMemoryAllocator { + pub fn new(memory: &'static mut [u32]) -> Self { + Self { + next_free_offset: 0, + max_size_words: 0, + memory, + tx_fifo_size_words: [0; 9], + _marker: PhantomData, + } + } + + pub fn allocate_rx_buffer(&mut self, size: usize) -> Result { + let size_words = (size + 3) / 4; + + let offset = self.next_free_offset; + if offset + size_words > self.memory.len() { + return Err(UsbError::EndpointMemoryOverflow); + } + + self.next_free_offset += size_words; + self.max_size_words = core::cmp::max(self.max_size_words, size_words); + + let buffer = unsafe { + let ptr = self.memory.as_mut_ptr().add(offset); + slice::from_raw_parts_mut(ptr, size_words) + }; + Ok(EndpointBuffer::new(buffer)) + } + + pub fn allocate_tx_buffer(&mut self, ep_number: u8, size: usize) -> Result<()> { + let ep_number = ep_number as usize; + assert!(ep_number < self.tx_fifo_size_words.len()); + + if self.tx_fifo_size_words[ep_number] != 0 { + return Err(UsbError::InvalidEndpoint); + } + + let used = 30 + + self.total_rx_buffer_size_words() as usize + + self.tx_fifo_size_words.iter().sum::() as usize; + + let size_words = core::cmp::max((size + 3) / 4, 16); + if (used + size_words) > USB::FIFO_DEPTH_WORDS { + return Err(UsbError::EndpointMemoryOverflow); + } + + self.tx_fifo_size_words[ep_number] = size_words as u16; + + Ok(()) + } + + /// Returns the size of memory allocated for OUT endpoints in words + pub fn total_rx_buffer_size_words(&self) -> u16 { + self.next_free_offset as u16 + } + + pub fn tx_fifo_size_words(&self, ep_number: usize) -> u16 { + self.tx_fifo_size_words[ep_number] + } + + pub fn max_buffer_size_words(&self) -> usize { + self.max_size_words + } +} diff --git a/firmware/vendor/synopsys-usb-otg/src/lib.rs b/firmware/vendor/synopsys-usb-otg/src/lib.rs new file mode 100644 index 0000000..49e98f9 --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/lib.rs @@ -0,0 +1,90 @@ +//! USB peripheral driver for Synopsys USB OTG peripherals. + +#![no_std] + +#[cfg(all(feature = "fs", feature = "hs"))] +compile_error!("choose only one USB mode"); + +#[cfg(not(any(feature = "fs", feature = "hs")))] +compile_error!("select USB mode feature (fs/hs)"); + +mod endpoint; +mod endpoint_memory; + +mod target; + +/// USB peripheral driver. +pub mod bus; + +pub use crate::bus::UsbBus; + +mod ral; +mod transition; + +/// A trait for device-specific USB peripherals. Implement this to add support for a new hardware +/// platform. Peripherals that have this trait must have the same register block as STM32 USB OTG +/// peripherals. +pub unsafe trait UsbPeripheral: Send + Sync { + /// Pointer to the register block + const REGISTERS: *const (); + + /// true for High Speed variants of the peripheral, false for Full Speed + const HIGH_SPEED: bool; + + /// FIFO size in 32-bit words + const FIFO_DEPTH_WORDS: usize; + + /// Number of (bidirectional) endpoints + const ENDPOINT_COUNT: usize; + + /// Enables USB device on its peripheral bus + fn enable(); + + /// AHB frequency in hertz + fn ahb_frequency_hz(&self) -> u32; + + /// Maximum elapsed time for one register wait, in microseconds. + const WAIT_TIMEOUT_US: u32; + + /// Independent finite limit if the clock fails to advance. + const WAIT_POLL_LIMIT: u32; + + /// Reads a monotonic clock which advances without USB or timer interrupts. + /// This must be bounded and must not block or panic. + fn now_us(&self) -> u64; + + /// Permanently stops this peripheral after failure. + /// + /// This must be bounded, synchronous, and must not panic, wait, reenter this bus, + /// or access USB registers. It runs without the driver's critical section. + /// Holding the peripheral in its external reset is a suitable implementation. + fn abort(&self); + + /// Returns PHY type that should be used for USB peripheral + fn phy_type(&self) -> PhyType { + PhyType::InternalFullSpeed + } + + /// Performs initial setup of the internal high-speed PHY + /// + /// This function should turn on LDO and PLL and wait for PHY clock to become stable. + fn setup_internal_hs_phy(&self) {} +} + +/// USB PHY type +#[derive(Copy, Clone, Debug, Eq, PartialEq)] +pub enum PhyType { + /// Internal Full-Speed PHY + /// + /// Available on most High-Speed peripherals. + InternalFullSpeed, + /// Internal High-Speed PHY + /// + /// Available on a few STM32 chips. + InternalHighSpeed, + /// External ULPI High-Speed PHY + ExternalHighSpeed, +} + +#[cfg(test)] +mod tests; diff --git a/firmware/vendor/synopsys-usb-otg/src/ral/instances/mod.rs b/firmware/vendor/synopsys-usb-otg/src/ral/instances/mod.rs new file mode 100644 index 0000000..5e28518 --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/ral/instances/mod.rs @@ -0,0 +1,8 @@ +#![allow(unused)] +pub mod otg_fs_device_f401_f405_f407_f411_f427_f429; +pub mod otg_fs_global_f401_f405_f407_f411_f427_f429; +pub mod otg_fs_host; +pub mod otg_hs_device; +pub mod otg_hs_global; +pub mod otg_hs_host; +pub mod otg_s_pwrclk; diff --git a/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_fs_device_f401_f405_f407_f411_f427_f429.rs b/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_fs_device_f401_f405_f407_f411_f427_f429.rs new file mode 100644 index 0000000..b81a59c --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_fs_device_f401_f405_f407_f411_f427_f429.rs @@ -0,0 +1,83 @@ +#![allow(non_snake_case, non_upper_case_globals)] +#![allow(non_camel_case_types)] +//! USB on the go full speed +//! +//! Used by: stm32f401, stm32f405, stm32f407, stm32f411, stm32f427, stm32f429 + +#[cfg(not(feature = "nosync"))] +pub use super::super::peripherals::otg_fs_device_v1::Instance; +pub use super::super::peripherals::otg_fs_device_v1::{RegisterBlock, ResetValues}; +pub use super::super::peripherals::otg_fs_device_v1::{ + DAINT, DAINTMSK, DCFG, DCTL, DIEPCTL0, DIEPCTL1, DIEPCTL2, DIEPCTL3, DIEPEMPMSK, DIEPINT0, + DIEPINT1, DIEPINT2, DIEPINT3, DIEPMSK, DIEPTSIZ0, DIEPTSIZ1, DIEPTSIZ2, DIEPTSIZ3, DOEPCTL0, + DOEPCTL1, DOEPCTL2, DOEPCTL3, DOEPINT0, DOEPINT1, DOEPINT2, DOEPINT3, DOEPMSK, DOEPTSIZ0, + DOEPTSIZ1, DOEPTSIZ2, DOEPTSIZ3, DSTS, DTXFSTS0, DTXFSTS1, DTXFSTS2, DTXFSTS3, DVBUSDIS, + DVBUSPULSE, +}; + +/// Access functions for the OTG_FS_DEVICE peripheral instance +pub mod OTG_FS_DEVICE { + use super::ResetValues; + + #[cfg(not(feature = "nosync"))] + use super::Instance; + + #[cfg(not(feature = "nosync"))] + const INSTANCE: Instance = Instance { + addr: 0x50000800, + _marker: ::core::marker::PhantomData, + }; + + /// Reset values for each field in OTG_FS_DEVICE + pub const reset: ResetValues = ResetValues { + DCFG: 0x02200000, + DCTL: 0x00000000, + DSTS: 0x00000010, + DIEPMSK: 0x00000000, + DOEPMSK: 0x00000000, + DAINT: 0x00000000, + DAINTMSK: 0x00000000, + DVBUSDIS: 0x000017D7, + DVBUSPULSE: 0x000005B8, + DIEPEMPMSK: 0x00000000, + DIEPCTL0: 0x00000000, + DIEPCTL1: 0x00000000, + DIEPCTL2: 0x00000000, + DIEPCTL3: 0x00000000, + DOEPCTL0: 0x00008000, + DOEPCTL1: 0x00000000, + DOEPCTL2: 0x00000000, + DOEPCTL3: 0x00000000, + DIEPINT0: 0x00000080, + DIEPINT1: 0x00000080, + DIEPINT2: 0x00000080, + DIEPINT3: 0x00000080, + DOEPINT0: 0x00000080, + DOEPINT1: 0x00000080, + DOEPINT2: 0x00000080, + DOEPINT3: 0x00000080, + DIEPTSIZ0: 0x00000000, + DOEPTSIZ0: 0x00000000, + DIEPTSIZ1: 0x00000000, + DIEPTSIZ2: 0x00000000, + DIEPTSIZ3: 0x00000000, + DTXFSTS0: 0x00000000, + DTXFSTS1: 0x00000000, + DTXFSTS2: 0x00000000, + DTXFSTS3: 0x00000000, + DOEPTSIZ1: 0x00000000, + DOEPTSIZ2: 0x00000000, + DOEPTSIZ3: 0x00000000, + }; +} + +/// Raw pointer to OTG_FS_DEVICE +/// +/// Dereferencing this is unsafe because you are not ensured unique +/// access to the peripheral, so you may encounter data races with +/// other users of this peripheral. It is up to you to ensure you +/// will not cause data races. +/// +/// This constant is provided for ease of use in unsafe code: you can +/// simply call for example `write_reg!(gpio, GPIOA, ODR, 1);`. +pub const OTG_FS_DEVICE: *const RegisterBlock = 0x50000800 as *const _; diff --git a/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_fs_global_f401_f405_f407_f411_f427_f429.rs b/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_fs_global_f401_f405_f407_f411_f427_f429.rs new file mode 100644 index 0000000..c6422e0 --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_fs_global_f401_f405_f407_f411_f427_f429.rs @@ -0,0 +1,60 @@ +#![allow(non_snake_case, non_upper_case_globals)] +#![allow(non_camel_case_types)] +//! USB on the go full speed +//! +//! Used by: stm32f401, stm32f405, stm32f407, stm32f411, stm32f427, stm32f429 + +#[cfg(not(feature = "nosync"))] +pub use super::super::peripherals::otg_fs_global_v1::Instance; +pub use super::super::peripherals::otg_fs_global_v1::{RegisterBlock, ResetValues}; +pub use super::super::peripherals::otg_fs_global_v1::{ + CID, DIEPTXF0, DIEPTXF1, DIEPTXF2, DIEPTXF3, GAHBCFG, GCCFG, GINTMSK, GINTSTS, GNPTXSTS, + GOTGCTL, GOTGINT, GRSTCTL, GRXFSIZ, GRXSTSP, GRXSTSR, GUSBCFG, HPTXFSIZ, +}; + +/// Access functions for the OTG_FS_GLOBAL peripheral instance +pub mod OTG_FS_GLOBAL { + use super::ResetValues; + + #[cfg(not(feature = "nosync"))] + use super::Instance; + + #[cfg(not(feature = "nosync"))] + const INSTANCE: Instance = Instance { + addr: 0x50000000, + _marker: ::core::marker::PhantomData, + }; + + /// Reset values for each field in OTG_FS_GLOBAL + pub const reset: ResetValues = ResetValues { + GOTGCTL: 0x00000800, + GOTGINT: 0x00000000, + GAHBCFG: 0x00000000, + GUSBCFG: 0x00000A00, + GRSTCTL: 0x20000000, + GINTSTS: 0x04000020, + GINTMSK: 0x00000000, + GRXSTSR: 0x00000000, + GRXFSIZ: 0x00000200, + DIEPTXF0: 0x00000200, + GNPTXSTS: 0x00080200, + GCCFG: 0x00000000, + CID: 0x00001000, + HPTXFSIZ: 0x02000600, + DIEPTXF1: 0x02000400, + DIEPTXF2: 0x02000400, + DIEPTXF3: 0x02000400, + GRXSTSP: 0x00000000, + }; +} + +/// Raw pointer to OTG_FS_GLOBAL +/// +/// Dereferencing this is unsafe because you are not ensured unique +/// access to the peripheral, so you may encounter data races with +/// other users of this peripheral. It is up to you to ensure you +/// will not cause data races. +/// +/// This constant is provided for ease of use in unsafe code: you can +/// simply call for example `write_reg!(gpio, GPIOA, ODR, 1);`. +pub const OTG_FS_GLOBAL: *const RegisterBlock = 0x50000000 as *const _; diff --git a/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_fs_host.rs b/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_fs_host.rs new file mode 100644 index 0000000..4df4958 --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_fs_host.rs @@ -0,0 +1,83 @@ +#![allow(non_snake_case, non_upper_case_globals)] +#![allow(non_camel_case_types)] +//! USB on the go full speed +//! +//! Used by: stm32f401, stm32f405, stm32f407, stm32f411, stm32f412, stm32f413, stm32f427, stm32f429, stm32f446, stm32f469 + +#[cfg(not(feature = "nosync"))] +pub use super::super::peripherals::otg_fs_host::Instance; +pub use super::super::peripherals::otg_fs_host::{RegisterBlock, ResetValues}; +pub use super::super::peripherals::otg_fs_host::{ + HAINT, HAINTMSK, HCCHAR0, HCCHAR1, HCCHAR2, HCCHAR3, HCCHAR4, HCCHAR5, HCCHAR6, HCCHAR7, HCFG, + HCINT0, HCINT1, HCINT2, HCINT3, HCINT4, HCINT5, HCINT6, HCINT7, HCINTMSK0, HCINTMSK1, + HCINTMSK2, HCINTMSK3, HCINTMSK4, HCINTMSK5, HCINTMSK6, HCINTMSK7, HCTSIZ0, HCTSIZ1, HCTSIZ2, + HCTSIZ3, HCTSIZ4, HCTSIZ5, HCTSIZ6, HCTSIZ7, HFIR, HFNUM, HPRT, HPTXSTS, +}; + +/// Access functions for the OTG_FS_HOST peripheral instance +pub mod OTG_FS_HOST { + use super::ResetValues; + + #[cfg(not(feature = "nosync"))] + use super::Instance; + + #[cfg(not(feature = "nosync"))] + const INSTANCE: Instance = Instance { + addr: 0x50000400, + _marker: ::core::marker::PhantomData, + }; + + /// Reset values for each field in OTG_FS_HOST + pub const reset: ResetValues = ResetValues { + HCFG: 0x00000000, + HFIR: 0x0000EA60, + HFNUM: 0x00003FFF, + HPTXSTS: 0x00080100, + HAINT: 0x00000000, + HAINTMSK: 0x00000000, + HPRT: 0x00000000, + HCCHAR0: 0x00000000, + HCCHAR1: 0x00000000, + HCCHAR2: 0x00000000, + HCCHAR3: 0x00000000, + HCCHAR4: 0x00000000, + HCCHAR5: 0x00000000, + HCCHAR6: 0x00000000, + HCCHAR7: 0x00000000, + HCINT0: 0x00000000, + HCINT1: 0x00000000, + HCINT2: 0x00000000, + HCINT3: 0x00000000, + HCINT4: 0x00000000, + HCINT5: 0x00000000, + HCINT6: 0x00000000, + HCINT7: 0x00000000, + HCINTMSK0: 0x00000000, + HCINTMSK1: 0x00000000, + HCINTMSK2: 0x00000000, + HCINTMSK3: 0x00000000, + HCINTMSK4: 0x00000000, + HCINTMSK5: 0x00000000, + HCINTMSK6: 0x00000000, + HCINTMSK7: 0x00000000, + HCTSIZ0: 0x00000000, + HCTSIZ1: 0x00000000, + HCTSIZ2: 0x00000000, + HCTSIZ3: 0x00000000, + HCTSIZ4: 0x00000000, + HCTSIZ5: 0x00000000, + HCTSIZ6: 0x00000000, + HCTSIZ7: 0x00000000, + }; +} + +/// Raw pointer to OTG_FS_HOST +/// +/// Dereferencing this is unsafe because you are not ensured unique +/// access to the peripheral, so you may encounter data races with +/// other users of this peripheral. It is up to you to ensure you +/// will not cause data races. +/// +/// This constant is provided for ease of use in unsafe code: you can +/// simply call for example `write_reg!(gpio, GPIOA, ODR, 1);`. +pub const OTG_FS_HOST: *const RegisterBlock = 0x50000400 as *const _; diff --git a/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_hs_device.rs b/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_hs_device.rs new file mode 100644 index 0000000..f15e237 --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_hs_device.rs @@ -0,0 +1,109 @@ +#![allow(non_snake_case, non_upper_case_globals)] +#![allow(non_camel_case_types)] +//! USB on the go high speed +//! +//! Used by: stm32f405, stm32f407, stm32f427, stm32f429, stm32f446 + +#[cfg(not(feature = "nosync"))] +pub use super::super::peripherals::otg_hs_device::Instance; +pub use super::super::peripherals::otg_hs_device::{RegisterBlock, ResetValues}; +pub use super::super::peripherals::otg_hs_device::{ + DAINT, DAINTMSK, DCFG, DCTL, DEACHINT, DEACHINTMSK, DIEPCTL0, DIEPCTL1, DIEPCTL2, DIEPCTL3, + DIEPCTL4, DIEPCTL5, DIEPDMA1, DIEPDMA2, DIEPDMA3, DIEPDMA4, DIEPDMA5, DIEPEACHMSK1, DIEPEMPMSK, + DIEPINT0, DIEPINT1, DIEPINT2, DIEPINT3, DIEPINT4, DIEPINT5, DIEPMSK, DIEPTSIZ0, DIEPTSIZ1, + DIEPTSIZ2, DIEPTSIZ3, DIEPTSIZ4, DIEPTSIZ5, DOEPCTL0, DOEPCTL1, DOEPCTL2, DOEPCTL3, DOEPCTL4, + DOEPCTL5, DOEPEACHMSK1, DOEPINT0, DOEPINT1, DOEPINT2, DOEPINT3, DOEPINT4, DOEPINT5, DOEPMSK, + DOEPTSIZ0, DOEPTSIZ1, DOEPTSIZ2, DOEPTSIZ3, DOEPTSIZ4, DOEPTSIZ5, DSTS, DTHRCTL, DTXFSTS0, + DTXFSTS1, DTXFSTS2, DTXFSTS3, DTXFSTS4, DTXFSTS5, DVBUSDIS, DVBUSPULSE, +}; + +/// Access functions for the OTG_HS_DEVICE peripheral instance +pub mod OTG_HS_DEVICE { + use super::ResetValues; + + #[cfg(not(feature = "nosync"))] + use super::Instance; + + #[cfg(not(feature = "nosync"))] + const INSTANCE: Instance = Instance { + addr: 0x40040800, + _marker: ::core::marker::PhantomData, + }; + + /// Reset values for each field in OTG_HS_DEVICE + pub const reset: ResetValues = ResetValues { + DCFG: 0x02200000, + DCTL: 0x00000000, + DSTS: 0x00000010, + DIEPMSK: 0x00000000, + DOEPMSK: 0x00000000, + DAINT: 0x00000000, + DAINTMSK: 0x00000000, + DVBUSDIS: 0x000017D7, + DVBUSPULSE: 0x000005B8, + DTHRCTL: 0x00000000, + DIEPEMPMSK: 0x00000000, + DEACHINT: 0x00000000, + DEACHINTMSK: 0x00000000, + DIEPEACHMSK1: 0x00000000, + DOEPEACHMSK1: 0x00000000, + DIEPCTL0: 0x00000000, + DIEPCTL1: 0x00000000, + DIEPCTL2: 0x00000000, + DIEPCTL3: 0x00000000, + DIEPCTL4: 0x00000000, + DIEPCTL5: 0x00000000, + DIEPINT0: 0x00000080, + DIEPINT1: 0x00000080, + DIEPINT2: 0x00000080, + DIEPINT3: 0x00000080, + DIEPINT4: 0x00000080, + DIEPINT5: 0x00000080, + DIEPTSIZ0: 0x00000000, + DIEPDMA1: 0x00000000, + DIEPDMA2: 0x00000000, + DIEPDMA3: 0x00000000, + DIEPDMA4: 0x00000000, + DIEPDMA5: 0x00000000, + DTXFSTS0: 0x00000000, + DTXFSTS1: 0x00000000, + DTXFSTS2: 0x00000000, + DTXFSTS3: 0x00000000, + DTXFSTS4: 0x00000000, + DTXFSTS5: 0x00000000, + DIEPTSIZ1: 0x00000000, + DIEPTSIZ2: 0x00000000, + DIEPTSIZ3: 0x00000000, + DIEPTSIZ4: 0x00000000, + DIEPTSIZ5: 0x00000000, + DOEPCTL0: 0x00008000, + DOEPCTL1: 0x00000000, + DOEPCTL2: 0x00000000, + DOEPCTL3: 0x00000000, + DOEPCTL4: 0x00000000, + DOEPCTL5: 0x00000000, + DOEPINT0: 0x00000080, + DOEPINT1: 0x00000080, + DOEPINT2: 0x00000080, + DOEPINT3: 0x00000080, + DOEPINT4: 0x00000080, + DOEPINT5: 0x00000080, + DOEPTSIZ0: 0x00000000, + DOEPTSIZ1: 0x00000000, + DOEPTSIZ2: 0x00000000, + DOEPTSIZ3: 0x00000000, + DOEPTSIZ4: 0x00000000, + DOEPTSIZ5: 0x00000000, + }; +} + +/// Raw pointer to OTG_HS_DEVICE +/// +/// Dereferencing this is unsafe because you are not ensured unique +/// access to the peripheral, so you may encounter data races with +/// other users of this peripheral. It is up to you to ensure you +/// will not cause data races. +/// +/// This constant is provided for ease of use in unsafe code: you can +/// simply call for example `write_reg!(gpio, GPIOA, ODR, 1);`. +pub const OTG_HS_DEVICE: *const RegisterBlock = 0x40040800 as *const _; diff --git a/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_hs_global.rs b/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_hs_global.rs new file mode 100644 index 0000000..ae37b65 --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_hs_global.rs @@ -0,0 +1,64 @@ +#![allow(non_snake_case, non_upper_case_globals)] +#![allow(non_camel_case_types)] +//! USB on the go high speed +//! +//! Used by: stm32f405, stm32f407, stm32f427, stm32f429, stm32f446 + +#[cfg(not(feature = "nosync"))] +pub use super::super::peripherals::otg_hs_global::Instance; +pub use super::super::peripherals::otg_hs_global::{RegisterBlock, ResetValues}; +pub use super::super::peripherals::otg_hs_global::{ + CID, DIEPTXF1, DIEPTXF2, DIEPTXF3, DIEPTXF4, DIEPTXF5, GAHBCFG, GCCFG, GINTMSK, GINTSTS, + GNPTXFSIZ, GNPTXSTS, GOTGCTL, GOTGINT, GRSTCTL, GRXFSIZ, GRXSTSP, GRXSTSR, GUSBCFG, HPTXFSIZ, + PHYCR, +}; + +/// Access functions for the OTG_HS_GLOBAL peripheral instance +pub mod OTG_HS_GLOBAL { + use super::ResetValues; + + #[cfg(not(feature = "nosync"))] + use super::Instance; + + #[cfg(not(feature = "nosync"))] + const INSTANCE: Instance = Instance { + addr: 0x40040000, + _marker: ::core::marker::PhantomData, + }; + + /// Reset values for each field in OTG_HS_GLOBAL + pub const reset: ResetValues = ResetValues { + GOTGCTL: 0x00000800, + GOTGINT: 0x00000000, + GAHBCFG: 0x00000000, + GUSBCFG: 0x00000A00, + GRSTCTL: 0x20000000, + GINTSTS: 0x04000020, + GINTMSK: 0x00000000, + GRXSTSR: 0x00000000, + GRXSTSP: 0x00000000, + GRXFSIZ: 0x00000200, + GNPTXFSIZ: 0x00000200, + GNPTXSTS: 0x00080200, + PHYCR: 0x00000000, + GCCFG: 0x00000000, + CID: 0x00001200, + HPTXFSIZ: 0x02000600, + DIEPTXF1: 0x02000400, + DIEPTXF2: 0x02000400, + DIEPTXF3: 0x02000400, + DIEPTXF4: 0x02000400, + DIEPTXF5: 0x02000400, + }; +} + +/// Raw pointer to OTG_HS_GLOBAL +/// +/// Dereferencing this is unsafe because you are not ensured unique +/// access to the peripheral, so you may encounter data races with +/// other users of this peripheral. It is up to you to ensure you +/// will not cause data races. +/// +/// This constant is provided for ease of use in unsafe code: you can +/// simply call for example `write_reg!(gpio, GPIOA, ODR, 1);`. +pub const OTG_HS_GLOBAL: *const RegisterBlock = 0x40040000 as *const _; diff --git a/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_hs_host.rs b/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_hs_host.rs new file mode 100644 index 0000000..d513ad8 --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_hs_host.rs @@ -0,0 +1,127 @@ +#![allow(non_snake_case, non_upper_case_globals)] +#![allow(non_camel_case_types)] +//! USB on the go high speed +//! +//! Used by: stm32f405, stm32f407, stm32f427, stm32f429, stm32f446, stm32f469 + +#[cfg(not(feature = "nosync"))] +pub use super::super::peripherals::otg_hs_host::Instance; +pub use super::super::peripherals::otg_hs_host::{RegisterBlock, ResetValues}; +pub use super::super::peripherals::otg_hs_host::{ + HAINT, HAINTMSK, HCCHAR0, HCCHAR1, HCCHAR10, HCCHAR11, HCCHAR2, HCCHAR3, HCCHAR4, HCCHAR5, + HCCHAR6, HCCHAR7, HCCHAR8, HCCHAR9, HCDMA0, HCDMA1, HCDMA10, HCDMA11, HCDMA2, HCDMA3, HCDMA4, + HCDMA5, HCDMA6, HCDMA7, HCDMA8, HCDMA9, HCFG, HCINT0, HCINT1, HCINT10, HCINT11, HCINT2, HCINT3, + HCINT4, HCINT5, HCINT6, HCINT7, HCINT8, HCINT9, HCINTMSK0, HCINTMSK1, HCINTMSK10, HCINTMSK11, + HCINTMSK2, HCINTMSK3, HCINTMSK4, HCINTMSK5, HCINTMSK6, HCINTMSK7, HCINTMSK8, HCINTMSK9, + HCSPLT0, HCSPLT1, HCSPLT10, HCSPLT11, HCSPLT2, HCSPLT3, HCSPLT4, HCSPLT5, HCSPLT6, HCSPLT7, + HCSPLT8, HCSPLT9, HCTSIZ0, HCTSIZ1, HCTSIZ10, HCTSIZ11, HCTSIZ2, HCTSIZ3, HCTSIZ4, HCTSIZ5, + HCTSIZ6, HCTSIZ7, HCTSIZ8, HCTSIZ9, HFIR, HFNUM, HPRT, HPTXSTS, +}; + +/// Access functions for the OTG_HS_HOST peripheral instance +pub mod OTG_HS_HOST { + use super::ResetValues; + + #[cfg(not(feature = "nosync"))] + use super::Instance; + + #[cfg(not(feature = "nosync"))] + const INSTANCE: Instance = Instance { + addr: 0x40040400, + _marker: ::core::marker::PhantomData, + }; + + /// Reset values for each field in OTG_HS_HOST + pub const reset: ResetValues = ResetValues { + HCFG: 0x00000000, + HFIR: 0x0000EA60, + HFNUM: 0x00003FFF, + HPTXSTS: 0x00080100, + HAINT: 0x00000000, + HAINTMSK: 0x00000000, + HPRT: 0x00000000, + HCCHAR0: 0x00000000, + HCCHAR1: 0x00000000, + HCCHAR2: 0x00000000, + HCCHAR3: 0x00000000, + HCCHAR4: 0x00000000, + HCCHAR5: 0x00000000, + HCCHAR6: 0x00000000, + HCCHAR7: 0x00000000, + HCCHAR8: 0x00000000, + HCCHAR9: 0x00000000, + HCCHAR10: 0x00000000, + HCCHAR11: 0x00000000, + HCSPLT0: 0x00000000, + HCSPLT1: 0x00000000, + HCSPLT2: 0x00000000, + HCSPLT3: 0x00000000, + HCSPLT4: 0x00000000, + HCSPLT5: 0x00000000, + HCSPLT6: 0x00000000, + HCSPLT7: 0x00000000, + HCSPLT8: 0x00000000, + HCSPLT9: 0x00000000, + HCSPLT10: 0x00000000, + HCSPLT11: 0x00000000, + HCINT0: 0x00000000, + HCINT1: 0x00000000, + HCINT2: 0x00000000, + HCINT3: 0x00000000, + HCINT4: 0x00000000, + HCINT5: 0x00000000, + HCINT6: 0x00000000, + HCINT7: 0x00000000, + HCINT8: 0x00000000, + HCINT9: 0x00000000, + HCINT10: 0x00000000, + HCINT11: 0x00000000, + HCINTMSK0: 0x00000000, + HCINTMSK1: 0x00000000, + HCINTMSK2: 0x00000000, + HCINTMSK3: 0x00000000, + HCINTMSK4: 0x00000000, + HCINTMSK5: 0x00000000, + HCINTMSK6: 0x00000000, + HCINTMSK7: 0x00000000, + HCINTMSK8: 0x00000000, + HCINTMSK9: 0x00000000, + HCINTMSK10: 0x00000000, + HCINTMSK11: 0x00000000, + HCTSIZ0: 0x00000000, + HCTSIZ1: 0x00000000, + HCTSIZ2: 0x00000000, + HCTSIZ3: 0x00000000, + HCTSIZ4: 0x00000000, + HCTSIZ5: 0x00000000, + HCTSIZ6: 0x00000000, + HCTSIZ7: 0x00000000, + HCTSIZ8: 0x00000000, + HCTSIZ9: 0x00000000, + HCTSIZ10: 0x00000000, + HCTSIZ11: 0x00000000, + HCDMA0: 0x00000000, + HCDMA1: 0x00000000, + HCDMA2: 0x00000000, + HCDMA3: 0x00000000, + HCDMA4: 0x00000000, + HCDMA5: 0x00000000, + HCDMA6: 0x00000000, + HCDMA7: 0x00000000, + HCDMA8: 0x00000000, + HCDMA9: 0x00000000, + HCDMA10: 0x00000000, + HCDMA11: 0x00000000, + }; +} + +/// Raw pointer to OTG_HS_HOST +/// +/// Dereferencing this is unsafe because you are not ensured unique +/// access to the peripheral, so you may encounter data races with +/// other users of this peripheral. It is up to you to ensure you +/// will not cause data races. +/// +/// This constant is provided for ease of use in unsafe code: you can +/// simply call for example `write_reg!(gpio, GPIOA, ODR, 1);`. +pub const OTG_HS_HOST: *const RegisterBlock = 0x40040400 as *const _; diff --git a/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_s_pwrclk.rs b/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_s_pwrclk.rs new file mode 100644 index 0000000..90e8d25 --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/ral/instances/otg_s_pwrclk.rs @@ -0,0 +1,70 @@ +#![allow(non_snake_case, non_upper_case_globals)] +#![allow(non_camel_case_types)] +//! USB on the go full speed +//! +//! Used by: stm32f405, stm32f407, stm32f427, stm32f429, stm32f446 + +#[cfg(not(feature = "nosync"))] +pub use super::super::peripherals::otg_fs_pwrclk::Instance; +pub use super::super::peripherals::otg_fs_pwrclk::PCGCCTL; +pub use super::super::peripherals::otg_fs_pwrclk::{RegisterBlock, ResetValues}; + +/// Access functions for the OTG_FS_PWRCLK peripheral instance +pub mod OTG_FS_PWRCLK { + use super::ResetValues; + + #[cfg(not(feature = "nosync"))] + use super::Instance; + + #[cfg(not(feature = "nosync"))] + const INSTANCE: Instance = Instance { + addr: 0x50000e00, + _marker: ::core::marker::PhantomData, + }; + + /// Reset values for each field in OTG_FS_PWRCLK + pub const reset: ResetValues = ResetValues { + PCGCCTL: 0x00000000, + }; +} + +/// Raw pointer to OTG_FS_PWRCLK +/// +/// Dereferencing this is unsafe because you are not ensured unique +/// access to the peripheral, so you may encounter data races with +/// other users of this peripheral. It is up to you to ensure you +/// will not cause data races. +/// +/// This constant is provided for ease of use in unsafe code: you can +/// simply call for example `write_reg!(gpio, GPIOA, ODR, 1);`. +pub const OTG_FS_PWRCLK: *const RegisterBlock = 0x50000e00 as *const _; + +/// Access functions for the OTG_HS_PWRCLK peripheral instance +pub mod OTG_HS_PWRCLK { + use super::ResetValues; + + #[cfg(not(feature = "nosync"))] + use super::Instance; + + #[cfg(not(feature = "nosync"))] + const INSTANCE: Instance = Instance { + addr: 0x40040e00, + _marker: ::core::marker::PhantomData, + }; + + /// Reset values for each field in OTG_HS_PWRCLK + pub const reset: ResetValues = ResetValues { + PCGCCTL: 0x00000000, + }; +} + +/// Raw pointer to OTG_HS_PWRCLK +/// +/// Dereferencing this is unsafe because you are not ensured unique +/// access to the peripheral, so you may encounter data races with +/// other users of this peripheral. It is up to you to ensure you +/// will not cause data races. +/// +/// This constant is provided for ease of use in unsafe code: you can +/// simply call for example `write_reg!(gpio, GPIOA, ODR, 1);`. +pub const OTG_HS_PWRCLK: *const RegisterBlock = 0x40040e00 as *const _; diff --git a/firmware/vendor/synopsys-usb-otg/src/ral/mod.rs b/firmware/vendor/synopsys-usb-otg/src/ral/mod.rs new file mode 100644 index 0000000..7d5032e --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/ral/mod.rs @@ -0,0 +1,118 @@ +#![allow(non_snake_case)] + +pub mod instances; +pub mod peripherals; +pub mod register; +pub mod stm32f429; + +pub use crate::{modify_reg, read_reg, write_reg}; + +pub mod otg_global { + #[cfg(feature = "fs")] + pub use super::stm32f429::otg_fs_global::OTG_FS_GLOBAL as OTG_GLOBAL; + #[cfg(feature = "fs")] + pub use super::stm32f429::otg_fs_global::*; + #[cfg(feature = "hs")] + pub use super::stm32f429::otg_hs_global::OTG_HS_GLOBAL as OTG_GLOBAL; + #[cfg(feature = "hs")] + pub use super::stm32f429::otg_hs_global::*; +} + +pub mod otg_device { + #[cfg(feature = "fs")] + pub use super::stm32f429::otg_fs_device::OTG_FS_DEVICE as OTG_DEVICE; + #[cfg(feature = "fs")] + pub use super::stm32f429::otg_fs_device::*; + #[cfg(feature = "hs")] + pub use super::stm32f429::otg_hs_device::OTG_HS_DEVICE as OTG_DEVICE; + #[cfg(feature = "hs")] + pub use super::stm32f429::otg_hs_device::*; +} + +pub mod otg_pwrclk { + #[cfg(feature = "fs")] + pub use super::stm32f429::otg_s_pwrclk::OTG_FS_PWRCLK as OTG_PWRCLK; + #[cfg(feature = "hs")] + pub use super::stm32f429::otg_s_pwrclk::OTG_HS_PWRCLK as OTG_PWRCLK; + pub use super::stm32f429::otg_s_pwrclk::*; +} + +pub mod otg_global_dieptxfx { + use super::register::RWRegister; + + #[cfg(feature = "fs")] + pub use super::stm32f429::otg_fs_global::DIEPTXF1 as DIEPTXFx; + #[cfg(feature = "hs")] + pub use super::stm32f429::otg_fs_global::DIEPTXF1 as DIEPTXFx; + + pub struct RegisterBlock { + pub DIEPTXFx: RWRegister, + } +} + +pub mod endpoint_in { + use super::register::RWRegister; + + #[cfg(feature = "fs")] + pub use super::stm32f429::otg_fs_device::{ + DIEPCTL1 as DIEPCTL, DIEPINT1 as DIEPINT, DIEPTSIZ1 as DIEPTSIZ, DTXFSTS1 as DTXFSTS, + }; + + #[cfg(feature = "hs")] + pub use super::stm32f429::otg_hs_device::{ + DIEPCTL1 as DIEPCTL, DIEPINT1 as DIEPINT, DIEPTSIZ1 as DIEPTSIZ, DTXFSTS1 as DTXFSTS, + }; + + pub struct RegisterBlock { + pub DIEPCTL: RWRegister, + _reserved0: u32, + pub DIEPINT: RWRegister, + _reserved1: u32, + pub DIEPTSIZ: RWRegister, + _reserved2: u32, + pub DTXFSTS: RWRegister, + _reserved3: u32, + } +} + +pub mod endpoint0_out { + use super::register::RWRegister; + + #[cfg(feature = "fs")] + pub use super::stm32f429::otg_fs_device::{DOEPCTL0, DOEPINT0, DOEPTSIZ0}; + + #[cfg(feature = "hs")] + pub use super::stm32f429::otg_hs_device::{DOEPCTL0, DOEPINT0, DOEPTSIZ0}; + + pub struct RegisterBlock { + pub DOEPCTL0: RWRegister, + _reserved0: u32, + pub DOEPINT0: RWRegister, + _reserved1: u32, + pub DOEPTSIZ0: RWRegister, + _reserved2: [u32; 3], + } +} + +pub mod endpoint_out { + use super::register::RWRegister; + + #[cfg(feature = "fs")] + pub use super::stm32f429::otg_fs_device::{ + DOEPCTL1 as DOEPCTL, DOEPINT1 as DOEPINT, DOEPTSIZ1 as DOEPTSIZ, + }; + + #[cfg(feature = "hs")] + pub use super::stm32f429::otg_hs_device::{ + DOEPCTL1 as DOEPCTL, DOEPINT1 as DOEPINT, DOEPTSIZ1 as DOEPTSIZ, + }; + + pub struct RegisterBlock { + pub DOEPCTL: RWRegister, + _reserved0: u32, + pub DOEPINT: RWRegister, + _reserved1: u32, + pub DOEPTSIZ: RWRegister, + _reserved2: [u32; 3], + } +} diff --git a/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/mod.rs b/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/mod.rs new file mode 100644 index 0000000..603621e --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/mod.rs @@ -0,0 +1,8 @@ +#![allow(unused)] +pub mod otg_fs_device_v1; +pub mod otg_fs_global_v1; +pub mod otg_fs_host; +pub mod otg_fs_pwrclk; +pub mod otg_hs_device; +pub mod otg_hs_global; +pub mod otg_hs_host; diff --git a/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_fs_device_v1.rs b/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_fs_device_v1.rs new file mode 100644 index 0000000..4610f49 --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_fs_device_v1.rs @@ -0,0 +1,1950 @@ +#![allow(non_snake_case, non_upper_case_globals)] +#![allow(non_camel_case_types)] +//! USB on the go full speed +//! +//! Used by: stm32f401, stm32f405, stm32f407, stm32f411, stm32f427, stm32f429 + +use super::super::register::{RORegister, RWRegister}; +#[cfg(not(feature = "nosync"))] +use core::marker::PhantomData; + +/// OTG_FS device configuration register (OTG_FS_DCFG) +pub mod DCFG { + + /// Device speed + pub mod DSPD { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (2 bits: 0b11 << 0) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Non-zero-length status OUT handshake + pub mod NZLSOHSK { + /// Offset (2 bits) + pub const offset: u32 = 2; + /// Mask (1 bit: 1 << 2) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Device address + pub mod DAD { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (7 bits: 0x7f << 4) + pub const mask: u32 = 0x7f << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Periodic frame interval + pub mod PFIVL { + /// Offset (11 bits) + pub const offset: u32 = 11; + /// Mask (2 bits: 0b11 << 11) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS device control register (OTG_FS_DCTL) +pub mod DCTL { + + /// Remote wakeup signaling + pub mod RWUSIG { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Soft disconnect + pub mod SDIS { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Global IN NAK status + pub mod GINSTS { + /// Offset (2 bits) + pub const offset: u32 = 2; + /// Mask (1 bit: 1 << 2) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Global OUT NAK status + pub mod GONSTS { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Test control + pub mod TCTL { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (3 bits: 0b111 << 4) + pub const mask: u32 = 0b111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Set global IN NAK + pub mod SGINAK { + /// Offset (7 bits) + pub const offset: u32 = 7; + /// Mask (1 bit: 1 << 7) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Clear global IN NAK + pub mod CGINAK { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Set global OUT NAK + pub mod SGONAK { + /// Offset (9 bits) + pub const offset: u32 = 9; + /// Mask (1 bit: 1 << 9) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Clear global OUT NAK + pub mod CGONAK { + /// Offset (10 bits) + pub const offset: u32 = 10; + /// Mask (1 bit: 1 << 10) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Power-on programming done + pub mod POPRGDNE { + /// Offset (11 bits) + pub const offset: u32 = 11; + /// Mask (1 bit: 1 << 11) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS device status register (OTG_FS_DSTS) +pub mod DSTS { + + /// Suspend status + pub mod SUSPSTS { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Enumerated speed + pub mod ENUMSPD { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (2 bits: 0b11 << 1) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Erratic error + pub mod EERR { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Frame number of the received SOF + pub mod FNSOF { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (14 bits: 0x3fff << 8) + pub const mask: u32 = 0x3fff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS device IN endpoint common interrupt mask register (OTG_FS_DIEPMSK) +pub mod DIEPMSK { + + /// Transfer completed interrupt mask + pub mod XFRCM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint disabled interrupt mask + pub mod EPDM { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Timeout condition mask (Non-isochronous endpoints) + pub mod TOM { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN token received when TxFIFO empty mask + pub mod ITTXFEMSK { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN token received with EP mismatch mask + pub mod INEPNMM { + /// Offset (5 bits) + pub const offset: u32 = 5; + /// Mask (1 bit: 1 << 5) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN endpoint NAK effective mask + pub mod INEPNEM { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS device OUT endpoint common interrupt mask register (OTG_FS_DOEPMSK) +pub mod DOEPMSK { + + /// Transfer completed interrupt mask + pub mod XFRCM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint disabled interrupt mask + pub mod EPDM { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// SETUP phase done mask + pub mod STUPM { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OUT token received when endpoint disabled mask + pub mod OTEPDM { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS device all endpoints interrupt register (OTG_FS_DAINT) +pub mod DAINT { + + /// IN endpoint interrupt bits + pub mod IEPINT { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OUT endpoint interrupt bits + pub mod OEPINT { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (16 bits: 0xffff << 16) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS all endpoints interrupt mask register (OTG_FS_DAINTMSK) +pub mod DAINTMSK { + + /// IN EP interrupt mask bits + pub mod IEPM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OUT EP interrupt mask bits + pub mod OEPM { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (16 bits: 0xffff << 16) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS device VBUS discharge time register +pub mod DVBUSDIS { + + /// Device VBUS discharge time + pub mod VBUSDT { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS device VBUS pulsing time register +pub mod DVBUSPULSE { + + /// Device VBUS pulsing time + pub mod DVBUSP { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (12 bits: 0xfff << 0) + pub const mask: u32 = 0xfff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS device IN endpoint FIFO empty interrupt mask register +pub mod DIEPEMPMSK { + + /// IN EP Tx FIFO empty interrupt mask bits + pub mod INEPTXFEM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS device control IN endpoint 0 control register (OTG_FS_DIEPCTL0) +pub mod DIEPCTL0 { + + /// Maximum packet size + pub mod MPSIZ { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (2 bits: 0b11 << 0) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// USB active endpoint + pub mod USBAEP { + /// Offset (15 bits) + pub const offset: u32 = 15; + /// Mask (1 bit: 1 << 15) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// NAK status + pub mod NAKSTS { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (1 bit: 1 << 17) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint type + pub mod EPTYP { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (2 bits: 0b11 << 18) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// STALL handshake + pub mod STALL { + /// Offset (21 bits) + pub const offset: u32 = 21; + /// Mask (1 bit: 1 << 21) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// TxFIFO number + pub mod TXFNUM { + /// Offset (22 bits) + pub const offset: u32 = 22; + /// Mask (4 bits: 0b1111 << 22) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Clear NAK + pub mod CNAK { + /// Offset (26 bits) + pub const offset: u32 = 26; + /// Mask (1 bit: 1 << 26) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Set NAK + pub mod SNAK { + /// Offset (27 bits) + pub const offset: u32 = 27; + /// Mask (1 bit: 1 << 27) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint disable + pub mod EPDIS { + /// Offset (30 bits) + pub const offset: u32 = 30; + /// Mask (1 bit: 1 << 30) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint enable + pub mod EPENA { + /// Offset (31 bits) + pub const offset: u32 = 31; + /// Mask (1 bit: 1 << 31) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG device endpoint-1 control register +pub mod DIEPCTL1 { + + /// EPENA + pub mod EPENA { + /// Offset (31 bits) + pub const offset: u32 = 31; + /// Mask (1 bit: 1 << 31) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// EPDIS + pub mod EPDIS { + /// Offset (30 bits) + pub const offset: u32 = 30; + /// Mask (1 bit: 1 << 30) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// SODDFRM/SD1PID + pub mod SODDFRM_SD1PID { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (1 bit: 1 << 29) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// SD0PID/SEVNFRM + pub mod SD0PID_SEVNFRM { + /// Offset (28 bits) + pub const offset: u32 = 28; + /// Mask (1 bit: 1 << 28) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// SNAK + pub mod SNAK { + /// Offset (27 bits) + pub const offset: u32 = 27; + /// Mask (1 bit: 1 << 27) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// CNAK + pub mod CNAK { + /// Offset (26 bits) + pub const offset: u32 = 26; + /// Mask (1 bit: 1 << 26) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// TXFNUM + pub mod TXFNUM { + /// Offset (22 bits) + pub const offset: u32 = 22; + /// Mask (4 bits: 0b1111 << 22) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// STALL + pub mod STALL { + /// Offset (21 bits) + pub const offset: u32 = 21; + /// Mask (1 bit: 1 << 21) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// EPTYP + pub mod EPTYP { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (2 bits: 0b11 << 18) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// NAKSTS + pub mod NAKSTS { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (1 bit: 1 << 17) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// EONUM/DPID + pub mod EONUM_DPID { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (1 bit: 1 << 16) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// USBAEP + pub mod USBAEP { + /// Offset (15 bits) + pub const offset: u32 = 15; + /// Mask (1 bit: 1 << 15) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// MPSIZ + pub mod MPSIZ { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (11 bits: 0x7ff << 0) + pub const mask: u32 = 0x7ff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG device endpoint-1 control register +pub mod DIEPCTL2 { + pub use super::DIEPCTL1::CNAK; + pub use super::DIEPCTL1::EONUM_DPID; + pub use super::DIEPCTL1::EPDIS; + pub use super::DIEPCTL1::EPENA; + pub use super::DIEPCTL1::EPTYP; + pub use super::DIEPCTL1::MPSIZ; + pub use super::DIEPCTL1::NAKSTS; + pub use super::DIEPCTL1::SD0PID_SEVNFRM; + pub use super::DIEPCTL1::SNAK; + pub use super::DIEPCTL1::SODDFRM_SD1PID; + pub use super::DIEPCTL1::STALL; + pub use super::DIEPCTL1::TXFNUM; + pub use super::DIEPCTL1::USBAEP; +} + +/// OTG device endpoint-1 control register +pub mod DIEPCTL3 { + pub use super::DIEPCTL1::CNAK; + pub use super::DIEPCTL1::EONUM_DPID; + pub use super::DIEPCTL1::EPDIS; + pub use super::DIEPCTL1::EPENA; + pub use super::DIEPCTL1::EPTYP; + pub use super::DIEPCTL1::MPSIZ; + pub use super::DIEPCTL1::NAKSTS; + pub use super::DIEPCTL1::SD0PID_SEVNFRM; + pub use super::DIEPCTL1::SNAK; + pub use super::DIEPCTL1::SODDFRM_SD1PID; + pub use super::DIEPCTL1::STALL; + pub use super::DIEPCTL1::TXFNUM; + pub use super::DIEPCTL1::USBAEP; +} + +/// device endpoint-0 control register +pub mod DOEPCTL0 { + + /// EPENA + pub mod EPENA { + /// Offset (31 bits) + pub const offset: u32 = 31; + /// Mask (1 bit: 1 << 31) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// EPDIS + pub mod EPDIS { + /// Offset (30 bits) + pub const offset: u32 = 30; + /// Mask (1 bit: 1 << 30) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// SNAK + pub mod SNAK { + /// Offset (27 bits) + pub const offset: u32 = 27; + /// Mask (1 bit: 1 << 27) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// CNAK + pub mod CNAK { + /// Offset (26 bits) + pub const offset: u32 = 26; + /// Mask (1 bit: 1 << 26) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// STALL + pub mod STALL { + /// Offset (21 bits) + pub const offset: u32 = 21; + /// Mask (1 bit: 1 << 21) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// SNPM + pub mod SNPM { + /// Offset (20 bits) + pub const offset: u32 = 20; + /// Mask (1 bit: 1 << 20) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// EPTYP + pub mod EPTYP { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (2 bits: 0b11 << 18) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// NAKSTS + pub mod NAKSTS { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (1 bit: 1 << 17) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// USBAEP + pub mod USBAEP { + /// Offset (15 bits) + pub const offset: u32 = 15; + /// Mask (1 bit: 1 << 15) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// MPSIZ + pub mod MPSIZ { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (2 bits: 0b11 << 0) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// device endpoint-1 control register +pub mod DOEPCTL1 { + + /// EPENA + pub mod EPENA { + /// Offset (31 bits) + pub const offset: u32 = 31; + /// Mask (1 bit: 1 << 31) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// EPDIS + pub mod EPDIS { + /// Offset (30 bits) + pub const offset: u32 = 30; + /// Mask (1 bit: 1 << 30) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// SODDFRM + pub mod SODDFRM { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (1 bit: 1 << 29) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// SD0PID/SEVNFRM + pub mod SD0PID_SEVNFRM { + /// Offset (28 bits) + pub const offset: u32 = 28; + /// Mask (1 bit: 1 << 28) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// SNAK + pub mod SNAK { + /// Offset (27 bits) + pub const offset: u32 = 27; + /// Mask (1 bit: 1 << 27) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// CNAK + pub mod CNAK { + /// Offset (26 bits) + pub const offset: u32 = 26; + /// Mask (1 bit: 1 << 26) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// STALL + pub mod STALL { + /// Offset (21 bits) + pub const offset: u32 = 21; + /// Mask (1 bit: 1 << 21) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// SNPM + pub mod SNPM { + /// Offset (20 bits) + pub const offset: u32 = 20; + /// Mask (1 bit: 1 << 20) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// EPTYP + pub mod EPTYP { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (2 bits: 0b11 << 18) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// NAKSTS + pub mod NAKSTS { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (1 bit: 1 << 17) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// EONUM/DPID + pub mod EONUM_DPID { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (1 bit: 1 << 16) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// USBAEP + pub mod USBAEP { + /// Offset (15 bits) + pub const offset: u32 = 15; + /// Mask (1 bit: 1 << 15) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// MPSIZ + pub mod MPSIZ { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (11 bits: 0x7ff << 0) + pub const mask: u32 = 0x7ff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// device endpoint-1 control register +pub mod DOEPCTL2 { + pub use super::DOEPCTL1::CNAK; + pub use super::DOEPCTL1::EONUM_DPID; + pub use super::DOEPCTL1::EPDIS; + pub use super::DOEPCTL1::EPENA; + pub use super::DOEPCTL1::EPTYP; + pub use super::DOEPCTL1::MPSIZ; + pub use super::DOEPCTL1::NAKSTS; + pub use super::DOEPCTL1::SD0PID_SEVNFRM; + pub use super::DOEPCTL1::SNAK; + pub use super::DOEPCTL1::SNPM; + pub use super::DOEPCTL1::SODDFRM; + pub use super::DOEPCTL1::STALL; + pub use super::DOEPCTL1::USBAEP; +} + +/// device endpoint-1 control register +pub mod DOEPCTL3 { + pub use super::DOEPCTL1::CNAK; + pub use super::DOEPCTL1::EONUM_DPID; + pub use super::DOEPCTL1::EPDIS; + pub use super::DOEPCTL1::EPENA; + pub use super::DOEPCTL1::EPTYP; + pub use super::DOEPCTL1::MPSIZ; + pub use super::DOEPCTL1::NAKSTS; + pub use super::DOEPCTL1::SD0PID_SEVNFRM; + pub use super::DOEPCTL1::SNAK; + pub use super::DOEPCTL1::SNPM; + pub use super::DOEPCTL1::SODDFRM; + pub use super::DOEPCTL1::STALL; + pub use super::DOEPCTL1::USBAEP; +} + +/// device endpoint-x interrupt register +pub mod DIEPINT0 { + + /// TXFE + pub mod TXFE { + /// Offset (7 bits) + pub const offset: u32 = 7; + /// Mask (1 bit: 1 << 7) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// INEPNE + pub mod INEPNE { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// ITTXFE + pub mod ITTXFE { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// TOC + pub mod TOC { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// EPDISD + pub mod EPDISD { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// XFRC + pub mod XFRC { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// device endpoint-1 interrupt register +pub mod DIEPINT1 { + pub use super::DIEPINT0::EPDISD; + pub use super::DIEPINT0::INEPNE; + pub use super::DIEPINT0::ITTXFE; + pub use super::DIEPINT0::TOC; + pub use super::DIEPINT0::TXFE; + pub use super::DIEPINT0::XFRC; +} + +/// device endpoint-2 interrupt register +pub mod DIEPINT2 { + pub use super::DIEPINT0::EPDISD; + pub use super::DIEPINT0::INEPNE; + pub use super::DIEPINT0::ITTXFE; + pub use super::DIEPINT0::TOC; + pub use super::DIEPINT0::TXFE; + pub use super::DIEPINT0::XFRC; +} + +/// device endpoint-3 interrupt register +pub mod DIEPINT3 { + pub use super::DIEPINT0::EPDISD; + pub use super::DIEPINT0::INEPNE; + pub use super::DIEPINT0::ITTXFE; + pub use super::DIEPINT0::TOC; + pub use super::DIEPINT0::TXFE; + pub use super::DIEPINT0::XFRC; +} + +/// device endpoint-0 interrupt register +pub mod DOEPINT0 { + + /// B2BSTUP + pub mod B2BSTUP { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OTEPDIS + pub mod OTEPDIS { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// STUP + pub mod STUP { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// EPDISD + pub mod EPDISD { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// XFRC + pub mod XFRC { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// device endpoint-1 interrupt register +pub mod DOEPINT1 { + pub use super::DOEPINT0::B2BSTUP; + pub use super::DOEPINT0::EPDISD; + pub use super::DOEPINT0::OTEPDIS; + pub use super::DOEPINT0::STUP; + pub use super::DOEPINT0::XFRC; +} + +/// device endpoint-2 interrupt register +pub mod DOEPINT2 { + pub use super::DOEPINT0::B2BSTUP; + pub use super::DOEPINT0::EPDISD; + pub use super::DOEPINT0::OTEPDIS; + pub use super::DOEPINT0::STUP; + pub use super::DOEPINT0::XFRC; +} + +/// device endpoint-3 interrupt register +pub mod DOEPINT3 { + pub use super::DOEPINT0::B2BSTUP; + pub use super::DOEPINT0::EPDISD; + pub use super::DOEPINT0::OTEPDIS; + pub use super::DOEPINT0::STUP; + pub use super::DOEPINT0::XFRC; +} + +/// device endpoint-0 transfer size register +pub mod DIEPTSIZ0 { + + /// Packet count + pub mod PKTCNT { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (2 bits: 0b11 << 19) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Transfer size + pub mod XFRSIZ { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (7 bits: 0x7f << 0) + pub const mask: u32 = 0x7f << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// device OUT endpoint-0 transfer size register +pub mod DOEPTSIZ0 { + + /// SETUP packet count + pub mod STUPCNT { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (2 bits: 0b11 << 29) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Packet count + pub mod PKTCNT { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (1 bit: 1 << 19) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Transfer size + pub mod XFRSIZ { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (7 bits: 0x7f << 0) + pub const mask: u32 = 0x7f << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// device endpoint-1 transfer size register +pub mod DIEPTSIZ1 { + + /// Multi count + pub mod MCNT { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (2 bits: 0b11 << 29) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Packet count + pub mod PKTCNT { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (10 bits: 0x3ff << 19) + pub const mask: u32 = 0x3ff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Transfer size + pub mod XFRSIZ { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (19 bits: 0x7ffff << 0) + pub const mask: u32 = 0x7ffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// device endpoint-2 transfer size register +pub mod DIEPTSIZ2 { + pub use super::DIEPTSIZ1::MCNT; + pub use super::DIEPTSIZ1::PKTCNT; + pub use super::DIEPTSIZ1::XFRSIZ; +} + +/// device endpoint-3 transfer size register +pub mod DIEPTSIZ3 { + pub use super::DIEPTSIZ1::MCNT; + pub use super::DIEPTSIZ1::PKTCNT; + pub use super::DIEPTSIZ1::XFRSIZ; +} + +/// OTG_FS device IN endpoint transmit FIFO status register +pub mod DTXFSTS0 { + + /// IN endpoint TxFIFO space available + pub mod INEPTFSAV { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS device IN endpoint transmit FIFO status register +pub mod DTXFSTS1 { + pub use super::DTXFSTS0::INEPTFSAV; +} + +/// OTG_FS device IN endpoint transmit FIFO status register +pub mod DTXFSTS2 { + pub use super::DTXFSTS0::INEPTFSAV; +} + +/// OTG_FS device IN endpoint transmit FIFO status register +pub mod DTXFSTS3 { + pub use super::DTXFSTS0::INEPTFSAV; +} + +/// device OUT endpoint-1 transfer size register +pub mod DOEPTSIZ1 { + + /// Received data PID/SETUP packet count + pub mod RXDPID_STUPCNT { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (2 bits: 0b11 << 29) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Packet count + pub mod PKTCNT { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (10 bits: 0x3ff << 19) + pub const mask: u32 = 0x3ff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Transfer size + pub mod XFRSIZ { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (19 bits: 0x7ffff << 0) + pub const mask: u32 = 0x7ffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// device OUT endpoint-2 transfer size register +pub mod DOEPTSIZ2 { + pub use super::DOEPTSIZ1::PKTCNT; + pub use super::DOEPTSIZ1::RXDPID_STUPCNT; + pub use super::DOEPTSIZ1::XFRSIZ; +} + +/// device OUT endpoint-3 transfer size register +pub mod DOEPTSIZ3 { + pub use super::DOEPTSIZ1::PKTCNT; + pub use super::DOEPTSIZ1::RXDPID_STUPCNT; + pub use super::DOEPTSIZ1::XFRSIZ; +} +#[repr(C)] +pub struct RegisterBlock { + /// OTG_FS device configuration register (OTG_FS_DCFG) + pub DCFG: RWRegister, + + /// OTG_FS device control register (OTG_FS_DCTL) + pub DCTL: RWRegister, + + /// OTG_FS device status register (OTG_FS_DSTS) + pub DSTS: RORegister, + + _reserved1: [u32; 1], + + /// OTG_FS device IN endpoint common interrupt mask register (OTG_FS_DIEPMSK) + pub DIEPMSK: RWRegister, + + /// OTG_FS device OUT endpoint common interrupt mask register (OTG_FS_DOEPMSK) + pub DOEPMSK: RWRegister, + + /// OTG_FS device all endpoints interrupt register (OTG_FS_DAINT) + pub DAINT: RORegister, + + /// OTG_FS all endpoints interrupt mask register (OTG_FS_DAINTMSK) + pub DAINTMSK: RWRegister, + + _reserved2: [u32; 2], + + /// OTG_FS device VBUS discharge time register + pub DVBUSDIS: RWRegister, + + /// OTG_FS device VBUS pulsing time register + pub DVBUSPULSE: RWRegister, + + _reserved3: [u32; 1], + + /// OTG_FS device IN endpoint FIFO empty interrupt mask register + pub DIEPEMPMSK: RWRegister, + + _reserved4: [u32; 50], + + /// OTG_FS device control IN endpoint 0 control register (OTG_FS_DIEPCTL0) + pub DIEPCTL0: RWRegister, + + _reserved5: [u32; 1], + + /// device endpoint-x interrupt register + pub DIEPINT0: RWRegister, + + _reserved6: [u32; 1], + + /// device endpoint-0 transfer size register + pub DIEPTSIZ0: RWRegister, + + _reserved7: [u32; 1], + + /// OTG_FS device IN endpoint transmit FIFO status register + pub DTXFSTS0: RORegister, + + _reserved8: [u32; 1], + + /// OTG device endpoint-1 control register + pub DIEPCTL1: RWRegister, + + _reserved9: [u32; 1], + + /// device endpoint-1 interrupt register + pub DIEPINT1: RWRegister, + + _reserved10: [u32; 1], + + /// device endpoint-1 transfer size register + pub DIEPTSIZ1: RWRegister, + + _reserved11: [u32; 1], + + /// OTG_FS device IN endpoint transmit FIFO status register + pub DTXFSTS1: RORegister, + + _reserved12: [u32; 1], + + /// OTG device endpoint-1 control register + pub DIEPCTL2: RWRegister, + + _reserved13: [u32; 1], + + /// device endpoint-2 interrupt register + pub DIEPINT2: RWRegister, + + _reserved14: [u32; 1], + + /// device endpoint-2 transfer size register + pub DIEPTSIZ2: RWRegister, + + _reserved15: [u32; 1], + + /// OTG_FS device IN endpoint transmit FIFO status register + pub DTXFSTS2: RORegister, + + _reserved16: [u32; 1], + + /// OTG device endpoint-1 control register + pub DIEPCTL3: RWRegister, + + _reserved17: [u32; 1], + + /// device endpoint-3 interrupt register + pub DIEPINT3: RWRegister, + + _reserved18: [u32; 1], + + /// device endpoint-3 transfer size register + pub DIEPTSIZ3: RWRegister, + + _reserved19: [u32; 1], + + /// OTG_FS device IN endpoint transmit FIFO status register + pub DTXFSTS3: RORegister, + + _reserved20: [u32; 97], + + /// device endpoint-0 control register + pub DOEPCTL0: RWRegister, + + _reserved21: [u32; 1], + + /// device endpoint-0 interrupt register + pub DOEPINT0: RWRegister, + + _reserved22: [u32; 1], + + /// device OUT endpoint-0 transfer size register + pub DOEPTSIZ0: RWRegister, + + _reserved23: [u32; 3], + + /// device endpoint-1 control register + pub DOEPCTL1: RWRegister, + + _reserved24: [u32; 1], + + /// device endpoint-1 interrupt register + pub DOEPINT1: RWRegister, + + _reserved25: [u32; 1], + + /// device OUT endpoint-1 transfer size register + pub DOEPTSIZ1: RWRegister, + + _reserved26: [u32; 3], + + /// device endpoint-1 control register + pub DOEPCTL2: RWRegister, + + _reserved27: [u32; 1], + + /// device endpoint-2 interrupt register + pub DOEPINT2: RWRegister, + + _reserved28: [u32; 1], + + /// device OUT endpoint-2 transfer size register + pub DOEPTSIZ2: RWRegister, + + _reserved29: [u32; 3], + + /// device endpoint-1 control register + pub DOEPCTL3: RWRegister, + + _reserved30: [u32; 1], + + /// device endpoint-3 interrupt register + pub DOEPINT3: RWRegister, + + _reserved31: [u32; 1], + + /// device OUT endpoint-3 transfer size register + pub DOEPTSIZ3: RWRegister, +} +pub struct ResetValues { + pub DCFG: u32, + pub DCTL: u32, + pub DSTS: u32, + pub DIEPMSK: u32, + pub DOEPMSK: u32, + pub DAINT: u32, + pub DAINTMSK: u32, + pub DVBUSDIS: u32, + pub DVBUSPULSE: u32, + pub DIEPEMPMSK: u32, + pub DIEPCTL0: u32, + pub DIEPINT0: u32, + pub DIEPTSIZ0: u32, + pub DTXFSTS0: u32, + pub DIEPCTL1: u32, + pub DIEPINT1: u32, + pub DIEPTSIZ1: u32, + pub DTXFSTS1: u32, + pub DIEPCTL2: u32, + pub DIEPINT2: u32, + pub DIEPTSIZ2: u32, + pub DTXFSTS2: u32, + pub DIEPCTL3: u32, + pub DIEPINT3: u32, + pub DIEPTSIZ3: u32, + pub DTXFSTS3: u32, + pub DOEPCTL0: u32, + pub DOEPINT0: u32, + pub DOEPTSIZ0: u32, + pub DOEPCTL1: u32, + pub DOEPINT1: u32, + pub DOEPTSIZ1: u32, + pub DOEPCTL2: u32, + pub DOEPINT2: u32, + pub DOEPTSIZ2: u32, + pub DOEPCTL3: u32, + pub DOEPINT3: u32, + pub DOEPTSIZ3: u32, +} +#[cfg(not(feature = "nosync"))] +pub struct Instance { + pub(crate) addr: u32, + pub(crate) _marker: PhantomData<*const RegisterBlock>, +} +#[cfg(not(feature = "nosync"))] +impl ::core::ops::Deref for Instance { + type Target = RegisterBlock; + #[inline(always)] + fn deref(&self) -> &RegisterBlock { + unsafe { &*(self.addr as *const _) } + } +} +#[cfg(feature = "rtfm")] +unsafe impl Send for Instance {} diff --git a/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_fs_global_v1.rs b/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_fs_global_v1.rs new file mode 100644 index 0000000..ae605da --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_fs_global_v1.rs @@ -0,0 +1,1774 @@ +#![allow(non_snake_case, non_upper_case_globals)] +#![allow(non_camel_case_types)] +//! USB on the go full speed +//! +//! Used by: stm32f401, stm32f405, stm32f407, stm32f411, stm32f427, stm32f429 + +use super::super::register::{RORegister, RWRegister}; +#[cfg(not(feature = "nosync"))] +use core::marker::PhantomData; + +/// OTG_FS control and status register (OTG_FS_GOTGCTL) +pub mod GOTGCTL { + + /// Session request success + pub mod SRQSCS { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Session request + pub mod SRQ { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host negotiation success + pub mod HNGSCS { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// HNP request + pub mod HNPRQ { + /// Offset (9 bits) + pub const offset: u32 = 9; + /// Mask (1 bit: 1 << 9) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host set HNP enable + pub mod HSHNPEN { + /// Offset (10 bits) + pub const offset: u32 = 10; + /// Mask (1 bit: 1 << 10) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Device HNP enabled + pub mod DHNPEN { + /// Offset (11 bits) + pub const offset: u32 = 11; + /// Mask (1 bit: 1 << 11) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Connector ID status + pub mod CIDSTS { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (1 bit: 1 << 16) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Long/short debounce time + pub mod DBCT { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (1 bit: 1 << 17) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// A-session valid + pub mod ASVLD { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (1 bit: 1 << 18) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// B-session valid + pub mod BSVLD { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (1 bit: 1 << 19) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS interrupt register (OTG_FS_GOTGINT) +pub mod GOTGINT { + + /// Session end detected + pub mod SEDET { + /// Offset (2 bits) + pub const offset: u32 = 2; + /// Mask (1 bit: 1 << 2) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Session request success status change + pub mod SRSSCHG { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host negotiation success status change + pub mod HNSSCHG { + /// Offset (9 bits) + pub const offset: u32 = 9; + /// Mask (1 bit: 1 << 9) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host negotiation detected + pub mod HNGDET { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (1 bit: 1 << 17) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// A-device timeout change + pub mod ADTOCHG { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (1 bit: 1 << 18) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Debounce done + pub mod DBCDNE { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (1 bit: 1 << 19) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS AHB configuration register (OTG_FS_GAHBCFG) +pub mod GAHBCFG { + + /// Global interrupt mask + pub mod GINT { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// TxFIFO empty level + pub mod TXFELVL { + /// Offset (7 bits) + pub const offset: u32 = 7; + /// Mask (1 bit: 1 << 7) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Periodic TxFIFO empty level + pub mod PTXFELVL { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS USB configuration register (OTG_FS_GUSBCFG) +pub mod GUSBCFG { + + /// FS timeout calibration + pub mod TOCAL { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (3 bits: 0b111 << 0) + pub const mask: u32 = 0b111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Full Speed serial transceiver select + pub mod PHYSEL { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// SRP-capable + pub mod SRPCAP { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// HNP-capable + pub mod HNPCAP { + /// Offset (9 bits) + pub const offset: u32 = 9; + /// Mask (1 bit: 1 << 9) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// USB turnaround time + pub mod TRDT { + /// Offset (10 bits) + pub const offset: u32 = 10; + /// Mask (4 bits: 0b1111 << 10) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Force host mode + pub mod FHMOD { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (1 bit: 1 << 29) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Force device mode + pub mod FDMOD { + /// Offset (30 bits) + pub const offset: u32 = 30; + /// Mask (1 bit: 1 << 30) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Corrupt Tx packet + pub mod CTXPKT { + /// Offset (31 bits) + pub const offset: u32 = 31; + /// Mask (1 bit: 1 << 31) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS reset register (OTG_FS_GRSTCTL) +pub mod GRSTCTL { + + /// Core soft reset + pub mod CSRST { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// HCLK soft reset + pub mod HSRST { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host frame counter reset + pub mod FCRST { + /// Offset (2 bits) + pub const offset: u32 = 2; + /// Mask (1 bit: 1 << 2) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// RxFIFO flush + pub mod RXFFLSH { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// TxFIFO flush + pub mod TXFFLSH { + /// Offset (5 bits) + pub const offset: u32 = 5; + /// Mask (1 bit: 1 << 5) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// TxFIFO number + pub mod TXFNUM { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (5 bits: 0b11111 << 6) + pub const mask: u32 = 0b11111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// AHB master idle + pub mod AHBIDL { + /// Offset (31 bits) + pub const offset: u32 = 31; + /// Mask (1 bit: 1 << 31) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS core interrupt register (OTG_FS_GINTSTS) +pub mod GINTSTS { + + /// Current mode of operation + pub mod CMOD { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Mode mismatch interrupt + pub mod MMIS { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OTG interrupt + pub mod OTGINT { + /// Offset (2 bits) + pub const offset: u32 = 2; + /// Mask (1 bit: 1 << 2) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Start of frame + pub mod SOF { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// RxFIFO non-empty + pub mod RXFLVL { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Non-periodic TxFIFO empty + pub mod NPTXFE { + /// Offset (5 bits) + pub const offset: u32 = 5; + /// Mask (1 bit: 1 << 5) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Global IN non-periodic NAK effective + pub mod GINAKEFF { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Global OUT NAK effective + pub mod GOUTNAKEFF { + /// Offset (7 bits) + pub const offset: u32 = 7; + /// Mask (1 bit: 1 << 7) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Early suspend + pub mod ESUSP { + /// Offset (10 bits) + pub const offset: u32 = 10; + /// Mask (1 bit: 1 << 10) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// USB suspend + pub mod USBSUSP { + /// Offset (11 bits) + pub const offset: u32 = 11; + /// Mask (1 bit: 1 << 11) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// USB reset + pub mod USBRST { + /// Offset (12 bits) + pub const offset: u32 = 12; + /// Mask (1 bit: 1 << 12) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Enumeration done + pub mod ENUMDNE { + /// Offset (13 bits) + pub const offset: u32 = 13; + /// Mask (1 bit: 1 << 13) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Isochronous OUT packet dropped interrupt + pub mod ISOODRP { + /// Offset (14 bits) + pub const offset: u32 = 14; + /// Mask (1 bit: 1 << 14) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// End of periodic frame interrupt + pub mod EOPF { + /// Offset (15 bits) + pub const offset: u32 = 15; + /// Mask (1 bit: 1 << 15) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN endpoint interrupt + pub mod IEPINT { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (1 bit: 1 << 18) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OUT endpoint interrupt + pub mod OEPINT { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (1 bit: 1 << 19) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Incomplete isochronous IN transfer + pub mod IISOIXFR { + /// Offset (20 bits) + pub const offset: u32 = 20; + /// Mask (1 bit: 1 << 20) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Incomplete periodic transfer(Host mode)/Incomplete isochronous OUT transfer(Device mode) + pub mod IPXFR_INCOMPISOOUT { + /// Offset (21 bits) + pub const offset: u32 = 21; + /// Mask (1 bit: 1 << 21) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host port interrupt + pub mod HPRTINT { + /// Offset (24 bits) + pub const offset: u32 = 24; + /// Mask (1 bit: 1 << 24) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host channels interrupt + pub mod HCINT { + /// Offset (25 bits) + pub const offset: u32 = 25; + /// Mask (1 bit: 1 << 25) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Periodic TxFIFO empty + pub mod PTXFE { + /// Offset (26 bits) + pub const offset: u32 = 26; + /// Mask (1 bit: 1 << 26) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Connector ID status change + pub mod CIDSCHG { + /// Offset (28 bits) + pub const offset: u32 = 28; + /// Mask (1 bit: 1 << 28) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Disconnect detected interrupt + pub mod DISCINT { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (1 bit: 1 << 29) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Session request/new session detected interrupt + pub mod SRQINT { + /// Offset (30 bits) + pub const offset: u32 = 30; + /// Mask (1 bit: 1 << 30) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Resume/remote wakeup detected interrupt + pub mod WKUPINT { + /// Offset (31 bits) + pub const offset: u32 = 31; + /// Mask (1 bit: 1 << 31) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS interrupt mask register (OTG_FS_GINTMSK) +pub mod GINTMSK { + + /// Mode mismatch interrupt mask + pub mod MMISM { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OTG interrupt mask + pub mod OTGINT { + /// Offset (2 bits) + pub const offset: u32 = 2; + /// Mask (1 bit: 1 << 2) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Start of frame mask + pub mod SOFM { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Receive FIFO non-empty mask + pub mod RXFLVLM { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Non-periodic TxFIFO empty mask + pub mod NPTXFEM { + /// Offset (5 bits) + pub const offset: u32 = 5; + /// Mask (1 bit: 1 << 5) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Global non-periodic IN NAK effective mask + pub mod GINAKEFFM { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Global OUT NAK effective mask + pub mod GONAKEFFM { + /// Offset (7 bits) + pub const offset: u32 = 7; + /// Mask (1 bit: 1 << 7) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Early suspend mask + pub mod ESUSPM { + /// Offset (10 bits) + pub const offset: u32 = 10; + /// Mask (1 bit: 1 << 10) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// USB suspend mask + pub mod USBSUSPM { + /// Offset (11 bits) + pub const offset: u32 = 11; + /// Mask (1 bit: 1 << 11) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// USB reset mask + pub mod USBRST { + /// Offset (12 bits) + pub const offset: u32 = 12; + /// Mask (1 bit: 1 << 12) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Enumeration done mask + pub mod ENUMDNEM { + /// Offset (13 bits) + pub const offset: u32 = 13; + /// Mask (1 bit: 1 << 13) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Isochronous OUT packet dropped interrupt mask + pub mod ISOODRPM { + /// Offset (14 bits) + pub const offset: u32 = 14; + /// Mask (1 bit: 1 << 14) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// End of periodic frame interrupt mask + pub mod EOPFM { + /// Offset (15 bits) + pub const offset: u32 = 15; + /// Mask (1 bit: 1 << 15) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint mismatch interrupt mask + pub mod EPMISM { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (1 bit: 1 << 17) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN endpoints interrupt mask + pub mod IEPINT { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (1 bit: 1 << 18) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OUT endpoints interrupt mask + pub mod OEPINT { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (1 bit: 1 << 19) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Incomplete isochronous IN transfer mask + pub mod IISOIXFRM { + /// Offset (20 bits) + pub const offset: u32 = 20; + /// Mask (1 bit: 1 << 20) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Incomplete periodic transfer mask(Host mode)/Incomplete isochronous OUT transfer mask(Device mode) + pub mod IPXFRM_IISOOXFRM { + /// Offset (21 bits) + pub const offset: u32 = 21; + /// Mask (1 bit: 1 << 21) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host port interrupt mask + pub mod PRTIM { + /// Offset (24 bits) + pub const offset: u32 = 24; + /// Mask (1 bit: 1 << 24) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host channels interrupt mask + pub mod HCIM { + /// Offset (25 bits) + pub const offset: u32 = 25; + /// Mask (1 bit: 1 << 25) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Periodic TxFIFO empty mask + pub mod PTXFEM { + /// Offset (26 bits) + pub const offset: u32 = 26; + /// Mask (1 bit: 1 << 26) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Connector ID status change mask + pub mod CIDSCHGM { + /// Offset (28 bits) + pub const offset: u32 = 28; + /// Mask (1 bit: 1 << 28) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Disconnect detected interrupt mask + pub mod DISCINT { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (1 bit: 1 << 29) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Session request/new session detected interrupt mask + pub mod SRQIM { + /// Offset (30 bits) + pub const offset: u32 = 30; + /// Mask (1 bit: 1 << 30) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Resume/remote wakeup detected interrupt mask + pub mod WUIM { + /// Offset (31 bits) + pub const offset: u32 = 31; + /// Mask (1 bit: 1 << 31) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// GRXSTSR_Device and GRXSTSR_Host +/// GRXSTSR_Device: OTG_FS Receive status debug read(Device mode) +/// GRXSTSR_Host: OTG_FS Receive status debug read(Host mode) +pub mod GRXSTSR { + + /// Endpoint number + pub mod EPNUM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (4 bits: 0b1111 << 0) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Byte count + pub mod BCNT { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (11 bits: 0x7ff << 4) + pub const mask: u32 = 0x7ff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Data PID + pub mod DPID { + /// Offset (15 bits) + pub const offset: u32 = 15; + /// Mask (2 bits: 0b11 << 15) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Packet status + pub mod PKTSTS { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (4 bits: 0b1111 << 17) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Frame number + pub mod FRMNUM { + /// Offset (21 bits) + pub const offset: u32 = 21; + /// Mask (4 bits: 0b1111 << 21) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS Receive FIFO size register (OTG_FS_GRXFSIZ) +pub mod GRXFSIZ { + + /// RxFIFO depth + pub mod RXFD { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// DIEPTXF0 and HNPTXFSIZ +/// DIEPTXF0: OTG_FS non-periodic transmit FIFO size register (Device mode) +/// HNPTXFSIZ: OTG_FS non-periodic transmit FIFO size register (Host mode) +pub mod DIEPTXF0 { + + /// Endpoint 0 transmit RAM start address + pub mod TX0FSA { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint 0 TxFIFO depth + pub mod TX0FD { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (16 bits: 0xffff << 16) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Non-periodic transmit RAM start address + pub mod NPTXFSA { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Non-periodic TxFIFO depth + pub mod NPTXFD { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (16 bits: 0xffff << 16) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS non-periodic transmit FIFO/queue status register (OTG_FS_GNPTXSTS) +pub mod GNPTXSTS { + + /// Non-periodic TxFIFO space available + pub mod NPTXFSAV { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Non-periodic transmit request queue space available + pub mod NPTQXSAV { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (8 bits: 0xff << 16) + pub const mask: u32 = 0xff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Top of the non-periodic transmit request queue + pub mod NPTXQTOP { + /// Offset (24 bits) + pub const offset: u32 = 24; + /// Mask (7 bits: 0x7f << 24) + pub const mask: u32 = 0x7f << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS general core configuration register (OTG_FS_GCCFG) +pub mod GCCFG { + + /// Power down + pub mod PWRDWN { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (1 bit: 1 << 16) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Enable the VBUS sensing device + pub mod VBUSASEN { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (1 bit: 1 << 18) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Enable the VBUS sensing device + pub mod VBUSBSEN { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (1 bit: 1 << 19) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// SOF output enable + pub mod SOFOUTEN { + /// Offset (20 bits) + pub const offset: u32 = 20; + /// Mask (1 bit: 1 << 20) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// core ID register +pub mod CID { + + /// Product ID field + pub mod PRODUCT_ID { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (32 bits: 0xffffffff << 0) + pub const mask: u32 = 0xffffffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS Host periodic transmit FIFO size register (OTG_FS_HPTXFSIZ) +pub mod HPTXFSIZ { + + /// Host periodic TxFIFO start address + pub mod PTXSA { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host periodic TxFIFO depth + pub mod PTXFSIZ { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (16 bits: 0xffff << 16) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS device IN endpoint transmit FIFO size register (OTG_FS_DIEPTXF2) +pub mod DIEPTXF1 { + + /// IN endpoint FIFO2 transmit RAM start address + pub mod INEPTXSA { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN endpoint TxFIFO depth + pub mod INEPTXFD { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (16 bits: 0xffff << 16) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS device IN endpoint transmit FIFO size register (OTG_FS_DIEPTXF2) +pub mod DIEPTXF2 { + pub use super::DIEPTXF1::INEPTXFD; + pub use super::DIEPTXF1::INEPTXSA; +} + +/// OTG_FS device IN endpoint transmit FIFO size register (OTG_FS_DIEPTXF2) +pub mod DIEPTXF3 { + pub use super::DIEPTXF1::INEPTXFD; + pub use super::DIEPTXF1::INEPTXSA; +} + +/// GRXSTSP_Device and GRXSTSP_Host +/// GRXSTSP_Device: OTG status read and pop (device mode) +/// GRXSTSP_Host: OTG status read and pop (host mode) +pub mod GRXSTSP { + + /// Status phase start + pub mod STSPHST { + /// Offset (27 bits) + pub const offset: u32 = 27; + /// Mask (1 bit: 1 << 27) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Frame number + pub mod FRMNUM { + /// Offset (21 bits) + pub const offset: u32 = 21; + /// Mask (4 bits: 0b1111 << 21) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Packet status + pub mod PKTSTS { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (4 bits: 0b1111 << 17) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Data PID + pub mod DPID { + /// Offset (15 bits) + pub const offset: u32 = 15; + /// Mask (2 bits: 0b11 << 15) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Byte count + pub mod BCNT { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (11 bits: 0x7ff << 4) + pub const mask: u32 = 0x7ff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint number + pub mod EPNUM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (4 bits: 0b1111 << 0) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Channel number + pub mod CHNUM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (4 bits: 0b1111 << 0) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} +#[repr(C)] +pub struct RegisterBlock { + /// OTG_FS control and status register (OTG_FS_GOTGCTL) + pub GOTGCTL: RWRegister, + + /// OTG_FS interrupt register (OTG_FS_GOTGINT) + pub GOTGINT: RWRegister, + + /// OTG_FS AHB configuration register (OTG_FS_GAHBCFG) + pub GAHBCFG: RWRegister, + + /// OTG_FS USB configuration register (OTG_FS_GUSBCFG) + pub GUSBCFG: RWRegister, + + /// OTG_FS reset register (OTG_FS_GRSTCTL) + pub GRSTCTL: RWRegister, + + /// OTG_FS core interrupt register (OTG_FS_GINTSTS) + pub GINTSTS: RWRegister, + + /// OTG_FS interrupt mask register (OTG_FS_GINTMSK) + pub GINTMSK: RWRegister, + + /// GRXSTSR_Device and GRXSTSR_Host + /// GRXSTSR_Device: OTG_FS Receive status debug read(Device mode) + /// GRXSTSR_Host: OTG_FS Receive status debug read(Host mode) + pub GRXSTSR: RWRegister, + + /// GRXSTSP_Device and GRXSTSP_Host + /// GRXSTSP_Device: OTG status read and pop (device mode) + /// GRXSTSP_Host: OTG status read and pop (host mode) + pub GRXSTSP: RWRegister, + + /// OTG_FS Receive FIFO size register (OTG_FS_GRXFSIZ) + pub GRXFSIZ: RWRegister, + + /// DIEPTXF0 and HNPTXFSIZ + /// DIEPTXF0: OTG_FS non-periodic transmit FIFO size register (Device mode) + /// HNPTXFSIZ: OTG_FS non-periodic transmit FIFO size register (Host mode) + pub DIEPTXF0: RWRegister, + + /// OTG_FS non-periodic transmit FIFO/queue status register (OTG_FS_GNPTXSTS) + pub GNPTXSTS: RORegister, + + _reserved1: [u32; 2], + + /// OTG_FS general core configuration register (OTG_FS_GCCFG) + pub GCCFG: RWRegister, + + /// core ID register + pub CID: RWRegister, + + _reserved2: [u32; 48], + + /// OTG_FS Host periodic transmit FIFO size register (OTG_FS_HPTXFSIZ) + pub HPTXFSIZ: RWRegister, + + /// OTG_FS device IN endpoint transmit FIFO size register (OTG_FS_DIEPTXF2) + pub DIEPTXF1: RWRegister, + + /// OTG_FS device IN endpoint transmit FIFO size register (OTG_FS_DIEPTXF2) + pub DIEPTXF2: RWRegister, + + /// OTG_FS device IN endpoint transmit FIFO size register (OTG_FS_DIEPTXF2) + pub DIEPTXF3: RWRegister, +} +pub struct ResetValues { + pub GOTGCTL: u32, + pub GOTGINT: u32, + pub GAHBCFG: u32, + pub GUSBCFG: u32, + pub GRSTCTL: u32, + pub GINTSTS: u32, + pub GINTMSK: u32, + pub GRXSTSR: u32, + pub GRXSTSP: u32, + pub GRXFSIZ: u32, + pub DIEPTXF0: u32, + pub GNPTXSTS: u32, + pub GCCFG: u32, + pub CID: u32, + pub HPTXFSIZ: u32, + pub DIEPTXF1: u32, + pub DIEPTXF2: u32, + pub DIEPTXF3: u32, +} +#[cfg(not(feature = "nosync"))] +pub struct Instance { + pub(crate) addr: u32, + pub(crate) _marker: PhantomData<*const RegisterBlock>, +} +#[cfg(not(feature = "nosync"))] +impl ::core::ops::Deref for Instance { + type Target = RegisterBlock; + #[inline(always)] + fn deref(&self) -> &RegisterBlock { + unsafe { &*(self.addr as *const _) } + } +} +#[cfg(feature = "rtfm")] +unsafe impl Send for Instance {} diff --git a/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_fs_host.rs b/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_fs_host.rs new file mode 100644 index 0000000..da571cd --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_fs_host.rs @@ -0,0 +1,1370 @@ +#![allow(non_snake_case, non_upper_case_globals)] +#![allow(non_camel_case_types)] +//! USB on the go full speed +//! +//! Used by: stm32f401, stm32f405, stm32f407, stm32f411, stm32f412, stm32f413, stm32f427, stm32f429, stm32f446, stm32f469 + +use super::super::register::{RORegister, RWRegister}; +#[cfg(not(feature = "nosync"))] +use core::marker::PhantomData; + +/// OTG_FS host configuration register (OTG_FS_HCFG) +pub mod HCFG { + + /// FS/LS PHY clock select + pub mod FSLSPCS { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (2 bits: 0b11 << 0) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// FS- and LS-only support + pub mod FSLSS { + /// Offset (2 bits) + pub const offset: u32 = 2; + /// Mask (1 bit: 1 << 2) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS Host frame interval register +pub mod HFIR { + + /// Frame interval + pub mod FRIVL { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS host frame number/frame time remaining register (OTG_FS_HFNUM) +pub mod HFNUM { + + /// Frame number + pub mod FRNUM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Frame time remaining + pub mod FTREM { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (16 bits: 0xffff << 16) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS_Host periodic transmit FIFO/queue status register (OTG_FS_HPTXSTS) +pub mod HPTXSTS { + + /// Periodic transmit data FIFO space available + pub mod PTXFSAVL { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Periodic transmit request queue space available + pub mod PTXQSAV { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (8 bits: 0xff << 16) + pub const mask: u32 = 0xff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Top of the periodic transmit request queue + pub mod PTXQTOP { + /// Offset (24 bits) + pub const offset: u32 = 24; + /// Mask (8 bits: 0xff << 24) + pub const mask: u32 = 0xff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS Host all channels interrupt register +pub mod HAINT { + + /// Channel interrupts + pub mod HAINT { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS host all channels interrupt mask register +pub mod HAINTMSK { + + /// Channel interrupt mask + pub mod HAINTM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS host port control and status register (OTG_FS_HPRT) +pub mod HPRT { + + /// Port connect status + pub mod PCSTS { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port connect detected + pub mod PCDET { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port enable + pub mod PENA { + /// Offset (2 bits) + pub const offset: u32 = 2; + /// Mask (1 bit: 1 << 2) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port enable/disable change + pub mod PENCHNG { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port overcurrent active + pub mod POCA { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port overcurrent change + pub mod POCCHNG { + /// Offset (5 bits) + pub const offset: u32 = 5; + /// Mask (1 bit: 1 << 5) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port resume + pub mod PRES { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port suspend + pub mod PSUSP { + /// Offset (7 bits) + pub const offset: u32 = 7; + /// Mask (1 bit: 1 << 7) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port reset + pub mod PRST { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port line status + pub mod PLSTS { + /// Offset (10 bits) + pub const offset: u32 = 10; + /// Mask (2 bits: 0b11 << 10) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port power + pub mod PPWR { + /// Offset (12 bits) + pub const offset: u32 = 12; + /// Mask (1 bit: 1 << 12) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port test control + pub mod PTCTL { + /// Offset (13 bits) + pub const offset: u32 = 13; + /// Mask (4 bits: 0b1111 << 13) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port speed + pub mod PSPD { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (2 bits: 0b11 << 17) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS host channel-0 characteristics register (OTG_FS_HCCHAR0) +pub mod HCCHAR0 { + + /// Maximum packet size + pub mod MPSIZ { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (11 bits: 0x7ff << 0) + pub const mask: u32 = 0x7ff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint number + pub mod EPNUM { + /// Offset (11 bits) + pub const offset: u32 = 11; + /// Mask (4 bits: 0b1111 << 11) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint direction + pub mod EPDIR { + /// Offset (15 bits) + pub const offset: u32 = 15; + /// Mask (1 bit: 1 << 15) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Low-speed device + pub mod LSDEV { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (1 bit: 1 << 17) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint type + pub mod EPTYP { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (2 bits: 0b11 << 18) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Multicount + pub mod MCNT { + /// Offset (20 bits) + pub const offset: u32 = 20; + /// Mask (2 bits: 0b11 << 20) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Device address + pub mod DAD { + /// Offset (22 bits) + pub const offset: u32 = 22; + /// Mask (7 bits: 0x7f << 22) + pub const mask: u32 = 0x7f << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Odd frame + pub mod ODDFRM { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (1 bit: 1 << 29) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Channel disable + pub mod CHDIS { + /// Offset (30 bits) + pub const offset: u32 = 30; + /// Mask (1 bit: 1 << 30) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Channel enable + pub mod CHENA { + /// Offset (31 bits) + pub const offset: u32 = 31; + /// Mask (1 bit: 1 << 31) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS host channel-1 characteristics register (OTG_FS_HCCHAR1) +pub mod HCCHAR1 { + pub use super::HCCHAR0::CHDIS; + pub use super::HCCHAR0::CHENA; + pub use super::HCCHAR0::DAD; + pub use super::HCCHAR0::EPDIR; + pub use super::HCCHAR0::EPNUM; + pub use super::HCCHAR0::EPTYP; + pub use super::HCCHAR0::LSDEV; + pub use super::HCCHAR0::MCNT; + pub use super::HCCHAR0::MPSIZ; + pub use super::HCCHAR0::ODDFRM; +} + +/// OTG_FS host channel-2 characteristics register (OTG_FS_HCCHAR2) +pub mod HCCHAR2 { + pub use super::HCCHAR0::CHDIS; + pub use super::HCCHAR0::CHENA; + pub use super::HCCHAR0::DAD; + pub use super::HCCHAR0::EPDIR; + pub use super::HCCHAR0::EPNUM; + pub use super::HCCHAR0::EPTYP; + pub use super::HCCHAR0::LSDEV; + pub use super::HCCHAR0::MCNT; + pub use super::HCCHAR0::MPSIZ; + pub use super::HCCHAR0::ODDFRM; +} + +/// OTG_FS host channel-3 characteristics register (OTG_FS_HCCHAR3) +pub mod HCCHAR3 { + pub use super::HCCHAR0::CHDIS; + pub use super::HCCHAR0::CHENA; + pub use super::HCCHAR0::DAD; + pub use super::HCCHAR0::EPDIR; + pub use super::HCCHAR0::EPNUM; + pub use super::HCCHAR0::EPTYP; + pub use super::HCCHAR0::LSDEV; + pub use super::HCCHAR0::MCNT; + pub use super::HCCHAR0::MPSIZ; + pub use super::HCCHAR0::ODDFRM; +} + +/// OTG_FS host channel-4 characteristics register (OTG_FS_HCCHAR4) +pub mod HCCHAR4 { + pub use super::HCCHAR0::CHDIS; + pub use super::HCCHAR0::CHENA; + pub use super::HCCHAR0::DAD; + pub use super::HCCHAR0::EPDIR; + pub use super::HCCHAR0::EPNUM; + pub use super::HCCHAR0::EPTYP; + pub use super::HCCHAR0::LSDEV; + pub use super::HCCHAR0::MCNT; + pub use super::HCCHAR0::MPSIZ; + pub use super::HCCHAR0::ODDFRM; +} + +/// OTG_FS host channel-5 characteristics register (OTG_FS_HCCHAR5) +pub mod HCCHAR5 { + pub use super::HCCHAR0::CHDIS; + pub use super::HCCHAR0::CHENA; + pub use super::HCCHAR0::DAD; + pub use super::HCCHAR0::EPDIR; + pub use super::HCCHAR0::EPNUM; + pub use super::HCCHAR0::EPTYP; + pub use super::HCCHAR0::LSDEV; + pub use super::HCCHAR0::MCNT; + pub use super::HCCHAR0::MPSIZ; + pub use super::HCCHAR0::ODDFRM; +} + +/// OTG_FS host channel-6 characteristics register (OTG_FS_HCCHAR6) +pub mod HCCHAR6 { + pub use super::HCCHAR0::CHDIS; + pub use super::HCCHAR0::CHENA; + pub use super::HCCHAR0::DAD; + pub use super::HCCHAR0::EPDIR; + pub use super::HCCHAR0::EPNUM; + pub use super::HCCHAR0::EPTYP; + pub use super::HCCHAR0::LSDEV; + pub use super::HCCHAR0::MCNT; + pub use super::HCCHAR0::MPSIZ; + pub use super::HCCHAR0::ODDFRM; +} + +/// OTG_FS host channel-7 characteristics register (OTG_FS_HCCHAR7) +pub mod HCCHAR7 { + pub use super::HCCHAR0::CHDIS; + pub use super::HCCHAR0::CHENA; + pub use super::HCCHAR0::DAD; + pub use super::HCCHAR0::EPDIR; + pub use super::HCCHAR0::EPNUM; + pub use super::HCCHAR0::EPTYP; + pub use super::HCCHAR0::LSDEV; + pub use super::HCCHAR0::MCNT; + pub use super::HCCHAR0::MPSIZ; + pub use super::HCCHAR0::ODDFRM; +} + +/// OTG_FS host channel-0 interrupt register (OTG_FS_HCINT0) +pub mod HCINT0 { + + /// Transfer completed + pub mod XFRC { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Channel halted + pub mod CHH { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// STALL response received interrupt + pub mod STALL { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// NAK response received interrupt + pub mod NAK { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// ACK response received/transmitted interrupt + pub mod ACK { + /// Offset (5 bits) + pub const offset: u32 = 5; + /// Mask (1 bit: 1 << 5) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Transaction error + pub mod TXERR { + /// Offset (7 bits) + pub const offset: u32 = 7; + /// Mask (1 bit: 1 << 7) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Babble error + pub mod BBERR { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Frame overrun + pub mod FRMOR { + /// Offset (9 bits) + pub const offset: u32 = 9; + /// Mask (1 bit: 1 << 9) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Data toggle error + pub mod DTERR { + /// Offset (10 bits) + pub const offset: u32 = 10; + /// Mask (1 bit: 1 << 10) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS host channel-1 interrupt register (OTG_FS_HCINT1) +pub mod HCINT1 { + pub use super::HCINT0::ACK; + pub use super::HCINT0::BBERR; + pub use super::HCINT0::CHH; + pub use super::HCINT0::DTERR; + pub use super::HCINT0::FRMOR; + pub use super::HCINT0::NAK; + pub use super::HCINT0::STALL; + pub use super::HCINT0::TXERR; + pub use super::HCINT0::XFRC; +} + +/// OTG_FS host channel-2 interrupt register (OTG_FS_HCINT2) +pub mod HCINT2 { + pub use super::HCINT0::ACK; + pub use super::HCINT0::BBERR; + pub use super::HCINT0::CHH; + pub use super::HCINT0::DTERR; + pub use super::HCINT0::FRMOR; + pub use super::HCINT0::NAK; + pub use super::HCINT0::STALL; + pub use super::HCINT0::TXERR; + pub use super::HCINT0::XFRC; +} + +/// OTG_FS host channel-3 interrupt register (OTG_FS_HCINT3) +pub mod HCINT3 { + pub use super::HCINT0::ACK; + pub use super::HCINT0::BBERR; + pub use super::HCINT0::CHH; + pub use super::HCINT0::DTERR; + pub use super::HCINT0::FRMOR; + pub use super::HCINT0::NAK; + pub use super::HCINT0::STALL; + pub use super::HCINT0::TXERR; + pub use super::HCINT0::XFRC; +} + +/// OTG_FS host channel-4 interrupt register (OTG_FS_HCINT4) +pub mod HCINT4 { + pub use super::HCINT0::ACK; + pub use super::HCINT0::BBERR; + pub use super::HCINT0::CHH; + pub use super::HCINT0::DTERR; + pub use super::HCINT0::FRMOR; + pub use super::HCINT0::NAK; + pub use super::HCINT0::STALL; + pub use super::HCINT0::TXERR; + pub use super::HCINT0::XFRC; +} + +/// OTG_FS host channel-5 interrupt register (OTG_FS_HCINT5) +pub mod HCINT5 { + pub use super::HCINT0::ACK; + pub use super::HCINT0::BBERR; + pub use super::HCINT0::CHH; + pub use super::HCINT0::DTERR; + pub use super::HCINT0::FRMOR; + pub use super::HCINT0::NAK; + pub use super::HCINT0::STALL; + pub use super::HCINT0::TXERR; + pub use super::HCINT0::XFRC; +} + +/// OTG_FS host channel-6 interrupt register (OTG_FS_HCINT6) +pub mod HCINT6 { + pub use super::HCINT0::ACK; + pub use super::HCINT0::BBERR; + pub use super::HCINT0::CHH; + pub use super::HCINT0::DTERR; + pub use super::HCINT0::FRMOR; + pub use super::HCINT0::NAK; + pub use super::HCINT0::STALL; + pub use super::HCINT0::TXERR; + pub use super::HCINT0::XFRC; +} + +/// OTG_FS host channel-7 interrupt register (OTG_FS_HCINT7) +pub mod HCINT7 { + pub use super::HCINT0::ACK; + pub use super::HCINT0::BBERR; + pub use super::HCINT0::CHH; + pub use super::HCINT0::DTERR; + pub use super::HCINT0::FRMOR; + pub use super::HCINT0::NAK; + pub use super::HCINT0::STALL; + pub use super::HCINT0::TXERR; + pub use super::HCINT0::XFRC; +} + +/// OTG_FS host channel-0 mask register (OTG_FS_HCINTMSK0) +pub mod HCINTMSK0 { + + /// Transfer completed mask + pub mod XFRCM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Channel halted mask + pub mod CHHM { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// STALL response received interrupt mask + pub mod STALLM { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// NAK response received interrupt mask + pub mod NAKM { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// ACK response received/transmitted interrupt mask + pub mod ACKM { + /// Offset (5 bits) + pub const offset: u32 = 5; + /// Mask (1 bit: 1 << 5) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// response received interrupt mask + pub mod NYET { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Transaction error mask + pub mod TXERRM { + /// Offset (7 bits) + pub const offset: u32 = 7; + /// Mask (1 bit: 1 << 7) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Babble error mask + pub mod BBERRM { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Frame overrun mask + pub mod FRMORM { + /// Offset (9 bits) + pub const offset: u32 = 9; + /// Mask (1 bit: 1 << 9) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Data toggle error mask + pub mod DTERRM { + /// Offset (10 bits) + pub const offset: u32 = 10; + /// Mask (1 bit: 1 << 10) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS host channel-1 mask register (OTG_FS_HCINTMSK1) +pub mod HCINTMSK1 { + pub use super::HCINTMSK0::ACKM; + pub use super::HCINTMSK0::BBERRM; + pub use super::HCINTMSK0::CHHM; + pub use super::HCINTMSK0::DTERRM; + pub use super::HCINTMSK0::FRMORM; + pub use super::HCINTMSK0::NAKM; + pub use super::HCINTMSK0::NYET; + pub use super::HCINTMSK0::STALLM; + pub use super::HCINTMSK0::TXERRM; + pub use super::HCINTMSK0::XFRCM; +} + +/// OTG_FS host channel-2 mask register (OTG_FS_HCINTMSK2) +pub mod HCINTMSK2 { + pub use super::HCINTMSK0::ACKM; + pub use super::HCINTMSK0::BBERRM; + pub use super::HCINTMSK0::CHHM; + pub use super::HCINTMSK0::DTERRM; + pub use super::HCINTMSK0::FRMORM; + pub use super::HCINTMSK0::NAKM; + pub use super::HCINTMSK0::NYET; + pub use super::HCINTMSK0::STALLM; + pub use super::HCINTMSK0::TXERRM; + pub use super::HCINTMSK0::XFRCM; +} + +/// OTG_FS host channel-3 mask register (OTG_FS_HCINTMSK3) +pub mod HCINTMSK3 { + pub use super::HCINTMSK0::ACKM; + pub use super::HCINTMSK0::BBERRM; + pub use super::HCINTMSK0::CHHM; + pub use super::HCINTMSK0::DTERRM; + pub use super::HCINTMSK0::FRMORM; + pub use super::HCINTMSK0::NAKM; + pub use super::HCINTMSK0::NYET; + pub use super::HCINTMSK0::STALLM; + pub use super::HCINTMSK0::TXERRM; + pub use super::HCINTMSK0::XFRCM; +} + +/// OTG_FS host channel-4 mask register (OTG_FS_HCINTMSK4) +pub mod HCINTMSK4 { + pub use super::HCINTMSK0::ACKM; + pub use super::HCINTMSK0::BBERRM; + pub use super::HCINTMSK0::CHHM; + pub use super::HCINTMSK0::DTERRM; + pub use super::HCINTMSK0::FRMORM; + pub use super::HCINTMSK0::NAKM; + pub use super::HCINTMSK0::NYET; + pub use super::HCINTMSK0::STALLM; + pub use super::HCINTMSK0::TXERRM; + pub use super::HCINTMSK0::XFRCM; +} + +/// OTG_FS host channel-5 mask register (OTG_FS_HCINTMSK5) +pub mod HCINTMSK5 { + pub use super::HCINTMSK0::ACKM; + pub use super::HCINTMSK0::BBERRM; + pub use super::HCINTMSK0::CHHM; + pub use super::HCINTMSK0::DTERRM; + pub use super::HCINTMSK0::FRMORM; + pub use super::HCINTMSK0::NAKM; + pub use super::HCINTMSK0::NYET; + pub use super::HCINTMSK0::STALLM; + pub use super::HCINTMSK0::TXERRM; + pub use super::HCINTMSK0::XFRCM; +} + +/// OTG_FS host channel-6 mask register (OTG_FS_HCINTMSK6) +pub mod HCINTMSK6 { + pub use super::HCINTMSK0::ACKM; + pub use super::HCINTMSK0::BBERRM; + pub use super::HCINTMSK0::CHHM; + pub use super::HCINTMSK0::DTERRM; + pub use super::HCINTMSK0::FRMORM; + pub use super::HCINTMSK0::NAKM; + pub use super::HCINTMSK0::NYET; + pub use super::HCINTMSK0::STALLM; + pub use super::HCINTMSK0::TXERRM; + pub use super::HCINTMSK0::XFRCM; +} + +/// OTG_FS host channel-7 mask register (OTG_FS_HCINTMSK7) +pub mod HCINTMSK7 { + pub use super::HCINTMSK0::ACKM; + pub use super::HCINTMSK0::BBERRM; + pub use super::HCINTMSK0::CHHM; + pub use super::HCINTMSK0::DTERRM; + pub use super::HCINTMSK0::FRMORM; + pub use super::HCINTMSK0::NAKM; + pub use super::HCINTMSK0::NYET; + pub use super::HCINTMSK0::STALLM; + pub use super::HCINTMSK0::TXERRM; + pub use super::HCINTMSK0::XFRCM; +} + +/// OTG_FS host channel-0 transfer size register +pub mod HCTSIZ0 { + + /// Transfer size + pub mod XFRSIZ { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (19 bits: 0x7ffff << 0) + pub const mask: u32 = 0x7ffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Packet count + pub mod PKTCNT { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (10 bits: 0x3ff << 19) + pub const mask: u32 = 0x3ff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Data PID + pub mod DPID { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (2 bits: 0b11 << 29) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_FS host channel-1 transfer size register +pub mod HCTSIZ1 { + pub use super::HCTSIZ0::DPID; + pub use super::HCTSIZ0::PKTCNT; + pub use super::HCTSIZ0::XFRSIZ; +} + +/// OTG_FS host channel-2 transfer size register +pub mod HCTSIZ2 { + pub use super::HCTSIZ0::DPID; + pub use super::HCTSIZ0::PKTCNT; + pub use super::HCTSIZ0::XFRSIZ; +} + +/// OTG_FS host channel-3 transfer size register +pub mod HCTSIZ3 { + pub use super::HCTSIZ0::DPID; + pub use super::HCTSIZ0::PKTCNT; + pub use super::HCTSIZ0::XFRSIZ; +} + +/// OTG_FS host channel-x transfer size register +pub mod HCTSIZ4 { + pub use super::HCTSIZ0::DPID; + pub use super::HCTSIZ0::PKTCNT; + pub use super::HCTSIZ0::XFRSIZ; +} + +/// OTG_FS host channel-5 transfer size register +pub mod HCTSIZ5 { + pub use super::HCTSIZ0::DPID; + pub use super::HCTSIZ0::PKTCNT; + pub use super::HCTSIZ0::XFRSIZ; +} + +/// OTG_FS host channel-6 transfer size register +pub mod HCTSIZ6 { + pub use super::HCTSIZ0::DPID; + pub use super::HCTSIZ0::PKTCNT; + pub use super::HCTSIZ0::XFRSIZ; +} + +/// OTG_FS host channel-7 transfer size register +pub mod HCTSIZ7 { + pub use super::HCTSIZ0::DPID; + pub use super::HCTSIZ0::PKTCNT; + pub use super::HCTSIZ0::XFRSIZ; +} +#[repr(C)] +pub struct RegisterBlock { + /// OTG_FS host configuration register (OTG_FS_HCFG) + pub HCFG: RWRegister, + + /// OTG_FS Host frame interval register + pub HFIR: RWRegister, + + /// OTG_FS host frame number/frame time remaining register (OTG_FS_HFNUM) + pub HFNUM: RORegister, + + _reserved1: [u32; 1], + + /// OTG_FS_Host periodic transmit FIFO/queue status register (OTG_FS_HPTXSTS) + pub HPTXSTS: RWRegister, + + /// OTG_FS Host all channels interrupt register + pub HAINT: RORegister, + + /// OTG_FS host all channels interrupt mask register + pub HAINTMSK: RWRegister, + + _reserved2: [u32; 9], + + /// OTG_FS host port control and status register (OTG_FS_HPRT) + pub HPRT: RWRegister, + + _reserved3: [u32; 47], + + /// OTG_FS host channel-0 characteristics register (OTG_FS_HCCHAR0) + pub HCCHAR0: RWRegister, + + _reserved4: [u32; 1], + + /// OTG_FS host channel-0 interrupt register (OTG_FS_HCINT0) + pub HCINT0: RWRegister, + + /// OTG_FS host channel-0 mask register (OTG_FS_HCINTMSK0) + pub HCINTMSK0: RWRegister, + + /// OTG_FS host channel-0 transfer size register + pub HCTSIZ0: RWRegister, + + _reserved5: [u32; 3], + + /// OTG_FS host channel-1 characteristics register (OTG_FS_HCCHAR1) + pub HCCHAR1: RWRegister, + + _reserved6: [u32; 1], + + /// OTG_FS host channel-1 interrupt register (OTG_FS_HCINT1) + pub HCINT1: RWRegister, + + /// OTG_FS host channel-1 mask register (OTG_FS_HCINTMSK1) + pub HCINTMSK1: RWRegister, + + /// OTG_FS host channel-1 transfer size register + pub HCTSIZ1: RWRegister, + + _reserved7: [u32; 3], + + /// OTG_FS host channel-2 characteristics register (OTG_FS_HCCHAR2) + pub HCCHAR2: RWRegister, + + _reserved8: [u32; 1], + + /// OTG_FS host channel-2 interrupt register (OTG_FS_HCINT2) + pub HCINT2: RWRegister, + + /// OTG_FS host channel-2 mask register (OTG_FS_HCINTMSK2) + pub HCINTMSK2: RWRegister, + + /// OTG_FS host channel-2 transfer size register + pub HCTSIZ2: RWRegister, + + _reserved9: [u32; 3], + + /// OTG_FS host channel-3 characteristics register (OTG_FS_HCCHAR3) + pub HCCHAR3: RWRegister, + + _reserved10: [u32; 1], + + /// OTG_FS host channel-3 interrupt register (OTG_FS_HCINT3) + pub HCINT3: RWRegister, + + /// OTG_FS host channel-3 mask register (OTG_FS_HCINTMSK3) + pub HCINTMSK3: RWRegister, + + /// OTG_FS host channel-3 transfer size register + pub HCTSIZ3: RWRegister, + + _reserved11: [u32; 3], + + /// OTG_FS host channel-4 characteristics register (OTG_FS_HCCHAR4) + pub HCCHAR4: RWRegister, + + _reserved12: [u32; 1], + + /// OTG_FS host channel-4 interrupt register (OTG_FS_HCINT4) + pub HCINT4: RWRegister, + + /// OTG_FS host channel-4 mask register (OTG_FS_HCINTMSK4) + pub HCINTMSK4: RWRegister, + + /// OTG_FS host channel-x transfer size register + pub HCTSIZ4: RWRegister, + + _reserved13: [u32; 3], + + /// OTG_FS host channel-5 characteristics register (OTG_FS_HCCHAR5) + pub HCCHAR5: RWRegister, + + _reserved14: [u32; 1], + + /// OTG_FS host channel-5 interrupt register (OTG_FS_HCINT5) + pub HCINT5: RWRegister, + + /// OTG_FS host channel-5 mask register (OTG_FS_HCINTMSK5) + pub HCINTMSK5: RWRegister, + + /// OTG_FS host channel-5 transfer size register + pub HCTSIZ5: RWRegister, + + _reserved15: [u32; 3], + + /// OTG_FS host channel-6 characteristics register (OTG_FS_HCCHAR6) + pub HCCHAR6: RWRegister, + + _reserved16: [u32; 1], + + /// OTG_FS host channel-6 interrupt register (OTG_FS_HCINT6) + pub HCINT6: RWRegister, + + /// OTG_FS host channel-6 mask register (OTG_FS_HCINTMSK6) + pub HCINTMSK6: RWRegister, + + /// OTG_FS host channel-6 transfer size register + pub HCTSIZ6: RWRegister, + + _reserved17: [u32; 3], + + /// OTG_FS host channel-7 characteristics register (OTG_FS_HCCHAR7) + pub HCCHAR7: RWRegister, + + _reserved18: [u32; 1], + + /// OTG_FS host channel-7 interrupt register (OTG_FS_HCINT7) + pub HCINT7: RWRegister, + + /// OTG_FS host channel-7 mask register (OTG_FS_HCINTMSK7) + pub HCINTMSK7: RWRegister, + + /// OTG_FS host channel-7 transfer size register + pub HCTSIZ7: RWRegister, +} +pub struct ResetValues { + pub HCFG: u32, + pub HFIR: u32, + pub HFNUM: u32, + pub HPTXSTS: u32, + pub HAINT: u32, + pub HAINTMSK: u32, + pub HPRT: u32, + pub HCCHAR0: u32, + pub HCINT0: u32, + pub HCINTMSK0: u32, + pub HCTSIZ0: u32, + pub HCCHAR1: u32, + pub HCINT1: u32, + pub HCINTMSK1: u32, + pub HCTSIZ1: u32, + pub HCCHAR2: u32, + pub HCINT2: u32, + pub HCINTMSK2: u32, + pub HCTSIZ2: u32, + pub HCCHAR3: u32, + pub HCINT3: u32, + pub HCINTMSK3: u32, + pub HCTSIZ3: u32, + pub HCCHAR4: u32, + pub HCINT4: u32, + pub HCINTMSK4: u32, + pub HCTSIZ4: u32, + pub HCCHAR5: u32, + pub HCINT5: u32, + pub HCINTMSK5: u32, + pub HCTSIZ5: u32, + pub HCCHAR6: u32, + pub HCINT6: u32, + pub HCINTMSK6: u32, + pub HCTSIZ6: u32, + pub HCCHAR7: u32, + pub HCINT7: u32, + pub HCINTMSK7: u32, + pub HCTSIZ7: u32, +} +#[cfg(not(feature = "nosync"))] +pub struct Instance { + pub(crate) addr: u32, + pub(crate) _marker: PhantomData<*const RegisterBlock>, +} +#[cfg(not(feature = "nosync"))] +impl ::core::ops::Deref for Instance { + type Target = RegisterBlock; + #[inline(always)] + fn deref(&self) -> &RegisterBlock { + unsafe { &*(self.addr as *const _) } + } +} +#[cfg(feature = "rtfm")] +unsafe impl Send for Instance {} diff --git a/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_fs_pwrclk.rs b/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_fs_pwrclk.rs new file mode 100644 index 0000000..cca413b --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_fs_pwrclk.rs @@ -0,0 +1,78 @@ +#![allow(non_snake_case, non_upper_case_globals)] +#![allow(non_camel_case_types)] +//! USB on the go full speed +//! +//! Used by: stm32f401, stm32f405, stm32f407, stm32f411, stm32f412, stm32f413, stm32f427, stm32f429, stm32f446, stm32f469 + +use super::super::register::RWRegister; +#[cfg(not(feature = "nosync"))] +use core::marker::PhantomData; + +/// OTG_FS power and clock gating control register +pub mod PCGCCTL { + + /// Stop PHY clock + pub mod STPPCLK { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Gate HCLK + pub mod GATEHCLK { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// PHY Suspended + pub mod PHYSUSP { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} +#[repr(C)] +pub struct RegisterBlock { + /// OTG_FS power and clock gating control register + pub PCGCCTL: RWRegister, +} +pub struct ResetValues { + pub PCGCCTL: u32, +} +#[cfg(not(feature = "nosync"))] +pub struct Instance { + pub(crate) addr: u32, + pub(crate) _marker: PhantomData<*const RegisterBlock>, +} +#[cfg(not(feature = "nosync"))] +impl ::core::ops::Deref for Instance { + type Target = RegisterBlock; + #[inline(always)] + fn deref(&self) -> &RegisterBlock { + unsafe { &*(self.addr as *const _) } + } +} +#[cfg(feature = "rtfm")] +unsafe impl Send for Instance {} diff --git a/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_hs_device.rs b/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_hs_device.rs new file mode 100644 index 0000000..4da30c0 --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_hs_device.rs @@ -0,0 +1,2772 @@ +#![allow(non_snake_case, non_upper_case_globals)] +#![allow(non_camel_case_types)] +//! USB on the go high speed +//! +//! Used by: stm32f405, stm32f407, stm32f427, stm32f429, stm32f446 + +use super::super::register::{RORegister, RWRegister, UnsafeRWRegister}; +#[cfg(not(feature = "nosync"))] +use core::marker::PhantomData; + +/// OTG_HS device configuration register +pub mod DCFG { + + /// Device speed + pub mod DSPD { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (2 bits: 0b11 << 0) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Nonzero-length status OUT handshake + pub mod NZLSOHSK { + /// Offset (2 bits) + pub const offset: u32 = 2; + /// Mask (1 bit: 1 << 2) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Device address + pub mod DAD { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (7 bits: 0x7f << 4) + pub const mask: u32 = 0x7f << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Periodic (micro)frame interval + pub mod PFIVL { + /// Offset (11 bits) + pub const offset: u32 = 11; + /// Mask (2 bits: 0b11 << 11) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + #[cfg(feature = "xcvrdly")] + /// Transceiver Delay + pub mod XCVRDLY { + /// Offset (14 bits) + pub const offset: u32 = 14; + /// Mask (1 bit: 1 << 14) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Periodic scheduling interval + pub mod PERSCHIVL { + /// Offset (24 bits) + pub const offset: u32 = 24; + /// Mask (2 bits: 0b11 << 24) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device control register +pub mod DCTL { + + /// Remote wakeup signaling + pub mod RWUSIG { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Soft disconnect + pub mod SDIS { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Global IN NAK status + pub mod GINSTS { + /// Offset (2 bits) + pub const offset: u32 = 2; + /// Mask (1 bit: 1 << 2) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Global OUT NAK status + pub mod GONSTS { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Test control + pub mod TCTL { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (3 bits: 0b111 << 4) + pub const mask: u32 = 0b111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Set global IN NAK + pub mod SGINAK { + /// Offset (7 bits) + pub const offset: u32 = 7; + /// Mask (1 bit: 1 << 7) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Clear global IN NAK + pub mod CGINAK { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Set global OUT NAK + pub mod SGONAK { + /// Offset (9 bits) + pub const offset: u32 = 9; + /// Mask (1 bit: 1 << 9) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Clear global OUT NAK + pub mod CGONAK { + /// Offset (10 bits) + pub const offset: u32 = 10; + /// Mask (1 bit: 1 << 10) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Power-on programming done + pub mod POPRGDNE { + /// Offset (11 bits) + pub const offset: u32 = 11; + /// Mask (1 bit: 1 << 11) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device status register +pub mod DSTS { + + /// Suspend status + pub mod SUSPSTS { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Enumerated speed + pub mod ENUMSPD { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (2 bits: 0b11 << 1) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Erratic error + pub mod EERR { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Frame number of the received SOF + pub mod FNSOF { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (14 bits: 0x3fff << 8) + pub const mask: u32 = 0x3fff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device IN endpoint common interrupt mask register +pub mod DIEPMSK { + + /// Transfer completed interrupt mask + pub mod XFRCM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint disabled interrupt mask + pub mod EPDM { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Timeout condition mask (nonisochronous endpoints) + pub mod TOM { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN token received when TxFIFO empty mask + pub mod ITTXFEMSK { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN token received with EP mismatch mask + pub mod INEPNMM { + /// Offset (5 bits) + pub const offset: u32 = 5; + /// Mask (1 bit: 1 << 5) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN endpoint NAK effective mask + pub mod INEPNEM { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// FIFO underrun mask + pub mod TXFURM { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// BNA interrupt mask + pub mod BIM { + /// Offset (9 bits) + pub const offset: u32 = 9; + /// Mask (1 bit: 1 << 9) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device OUT endpoint common interrupt mask register +pub mod DOEPMSK { + + /// Transfer completed interrupt mask + pub mod XFRCM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint disabled interrupt mask + pub mod EPDM { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// SETUP phase done mask + pub mod STUPM { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OUT token received when endpoint disabled mask + pub mod OTEPDM { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Back-to-back SETUP packets received mask + pub mod B2BSTUP { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OUT packet error mask + pub mod OPEM { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// BNA interrupt mask + pub mod BOIM { + /// Offset (9 bits) + pub const offset: u32 = 9; + /// Mask (1 bit: 1 << 9) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device all endpoints interrupt register +pub mod DAINT { + + /// IN endpoint interrupt bits + pub mod IEPINT { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OUT endpoint interrupt bits + pub mod OEPINT { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (16 bits: 0xffff << 16) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS all endpoints interrupt mask register +pub mod DAINTMSK { + + /// IN EP interrupt mask bits + pub mod IEPM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OUT EP interrupt mask bits + pub mod OEPM { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (16 bits: 0xffff << 16) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device VBUS discharge time register +pub mod DVBUSDIS { + + /// Device VBUS discharge time + pub mod VBUSDT { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device VBUS pulsing time register +pub mod DVBUSPULSE { + + /// Device VBUS pulsing time + pub mod DVBUSP { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (12 bits: 0xfff << 0) + pub const mask: u32 = 0xfff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS Device threshold control register +pub mod DTHRCTL { + + /// Nonisochronous IN endpoints threshold enable + pub mod NONISOTHREN { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// ISO IN endpoint threshold enable + pub mod ISOTHREN { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Transmit threshold length + pub mod TXTHRLEN { + /// Offset (2 bits) + pub const offset: u32 = 2; + /// Mask (9 bits: 0x1ff << 2) + pub const mask: u32 = 0x1ff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Receive threshold enable + pub mod RXTHREN { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (1 bit: 1 << 16) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Receive threshold length + pub mod RXTHRLEN { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (9 bits: 0x1ff << 17) + pub const mask: u32 = 0x1ff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Arbiter parking enable + pub mod ARPEN { + /// Offset (27 bits) + pub const offset: u32 = 27; + /// Mask (1 bit: 1 << 27) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device IN endpoint FIFO empty interrupt mask register +pub mod DIEPEMPMSK { + + /// IN EP Tx FIFO empty interrupt mask bits + pub mod INEPTXFEM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device each endpoint interrupt register +pub mod DEACHINT { + + /// IN endpoint 1interrupt bit + pub mod IEP1INT { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OUT endpoint 1 interrupt bit + pub mod OEP1INT { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (1 bit: 1 << 17) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device each endpoint interrupt register mask +pub mod DEACHINTMSK { + + /// IN Endpoint 1 interrupt mask bit + pub mod IEP1INTM { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OUT Endpoint 1 interrupt mask bit + pub mod OEP1INTM { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (1 bit: 1 << 17) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device each in endpoint-1 interrupt register +pub mod DIEPEACHMSK1 { + + /// Transfer completed interrupt mask + pub mod XFRCM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint disabled interrupt mask + pub mod EPDM { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Timeout condition mask (nonisochronous endpoints) + pub mod TOM { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN token received when TxFIFO empty mask + pub mod ITTXFEMSK { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN token received with EP mismatch mask + pub mod INEPNMM { + /// Offset (5 bits) + pub const offset: u32 = 5; + /// Mask (1 bit: 1 << 5) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN endpoint NAK effective mask + pub mod INEPNEM { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// FIFO underrun mask + pub mod TXFURM { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// BNA interrupt mask + pub mod BIM { + /// Offset (9 bits) + pub const offset: u32 = 9; + /// Mask (1 bit: 1 << 9) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// NAK interrupt mask + pub mod NAKM { + /// Offset (13 bits) + pub const offset: u32 = 13; + /// Mask (1 bit: 1 << 13) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device each OUT endpoint-1 interrupt register +pub mod DOEPEACHMSK1 { + + /// Transfer completed interrupt mask + pub mod XFRCM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint disabled interrupt mask + pub mod EPDM { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Timeout condition mask + pub mod TOM { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN token received when TxFIFO empty mask + pub mod ITTXFEMSK { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN token received with EP mismatch mask + pub mod INEPNMM { + /// Offset (5 bits) + pub const offset: u32 = 5; + /// Mask (1 bit: 1 << 5) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN endpoint NAK effective mask + pub mod INEPNEM { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OUT packet error mask + pub mod TXFURM { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// BNA interrupt mask + pub mod BIM { + /// Offset (9 bits) + pub const offset: u32 = 9; + /// Mask (1 bit: 1 << 9) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Bubble error interrupt mask + pub mod BERRM { + /// Offset (12 bits) + pub const offset: u32 = 12; + /// Mask (1 bit: 1 << 12) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// NAK interrupt mask + pub mod NAKM { + /// Offset (13 bits) + pub const offset: u32 = 13; + /// Mask (1 bit: 1 << 13) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// NYET interrupt mask + pub mod NYETM { + /// Offset (14 bits) + pub const offset: u32 = 14; + /// Mask (1 bit: 1 << 14) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG device endpoint-0 control register +pub mod DIEPCTL0 { + + /// Maximum packet size + pub mod MPSIZ { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (11 bits: 0x7ff << 0) + pub const mask: u32 = 0x7ff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// USB active endpoint + pub mod USBAEP { + /// Offset (15 bits) + pub const offset: u32 = 15; + /// Mask (1 bit: 1 << 15) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Even/odd frame + pub mod EONUM_DPID { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (1 bit: 1 << 16) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// NAK status + pub mod NAKSTS { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (1 bit: 1 << 17) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint type + pub mod EPTYP { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (2 bits: 0b11 << 18) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// STALL handshake + pub mod STALL { + /// Offset (21 bits) + pub const offset: u32 = 21; + /// Mask (1 bit: 1 << 21) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// TxFIFO number + pub mod TXFNUM { + /// Offset (22 bits) + pub const offset: u32 = 22; + /// Mask (4 bits: 0b1111 << 22) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Clear NAK + pub mod CNAK { + /// Offset (26 bits) + pub const offset: u32 = 26; + /// Mask (1 bit: 1 << 26) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Set NAK + pub mod SNAK { + /// Offset (27 bits) + pub const offset: u32 = 27; + /// Mask (1 bit: 1 << 27) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Set DATA0 PID + pub mod SD0PID_SEVNFRM { + /// Offset (28 bits) + pub const offset: u32 = 28; + /// Mask (1 bit: 1 << 28) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Set odd frame + pub mod SODDFRM { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (1 bit: 1 << 29) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint disable + pub mod EPDIS { + /// Offset (30 bits) + pub const offset: u32 = 30; + /// Mask (1 bit: 1 << 30) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint enable + pub mod EPENA { + /// Offset (31 bits) + pub const offset: u32 = 31; + /// Mask (1 bit: 1 << 31) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG device endpoint-1 control register +pub mod DIEPCTL1 { + pub use super::DIEPCTL0::CNAK; + pub use super::DIEPCTL0::EONUM_DPID; + pub use super::DIEPCTL0::EPDIS; + pub use super::DIEPCTL0::EPENA; + pub use super::DIEPCTL0::EPTYP; + pub use super::DIEPCTL0::MPSIZ; + pub use super::DIEPCTL0::NAKSTS; + pub use super::DIEPCTL0::SD0PID_SEVNFRM; + pub use super::DIEPCTL0::SNAK; + pub use super::DIEPCTL0::SODDFRM; + pub use super::DIEPCTL0::STALL; + pub use super::DIEPCTL0::TXFNUM; + pub use super::DIEPCTL0::USBAEP; +} + +/// OTG device endpoint-1 control register +pub mod DIEPCTL2 { + pub use super::DIEPCTL0::CNAK; + pub use super::DIEPCTL0::EONUM_DPID; + pub use super::DIEPCTL0::EPDIS; + pub use super::DIEPCTL0::EPENA; + pub use super::DIEPCTL0::EPTYP; + pub use super::DIEPCTL0::MPSIZ; + pub use super::DIEPCTL0::NAKSTS; + pub use super::DIEPCTL0::SD0PID_SEVNFRM; + pub use super::DIEPCTL0::SNAK; + pub use super::DIEPCTL0::SODDFRM; + pub use super::DIEPCTL0::STALL; + pub use super::DIEPCTL0::TXFNUM; + pub use super::DIEPCTL0::USBAEP; +} + +/// OTG device endpoint-1 control register +pub mod DIEPCTL3 { + pub use super::DIEPCTL0::CNAK; + pub use super::DIEPCTL0::EONUM_DPID; + pub use super::DIEPCTL0::EPDIS; + pub use super::DIEPCTL0::EPENA; + pub use super::DIEPCTL0::EPTYP; + pub use super::DIEPCTL0::MPSIZ; + pub use super::DIEPCTL0::NAKSTS; + pub use super::DIEPCTL0::SD0PID_SEVNFRM; + pub use super::DIEPCTL0::SNAK; + pub use super::DIEPCTL0::SODDFRM; + pub use super::DIEPCTL0::STALL; + pub use super::DIEPCTL0::TXFNUM; + pub use super::DIEPCTL0::USBAEP; +} + +/// OTG device endpoint-1 control register +pub mod DIEPCTL4 { + pub use super::DIEPCTL0::CNAK; + pub use super::DIEPCTL0::EONUM_DPID; + pub use super::DIEPCTL0::EPDIS; + pub use super::DIEPCTL0::EPENA; + pub use super::DIEPCTL0::EPTYP; + pub use super::DIEPCTL0::MPSIZ; + pub use super::DIEPCTL0::NAKSTS; + pub use super::DIEPCTL0::SD0PID_SEVNFRM; + pub use super::DIEPCTL0::SNAK; + pub use super::DIEPCTL0::SODDFRM; + pub use super::DIEPCTL0::STALL; + pub use super::DIEPCTL0::TXFNUM; + pub use super::DIEPCTL0::USBAEP; +} + +/// OTG device endpoint-1 control register +pub mod DIEPCTL5 { + pub use super::DIEPCTL0::CNAK; + pub use super::DIEPCTL0::EONUM_DPID; + pub use super::DIEPCTL0::EPDIS; + pub use super::DIEPCTL0::EPENA; + pub use super::DIEPCTL0::EPTYP; + pub use super::DIEPCTL0::MPSIZ; + pub use super::DIEPCTL0::NAKSTS; + pub use super::DIEPCTL0::SD0PID_SEVNFRM; + pub use super::DIEPCTL0::SNAK; + pub use super::DIEPCTL0::SODDFRM; + pub use super::DIEPCTL0::STALL; + pub use super::DIEPCTL0::TXFNUM; + pub use super::DIEPCTL0::USBAEP; +} + +/// OTG device endpoint-0 interrupt register +pub mod DIEPINT0 { + + /// Transfer completed interrupt + pub mod XFRC { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint disabled interrupt + pub mod EPDISD { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Timeout condition + pub mod TOC { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN token received when TxFIFO is empty + pub mod ITTXFE { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN endpoint NAK effective + pub mod INEPNE { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Transmit FIFO empty + pub mod TXFE { + /// Offset (7 bits) + pub const offset: u32 = 7; + /// Mask (1 bit: 1 << 7) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Transmit Fifo Underrun + pub mod TXFIFOUDRN { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Buffer not available interrupt + pub mod BNA { + /// Offset (9 bits) + pub const offset: u32 = 9; + /// Mask (1 bit: 1 << 9) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Packet dropped status + pub mod PKTDRPSTS { + /// Offset (11 bits) + pub const offset: u32 = 11; + /// Mask (1 bit: 1 << 11) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Babble error interrupt + pub mod BERR { + /// Offset (12 bits) + pub const offset: u32 = 12; + /// Mask (1 bit: 1 << 12) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// NAK interrupt + pub mod NAK { + /// Offset (13 bits) + pub const offset: u32 = 13; + /// Mask (1 bit: 1 << 13) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG device endpoint-0 interrupt register +pub mod DIEPINT1 { + pub use super::DIEPINT0::BERR; + pub use super::DIEPINT0::BNA; + pub use super::DIEPINT0::EPDISD; + pub use super::DIEPINT0::INEPNE; + pub use super::DIEPINT0::ITTXFE; + pub use super::DIEPINT0::NAK; + pub use super::DIEPINT0::PKTDRPSTS; + pub use super::DIEPINT0::TOC; + pub use super::DIEPINT0::TXFE; + pub use super::DIEPINT0::TXFIFOUDRN; + pub use super::DIEPINT0::XFRC; +} + +/// OTG device endpoint-0 interrupt register +pub mod DIEPINT2 { + pub use super::DIEPINT0::BERR; + pub use super::DIEPINT0::BNA; + pub use super::DIEPINT0::EPDISD; + pub use super::DIEPINT0::INEPNE; + pub use super::DIEPINT0::ITTXFE; + pub use super::DIEPINT0::NAK; + pub use super::DIEPINT0::PKTDRPSTS; + pub use super::DIEPINT0::TOC; + pub use super::DIEPINT0::TXFE; + pub use super::DIEPINT0::TXFIFOUDRN; + pub use super::DIEPINT0::XFRC; +} + +/// OTG device endpoint-0 interrupt register +pub mod DIEPINT3 { + pub use super::DIEPINT0::BERR; + pub use super::DIEPINT0::BNA; + pub use super::DIEPINT0::EPDISD; + pub use super::DIEPINT0::INEPNE; + pub use super::DIEPINT0::ITTXFE; + pub use super::DIEPINT0::NAK; + pub use super::DIEPINT0::PKTDRPSTS; + pub use super::DIEPINT0::TOC; + pub use super::DIEPINT0::TXFE; + pub use super::DIEPINT0::TXFIFOUDRN; + pub use super::DIEPINT0::XFRC; +} + +/// OTG device endpoint-0 interrupt register +pub mod DIEPINT4 { + pub use super::DIEPINT0::BERR; + pub use super::DIEPINT0::BNA; + pub use super::DIEPINT0::EPDISD; + pub use super::DIEPINT0::INEPNE; + pub use super::DIEPINT0::ITTXFE; + pub use super::DIEPINT0::NAK; + pub use super::DIEPINT0::PKTDRPSTS; + pub use super::DIEPINT0::TOC; + pub use super::DIEPINT0::TXFE; + pub use super::DIEPINT0::TXFIFOUDRN; + pub use super::DIEPINT0::XFRC; +} + +/// OTG device endpoint-0 interrupt register +pub mod DIEPINT5 { + pub use super::DIEPINT0::BERR; + pub use super::DIEPINT0::BNA; + pub use super::DIEPINT0::EPDISD; + pub use super::DIEPINT0::INEPNE; + pub use super::DIEPINT0::ITTXFE; + pub use super::DIEPINT0::NAK; + pub use super::DIEPINT0::PKTDRPSTS; + pub use super::DIEPINT0::TOC; + pub use super::DIEPINT0::TXFE; + pub use super::DIEPINT0::TXFIFOUDRN; + pub use super::DIEPINT0::XFRC; +} + +/// OTG_HS device IN endpoint 0 transfer size register +pub mod DIEPTSIZ0 { + + /// Transfer size + pub mod XFRSIZ { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (7 bits: 0x7f << 0) + pub const mask: u32 = 0x7f << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Packet count + pub mod PKTCNT { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (2 bits: 0b11 << 19) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device endpoint-1 DMA address register +pub mod DIEPDMA1 { + + /// DMA address + pub mod DMAADDR { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (32 bits: 0xffffffff << 0) + pub const mask: u32 = 0xffffffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device endpoint-2 DMA address register +pub mod DIEPDMA2 { + pub use super::DIEPDMA1::DMAADDR; +} + +/// OTG_HS device endpoint-3 DMA address register +pub mod DIEPDMA3 { + pub use super::DIEPDMA1::DMAADDR; +} + +/// OTG_HS device endpoint-4 DMA address register +pub mod DIEPDMA4 { + pub use super::DIEPDMA1::DMAADDR; +} + +/// OTG_HS device endpoint-5 DMA address register +pub mod DIEPDMA5 { + pub use super::DIEPDMA1::DMAADDR; +} + +/// OTG_HS device IN endpoint transmit FIFO status register +pub mod DTXFSTS0 { + + /// IN endpoint TxFIFO space avail + pub mod INEPTFSAV { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device IN endpoint transmit FIFO status register +pub mod DTXFSTS1 { + pub use super::DTXFSTS0::INEPTFSAV; +} + +/// OTG_HS device IN endpoint transmit FIFO status register +pub mod DTXFSTS2 { + pub use super::DTXFSTS0::INEPTFSAV; +} + +/// OTG_HS device IN endpoint transmit FIFO status register +pub mod DTXFSTS3 { + pub use super::DTXFSTS0::INEPTFSAV; +} + +/// OTG_HS device IN endpoint transmit FIFO status register +pub mod DTXFSTS4 { + pub use super::DTXFSTS0::INEPTFSAV; +} + +/// OTG_HS device IN endpoint transmit FIFO status register +pub mod DTXFSTS5 { + pub use super::DTXFSTS0::INEPTFSAV; +} + +/// OTG_HS device endpoint transfer size register +pub mod DIEPTSIZ1 { + + /// Transfer size + pub mod XFRSIZ { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (19 bits: 0x7ffff << 0) + pub const mask: u32 = 0x7ffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Packet count + pub mod PKTCNT { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (10 bits: 0x3ff << 19) + pub const mask: u32 = 0x3ff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Multi count + pub mod MCNT { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (2 bits: 0b11 << 29) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device endpoint transfer size register +pub mod DIEPTSIZ2 { + pub use super::DIEPTSIZ1::MCNT; + pub use super::DIEPTSIZ1::PKTCNT; + pub use super::DIEPTSIZ1::XFRSIZ; +} + +/// OTG_HS device endpoint transfer size register +pub mod DIEPTSIZ3 { + pub use super::DIEPTSIZ1::MCNT; + pub use super::DIEPTSIZ1::PKTCNT; + pub use super::DIEPTSIZ1::XFRSIZ; +} + +/// OTG_HS device endpoint transfer size register +pub mod DIEPTSIZ4 { + pub use super::DIEPTSIZ1::MCNT; + pub use super::DIEPTSIZ1::PKTCNT; + pub use super::DIEPTSIZ1::XFRSIZ; +} + +/// OTG_HS device endpoint transfer size register +pub mod DIEPTSIZ5 { + pub use super::DIEPTSIZ1::MCNT; + pub use super::DIEPTSIZ1::PKTCNT; + pub use super::DIEPTSIZ1::XFRSIZ; +} + +/// OTG_HS device control OUT endpoint 0 control register +pub mod DOEPCTL0 { + + /// Maximum packet size + pub mod MPSIZ { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (2 bits: 0b11 << 0) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// USB active endpoint + pub mod USBAEP { + /// Offset (15 bits) + pub const offset: u32 = 15; + /// Mask (1 bit: 1 << 15) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// NAK status + pub mod NAKSTS { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (1 bit: 1 << 17) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint type + pub mod EPTYP { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (2 bits: 0b11 << 18) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Snoop mode + pub mod SNPM { + /// Offset (20 bits) + pub const offset: u32 = 20; + /// Mask (1 bit: 1 << 20) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// STALL handshake + pub mod STALL { + /// Offset (21 bits) + pub const offset: u32 = 21; + /// Mask (1 bit: 1 << 21) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Clear NAK + pub mod CNAK { + /// Offset (26 bits) + pub const offset: u32 = 26; + /// Mask (1 bit: 1 << 26) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Set NAK + pub mod SNAK { + /// Offset (27 bits) + pub const offset: u32 = 27; + /// Mask (1 bit: 1 << 27) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint disable + pub mod EPDIS { + /// Offset (30 bits) + pub const offset: u32 = 30; + /// Mask (1 bit: 1 << 30) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint enable + pub mod EPENA { + /// Offset (31 bits) + pub const offset: u32 = 31; + /// Mask (1 bit: 1 << 31) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG device endpoint-1 control register +pub mod DOEPCTL1 { + + /// Maximum packet size + pub mod MPSIZ { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (11 bits: 0x7ff << 0) + pub const mask: u32 = 0x7ff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// USB active endpoint + pub mod USBAEP { + /// Offset (15 bits) + pub const offset: u32 = 15; + /// Mask (1 bit: 1 << 15) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Even odd frame/Endpoint data PID + pub mod EONUM_DPID { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (1 bit: 1 << 16) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// NAK status + pub mod NAKSTS { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (1 bit: 1 << 17) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint type + pub mod EPTYP { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (2 bits: 0b11 << 18) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Snoop mode + pub mod SNPM { + /// Offset (20 bits) + pub const offset: u32 = 20; + /// Mask (1 bit: 1 << 20) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// STALL handshake + pub mod STALL { + /// Offset (21 bits) + pub const offset: u32 = 21; + /// Mask (1 bit: 1 << 21) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Clear NAK + pub mod CNAK { + /// Offset (26 bits) + pub const offset: u32 = 26; + /// Mask (1 bit: 1 << 26) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Set NAK + pub mod SNAK { + /// Offset (27 bits) + pub const offset: u32 = 27; + /// Mask (1 bit: 1 << 27) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Set DATA0 PID/Set even frame + pub mod SD0PID_SEVNFRM { + /// Offset (28 bits) + pub const offset: u32 = 28; + /// Mask (1 bit: 1 << 28) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Set odd frame + pub mod SODDFRM { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (1 bit: 1 << 29) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint disable + pub mod EPDIS { + /// Offset (30 bits) + pub const offset: u32 = 30; + /// Mask (1 bit: 1 << 30) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint enable + pub mod EPENA { + /// Offset (31 bits) + pub const offset: u32 = 31; + /// Mask (1 bit: 1 << 31) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG device endpoint-1 control register +pub mod DOEPCTL2 { + pub use super::DOEPCTL1::CNAK; + pub use super::DOEPCTL1::EONUM_DPID; + pub use super::DOEPCTL1::EPDIS; + pub use super::DOEPCTL1::EPENA; + pub use super::DOEPCTL1::EPTYP; + pub use super::DOEPCTL1::MPSIZ; + pub use super::DOEPCTL1::NAKSTS; + pub use super::DOEPCTL1::SD0PID_SEVNFRM; + pub use super::DOEPCTL1::SNAK; + pub use super::DOEPCTL1::SNPM; + pub use super::DOEPCTL1::SODDFRM; + pub use super::DOEPCTL1::STALL; + pub use super::DOEPCTL1::USBAEP; +} + +/// OTG device endpoint-1 control register +pub mod DOEPCTL3 { + pub use super::DOEPCTL1::CNAK; + pub use super::DOEPCTL1::EONUM_DPID; + pub use super::DOEPCTL1::EPDIS; + pub use super::DOEPCTL1::EPENA; + pub use super::DOEPCTL1::EPTYP; + pub use super::DOEPCTL1::MPSIZ; + pub use super::DOEPCTL1::NAKSTS; + pub use super::DOEPCTL1::SD0PID_SEVNFRM; + pub use super::DOEPCTL1::SNAK; + pub use super::DOEPCTL1::SNPM; + pub use super::DOEPCTL1::SODDFRM; + pub use super::DOEPCTL1::STALL; + pub use super::DOEPCTL1::USBAEP; +} + +/// OTG device endpoint-1 control register +pub mod DOEPCTL4 { + pub use super::DOEPCTL1::CNAK; + pub use super::DOEPCTL1::EONUM_DPID; + pub use super::DOEPCTL1::EPDIS; + pub use super::DOEPCTL1::EPENA; + pub use super::DOEPCTL1::EPTYP; + pub use super::DOEPCTL1::MPSIZ; + pub use super::DOEPCTL1::NAKSTS; + pub use super::DOEPCTL1::SD0PID_SEVNFRM; + pub use super::DOEPCTL1::SNAK; + pub use super::DOEPCTL1::SNPM; + pub use super::DOEPCTL1::SODDFRM; + pub use super::DOEPCTL1::STALL; + pub use super::DOEPCTL1::USBAEP; +} + +/// OTG device endpoint-1 control register +pub mod DOEPCTL5 { + pub use super::DOEPCTL1::CNAK; + pub use super::DOEPCTL1::EONUM_DPID; + pub use super::DOEPCTL1::EPDIS; + pub use super::DOEPCTL1::EPENA; + pub use super::DOEPCTL1::EPTYP; + pub use super::DOEPCTL1::MPSIZ; + pub use super::DOEPCTL1::NAKSTS; + pub use super::DOEPCTL1::SD0PID_SEVNFRM; + pub use super::DOEPCTL1::SNAK; + pub use super::DOEPCTL1::SNPM; + pub use super::DOEPCTL1::SODDFRM; + pub use super::DOEPCTL1::STALL; + pub use super::DOEPCTL1::USBAEP; +} + +/// OTG_HS device endpoint-0 interrupt register +pub mod DOEPINT0 { + + /// Transfer completed interrupt + pub mod XFRC { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint disabled interrupt + pub mod EPDISD { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// SETUP phase done + pub mod STUP { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OUT token received when endpoint disabled + pub mod OTEPDIS { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Back-to-back SETUP packets received + pub mod B2BSTUP { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// NYET interrupt + pub mod NYET { + /// Offset (14 bits) + pub const offset: u32 = 14; + /// Mask (1 bit: 1 << 14) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device endpoint-0 interrupt register +pub mod DOEPINT1 { + pub use super::DOEPINT0::B2BSTUP; + pub use super::DOEPINT0::EPDISD; + pub use super::DOEPINT0::NYET; + pub use super::DOEPINT0::OTEPDIS; + pub use super::DOEPINT0::STUP; + pub use super::DOEPINT0::XFRC; +} + +/// OTG_HS device endpoint-0 interrupt register +pub mod DOEPINT2 { + pub use super::DOEPINT0::B2BSTUP; + pub use super::DOEPINT0::EPDISD; + pub use super::DOEPINT0::NYET; + pub use super::DOEPINT0::OTEPDIS; + pub use super::DOEPINT0::STUP; + pub use super::DOEPINT0::XFRC; +} + +/// OTG_HS device endpoint-0 interrupt register +pub mod DOEPINT3 { + pub use super::DOEPINT0::B2BSTUP; + pub use super::DOEPINT0::EPDISD; + pub use super::DOEPINT0::NYET; + pub use super::DOEPINT0::OTEPDIS; + pub use super::DOEPINT0::STUP; + pub use super::DOEPINT0::XFRC; +} + +/// OTG_HS device endpoint-0 interrupt register +pub mod DOEPINT4 { + pub use super::DOEPINT0::B2BSTUP; + pub use super::DOEPINT0::EPDISD; + pub use super::DOEPINT0::NYET; + pub use super::DOEPINT0::OTEPDIS; + pub use super::DOEPINT0::STUP; + pub use super::DOEPINT0::XFRC; +} + +/// OTG_HS device endpoint-0 interrupt register +pub mod DOEPINT5 { + pub use super::DOEPINT0::B2BSTUP; + pub use super::DOEPINT0::EPDISD; + pub use super::DOEPINT0::NYET; + pub use super::DOEPINT0::OTEPDIS; + pub use super::DOEPINT0::STUP; + pub use super::DOEPINT0::XFRC; +} + +/// OTG_HS device endpoint-1 transfer size register +pub mod DOEPTSIZ0 { + + /// Transfer size + pub mod XFRSIZ { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (7 bits: 0x7f << 0) + pub const mask: u32 = 0x7f << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Packet count + pub mod PKTCNT { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (1 bit: 1 << 19) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// SETUP packet count + pub mod STUPCNT { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (2 bits: 0b11 << 29) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device endpoint-2 transfer size register +pub mod DOEPTSIZ1 { + + /// Transfer size + pub mod XFRSIZ { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (19 bits: 0x7ffff << 0) + pub const mask: u32 = 0x7ffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Packet count + pub mod PKTCNT { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (10 bits: 0x3ff << 19) + pub const mask: u32 = 0x3ff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Received data PID/SETUP packet count + pub mod RXDPID_STUPCNT { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (2 bits: 0b11 << 29) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device endpoint-2 transfer size register +pub mod DOEPTSIZ2 { + pub use super::DOEPTSIZ1::PKTCNT; + pub use super::DOEPTSIZ1::RXDPID_STUPCNT; + pub use super::DOEPTSIZ1::XFRSIZ; +} + +/// OTG_HS device endpoint-2 transfer size register +pub mod DOEPTSIZ3 { + pub use super::DOEPTSIZ1::PKTCNT; + pub use super::DOEPTSIZ1::RXDPID_STUPCNT; + pub use super::DOEPTSIZ1::XFRSIZ; +} + +/// OTG_HS device endpoint-2 transfer size register +pub mod DOEPTSIZ4 { + pub use super::DOEPTSIZ1::PKTCNT; + pub use super::DOEPTSIZ1::RXDPID_STUPCNT; + pub use super::DOEPTSIZ1::XFRSIZ; +} + +/// OTG_HS device endpoint-2 transfer size register +pub mod DOEPTSIZ5 { + pub use super::DOEPTSIZ1::PKTCNT; + pub use super::DOEPTSIZ1::RXDPID_STUPCNT; + pub use super::DOEPTSIZ1::XFRSIZ; +} +#[repr(C)] +pub struct RegisterBlock { + /// OTG_HS device configuration register + pub DCFG: RWRegister, + + /// OTG_HS device control register + pub DCTL: RWRegister, + + /// OTG_HS device status register + pub DSTS: RORegister, + + _reserved1: [u32; 1], + + /// OTG_HS device IN endpoint common interrupt mask register + pub DIEPMSK: RWRegister, + + /// OTG_HS device OUT endpoint common interrupt mask register + pub DOEPMSK: RWRegister, + + /// OTG_HS device all endpoints interrupt register + pub DAINT: RORegister, + + /// OTG_HS all endpoints interrupt mask register + pub DAINTMSK: RWRegister, + + _reserved2: [u32; 2], + + /// OTG_HS device VBUS discharge time register + pub DVBUSDIS: RWRegister, + + /// OTG_HS device VBUS pulsing time register + pub DVBUSPULSE: RWRegister, + + /// OTG_HS Device threshold control register + pub DTHRCTL: RWRegister, + + /// OTG_HS device IN endpoint FIFO empty interrupt mask register + pub DIEPEMPMSK: RWRegister, + + /// OTG_HS device each endpoint interrupt register + pub DEACHINT: RWRegister, + + /// OTG_HS device each endpoint interrupt register mask + pub DEACHINTMSK: RWRegister, + + /// OTG_HS device each in endpoint-1 interrupt register + pub DIEPEACHMSK1: RWRegister, + + _reserved3: [u32; 15], + + /// OTG_HS device each OUT endpoint-1 interrupt register + pub DOEPEACHMSK1: RWRegister, + + _reserved4: [u32; 31], + + /// OTG device endpoint-0 control register + pub DIEPCTL0: RWRegister, + + _reserved5: [u32; 1], + + /// OTG device endpoint-0 interrupt register + pub DIEPINT0: RWRegister, + + _reserved6: [u32; 1], + + /// OTG_HS device IN endpoint 0 transfer size register + pub DIEPTSIZ0: RWRegister, + + /// OTG_HS device endpoint-1 DMA address register + pub DIEPDMA1: UnsafeRWRegister, + + /// OTG_HS device IN endpoint transmit FIFO status register + pub DTXFSTS0: RORegister, + + _reserved7: [u32; 1], + + /// OTG device endpoint-1 control register + pub DIEPCTL1: RWRegister, + + _reserved8: [u32; 1], + + /// OTG device endpoint-0 interrupt register + pub DIEPINT1: RWRegister, + + _reserved9: [u32; 1], + + /// OTG_HS device endpoint transfer size register + pub DIEPTSIZ1: RWRegister, + + /// OTG_HS device endpoint-2 DMA address register + pub DIEPDMA2: UnsafeRWRegister, + + /// OTG_HS device IN endpoint transmit FIFO status register + pub DTXFSTS1: RORegister, + + _reserved10: [u32; 1], + + /// OTG device endpoint-1 control register + pub DIEPCTL2: RWRegister, + + _reserved11: [u32; 1], + + /// OTG device endpoint-0 interrupt register + pub DIEPINT2: RWRegister, + + _reserved12: [u32; 1], + + /// OTG_HS device endpoint transfer size register + pub DIEPTSIZ2: RWRegister, + + /// OTG_HS device endpoint-3 DMA address register + pub DIEPDMA3: UnsafeRWRegister, + + /// OTG_HS device IN endpoint transmit FIFO status register + pub DTXFSTS2: RORegister, + + _reserved13: [u32; 1], + + /// OTG device endpoint-1 control register + pub DIEPCTL3: RWRegister, + + _reserved14: [u32; 1], + + /// OTG device endpoint-0 interrupt register + pub DIEPINT3: RWRegister, + + _reserved15: [u32; 1], + + /// OTG_HS device endpoint transfer size register + pub DIEPTSIZ3: RWRegister, + + /// OTG_HS device endpoint-4 DMA address register + pub DIEPDMA4: UnsafeRWRegister, + + /// OTG_HS device IN endpoint transmit FIFO status register + pub DTXFSTS3: RORegister, + + _reserved16: [u32; 1], + + /// OTG device endpoint-1 control register + pub DIEPCTL4: RWRegister, + + _reserved17: [u32; 1], + + /// OTG device endpoint-0 interrupt register + pub DIEPINT4: RWRegister, + + _reserved18: [u32; 1], + + /// OTG_HS device endpoint transfer size register + pub DIEPTSIZ4: RWRegister, + + /// OTG_HS device endpoint-5 DMA address register + pub DIEPDMA5: UnsafeRWRegister, + + /// OTG_HS device IN endpoint transmit FIFO status register + pub DTXFSTS4: RORegister, + + _reserved19: [u32; 1], + + /// OTG device endpoint-1 control register + pub DIEPCTL5: RWRegister, + + _reserved20: [u32; 1], + + /// OTG device endpoint-0 interrupt register + pub DIEPINT5: RWRegister, + + _reserved21: [u32; 1], + + /// OTG_HS device endpoint transfer size register + pub DIEPTSIZ5: RWRegister, + + _reserved22: [u32; 1], + + /// OTG_HS device IN endpoint transmit FIFO status register + pub DTXFSTS5: RORegister, + + _reserved23: [u32; 81], + + /// OTG_HS device control OUT endpoint 0 control register + pub DOEPCTL0: RWRegister, + + _reserved24: [u32; 1], + + /// OTG_HS device endpoint-0 interrupt register + pub DOEPINT0: RWRegister, + + _reserved25: [u32; 1], + + /// OTG_HS device endpoint-1 transfer size register + pub DOEPTSIZ0: RWRegister, + + _reserved26: [u32; 3], + + /// OTG device endpoint-1 control register + pub DOEPCTL1: RWRegister, + + _reserved27: [u32; 1], + + /// OTG_HS device endpoint-0 interrupt register + pub DOEPINT1: RWRegister, + + _reserved28: [u32; 1], + + /// OTG_HS device endpoint-2 transfer size register + pub DOEPTSIZ1: RWRegister, + + _reserved29: [u32; 3], + + /// OTG device endpoint-1 control register + pub DOEPCTL2: RWRegister, + + _reserved30: [u32; 1], + + /// OTG_HS device endpoint-0 interrupt register + pub DOEPINT2: RWRegister, + + _reserved31: [u32; 1], + + /// OTG_HS device endpoint-2 transfer size register + pub DOEPTSIZ2: RWRegister, + + _reserved32: [u32; 3], + + /// OTG device endpoint-1 control register + pub DOEPCTL3: RWRegister, + + _reserved33: [u32; 1], + + /// OTG_HS device endpoint-0 interrupt register + pub DOEPINT3: RWRegister, + + _reserved34: [u32; 1], + + /// OTG_HS device endpoint-2 transfer size register + pub DOEPTSIZ3: RWRegister, + + _reserved35: [u32; 3], + + /// OTG device endpoint-1 control register + pub DOEPCTL4: RWRegister, + + _reserved36: [u32; 1], + + /// OTG_HS device endpoint-0 interrupt register + pub DOEPINT4: RWRegister, + + _reserved37: [u32; 1], + + /// OTG_HS device endpoint-2 transfer size register + pub DOEPTSIZ4: RWRegister, + + _reserved38: [u32; 3], + + /// OTG device endpoint-1 control register + pub DOEPCTL5: RWRegister, + + _reserved39: [u32; 1], + + /// OTG_HS device endpoint-0 interrupt register + pub DOEPINT5: RWRegister, + + _reserved40: [u32; 1], + + /// OTG_HS device endpoint-2 transfer size register + pub DOEPTSIZ5: RWRegister, +} +pub struct ResetValues { + pub DCFG: u32, + pub DCTL: u32, + pub DSTS: u32, + pub DIEPMSK: u32, + pub DOEPMSK: u32, + pub DAINT: u32, + pub DAINTMSK: u32, + pub DVBUSDIS: u32, + pub DVBUSPULSE: u32, + pub DTHRCTL: u32, + pub DIEPEMPMSK: u32, + pub DEACHINT: u32, + pub DEACHINTMSK: u32, + pub DIEPEACHMSK1: u32, + pub DOEPEACHMSK1: u32, + pub DIEPCTL0: u32, + pub DIEPINT0: u32, + pub DIEPTSIZ0: u32, + pub DIEPDMA1: u32, + pub DTXFSTS0: u32, + pub DIEPCTL1: u32, + pub DIEPINT1: u32, + pub DIEPTSIZ1: u32, + pub DIEPDMA2: u32, + pub DTXFSTS1: u32, + pub DIEPCTL2: u32, + pub DIEPINT2: u32, + pub DIEPTSIZ2: u32, + pub DIEPDMA3: u32, + pub DTXFSTS2: u32, + pub DIEPCTL3: u32, + pub DIEPINT3: u32, + pub DIEPTSIZ3: u32, + pub DIEPDMA4: u32, + pub DTXFSTS3: u32, + pub DIEPCTL4: u32, + pub DIEPINT4: u32, + pub DIEPTSIZ4: u32, + pub DIEPDMA5: u32, + pub DTXFSTS4: u32, + pub DIEPCTL5: u32, + pub DIEPINT5: u32, + pub DIEPTSIZ5: u32, + pub DTXFSTS5: u32, + pub DOEPCTL0: u32, + pub DOEPINT0: u32, + pub DOEPTSIZ0: u32, + pub DOEPCTL1: u32, + pub DOEPINT1: u32, + pub DOEPTSIZ1: u32, + pub DOEPCTL2: u32, + pub DOEPINT2: u32, + pub DOEPTSIZ2: u32, + pub DOEPCTL3: u32, + pub DOEPINT3: u32, + pub DOEPTSIZ3: u32, + pub DOEPCTL4: u32, + pub DOEPINT4: u32, + pub DOEPTSIZ4: u32, + pub DOEPCTL5: u32, + pub DOEPINT5: u32, + pub DOEPTSIZ5: u32, +} +#[cfg(not(feature = "nosync"))] +pub struct Instance { + pub(crate) addr: u32, + pub(crate) _marker: PhantomData<*const RegisterBlock>, +} +#[cfg(not(feature = "nosync"))] +impl ::core::ops::Deref for Instance { + type Target = RegisterBlock; + #[inline(always)] + fn deref(&self) -> &RegisterBlock { + unsafe { &*(self.addr as *const _) } + } +} +#[cfg(feature = "rtfm")] +unsafe impl Send for Instance {} diff --git a/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_hs_global.rs b/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_hs_global.rs new file mode 100644 index 0000000..6eb1b0f --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_hs_global.rs @@ -0,0 +1,2046 @@ +#![allow(non_snake_case, non_upper_case_globals)] +#![allow(non_camel_case_types)] +//! USB on the go high speed +//! +//! Used by: stm32f405, stm32f407, stm32f427, stm32f429, stm32f446 + +use super::super::register::{RORegister, RWRegister}; +#[cfg(not(feature = "nosync"))] +use core::marker::PhantomData; + +/// OTG_HS control and status register +pub mod GOTGCTL { + + /// Session request success + pub mod SRQSCS { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Session request + pub mod SRQ { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host negotiation success + pub mod HNGSCS { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// HNP request + pub mod HNPRQ { + /// Offset (9 bits) + pub const offset: u32 = 9; + /// Mask (1 bit: 1 << 9) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host set HNP enable + pub mod HSHNPEN { + /// Offset (10 bits) + pub const offset: u32 = 10; + /// Mask (1 bit: 1 << 10) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Device HNP enabled + pub mod DHNPEN { + /// Offset (11 bits) + pub const offset: u32 = 11; + /// Mask (1 bit: 1 << 11) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Connector ID status + pub mod CIDSTS { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (1 bit: 1 << 16) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Long/short debounce time + pub mod DBCT { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (1 bit: 1 << 17) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// A-session valid + pub mod ASVLD { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (1 bit: 1 << 18) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// B-session valid + pub mod BSVLD { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (1 bit: 1 << 19) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS interrupt register +pub mod GOTGINT { + + /// Session end detected + pub mod SEDET { + /// Offset (2 bits) + pub const offset: u32 = 2; + /// Mask (1 bit: 1 << 2) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Session request success status change + pub mod SRSSCHG { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host negotiation success status change + pub mod HNSSCHG { + /// Offset (9 bits) + pub const offset: u32 = 9; + /// Mask (1 bit: 1 << 9) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host negotiation detected + pub mod HNGDET { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (1 bit: 1 << 17) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// A-device timeout change + pub mod ADTOCHG { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (1 bit: 1 << 18) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Debounce done + pub mod DBCDNE { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (1 bit: 1 << 19) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS AHB configuration register +pub mod GAHBCFG { + + /// Global interrupt mask + pub mod GINT { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Burst length/type + pub mod HBSTLEN { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (4 bits: 0b1111 << 1) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// DMA enable + pub mod DMAEN { + /// Offset (5 bits) + pub const offset: u32 = 5; + /// Mask (1 bit: 1 << 5) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// TxFIFO empty level + pub mod TXFELVL { + /// Offset (7 bits) + pub const offset: u32 = 7; + /// Mask (1 bit: 1 << 7) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Periodic TxFIFO empty level + pub mod PTXFELVL { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS USB configuration register +pub mod GUSBCFG { + + /// FS timeout calibration + pub mod TOCAL { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (3 bits: 0b111 << 0) + pub const mask: u32 = 0b111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// USB 2.0 high-speed ULPI PHY or USB 1.1 full-speed serial transceiver select + pub mod PHYSEL { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// SRP-capable + pub mod SRPCAP { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// HNP-capable + pub mod HNPCAP { + /// Offset (9 bits) + pub const offset: u32 = 9; + /// Mask (1 bit: 1 << 9) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// USB turnaround time + pub mod TRDT { + /// Offset (10 bits) + pub const offset: u32 = 10; + /// Mask (4 bits: 0b1111 << 10) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// PHY Low-power clock select + pub mod PHYLPCS { + /// Offset (15 bits) + pub const offset: u32 = 15; + /// Mask (1 bit: 1 << 15) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// ULPI FS/LS select + pub mod ULPIFSLS { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (1 bit: 1 << 17) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// ULPI Auto-resume + pub mod ULPIAR { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (1 bit: 1 << 18) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// ULPI Clock SuspendM + pub mod ULPICSM { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (1 bit: 1 << 19) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// ULPI External VBUS Drive + pub mod ULPIEVBUSD { + /// Offset (20 bits) + pub const offset: u32 = 20; + /// Mask (1 bit: 1 << 20) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// ULPI external VBUS indicator + pub mod ULPIEVBUSI { + /// Offset (21 bits) + pub const offset: u32 = 21; + /// Mask (1 bit: 1 << 21) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// TermSel DLine pulsing selection + pub mod TSDPS { + /// Offset (22 bits) + pub const offset: u32 = 22; + /// Mask (1 bit: 1 << 22) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Indicator complement + pub mod PCCI { + /// Offset (23 bits) + pub const offset: u32 = 23; + /// Mask (1 bit: 1 << 23) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Indicator pass through + pub mod PTCI { + /// Offset (24 bits) + pub const offset: u32 = 24; + /// Mask (1 bit: 1 << 24) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// ULPI interface protect disable + pub mod ULPIIPD { + /// Offset (25 bits) + pub const offset: u32 = 25; + /// Mask (1 bit: 1 << 25) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Forced host mode + pub mod FHMOD { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (1 bit: 1 << 29) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Forced peripheral mode + pub mod FDMOD { + /// Offset (30 bits) + pub const offset: u32 = 30; + /// Mask (1 bit: 1 << 30) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Corrupt Tx packet + pub mod CTXPKT { + /// Offset (31 bits) + pub const offset: u32 = 31; + /// Mask (1 bit: 1 << 31) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS reset register +pub mod GRSTCTL { + + /// Core soft reset + pub mod CSRST { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// HCLK soft reset + pub mod HSRST { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host frame counter reset + pub mod FCRST { + /// Offset (2 bits) + pub const offset: u32 = 2; + /// Mask (1 bit: 1 << 2) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// RxFIFO flush + pub mod RXFFLSH { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// TxFIFO flush + pub mod TXFFLSH { + /// Offset (5 bits) + pub const offset: u32 = 5; + /// Mask (1 bit: 1 << 5) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// TxFIFO number + pub mod TXFNUM { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (5 bits: 0b11111 << 6) + pub const mask: u32 = 0b11111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// DMA request signal + pub mod DMAREQ { + /// Offset (30 bits) + pub const offset: u32 = 30; + /// Mask (1 bit: 1 << 30) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// AHB master idle + pub mod AHBIDL { + /// Offset (31 bits) + pub const offset: u32 = 31; + /// Mask (1 bit: 1 << 31) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS core interrupt register +pub mod GINTSTS { + + /// Current mode of operation + pub mod CMOD { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Mode mismatch interrupt + pub mod MMIS { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OTG interrupt + pub mod OTGINT { + /// Offset (2 bits) + pub const offset: u32 = 2; + /// Mask (1 bit: 1 << 2) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Start of frame + pub mod SOF { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// RxFIFO nonempty + pub mod RXFLVL { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Nonperiodic TxFIFO empty + pub mod NPTXFE { + /// Offset (5 bits) + pub const offset: u32 = 5; + /// Mask (1 bit: 1 << 5) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Global IN nonperiodic NAK effective + pub mod GINAKEFF { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Global OUT NAK effective + pub mod BOUTNAKEFF { + /// Offset (7 bits) + pub const offset: u32 = 7; + /// Mask (1 bit: 1 << 7) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Early suspend + pub mod ESUSP { + /// Offset (10 bits) + pub const offset: u32 = 10; + /// Mask (1 bit: 1 << 10) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// USB suspend + pub mod USBSUSP { + /// Offset (11 bits) + pub const offset: u32 = 11; + /// Mask (1 bit: 1 << 11) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// USB reset + pub mod USBRST { + /// Offset (12 bits) + pub const offset: u32 = 12; + /// Mask (1 bit: 1 << 12) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Enumeration done + pub mod ENUMDNE { + /// Offset (13 bits) + pub const offset: u32 = 13; + /// Mask (1 bit: 1 << 13) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Isochronous OUT packet dropped interrupt + pub mod ISOODRP { + /// Offset (14 bits) + pub const offset: u32 = 14; + /// Mask (1 bit: 1 << 14) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// End of periodic frame interrupt + pub mod EOPF { + /// Offset (15 bits) + pub const offset: u32 = 15; + /// Mask (1 bit: 1 << 15) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN endpoint interrupt + pub mod IEPINT { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (1 bit: 1 << 18) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OUT endpoint interrupt + pub mod OEPINT { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (1 bit: 1 << 19) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Incomplete isochronous IN transfer + pub mod IISOIXFR { + /// Offset (20 bits) + pub const offset: u32 = 20; + /// Mask (1 bit: 1 << 20) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Incomplete periodic transfer + pub mod PXFR_INCOMPISOOUT { + /// Offset (21 bits) + pub const offset: u32 = 21; + /// Mask (1 bit: 1 << 21) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Data fetch suspended + pub mod DATAFSUSP { + /// Offset (22 bits) + pub const offset: u32 = 22; + /// Mask (1 bit: 1 << 22) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host port interrupt + pub mod HPRTINT { + /// Offset (24 bits) + pub const offset: u32 = 24; + /// Mask (1 bit: 1 << 24) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host channels interrupt + pub mod HCINT { + /// Offset (25 bits) + pub const offset: u32 = 25; + /// Mask (1 bit: 1 << 25) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Periodic TxFIFO empty + pub mod PTXFE { + /// Offset (26 bits) + pub const offset: u32 = 26; + /// Mask (1 bit: 1 << 26) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Connector ID status change + pub mod CIDSCHG { + /// Offset (28 bits) + pub const offset: u32 = 28; + /// Mask (1 bit: 1 << 28) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Disconnect detected interrupt + pub mod DISCINT { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (1 bit: 1 << 29) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Session request/new session detected interrupt + pub mod SRQINT { + /// Offset (30 bits) + pub const offset: u32 = 30; + /// Mask (1 bit: 1 << 30) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Resume/remote wakeup detected interrupt + pub mod WKUPINT { + /// Offset (31 bits) + pub const offset: u32 = 31; + /// Mask (1 bit: 1 << 31) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS interrupt mask register +pub mod GINTMSK { + + /// Mode mismatch interrupt mask + pub mod MMISM { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OTG interrupt mask + pub mod OTGINT { + /// Offset (2 bits) + pub const offset: u32 = 2; + /// Mask (1 bit: 1 << 2) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Start of frame mask + pub mod SOFM { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Receive FIFO nonempty mask + pub mod RXFLVLM { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Nonperiodic TxFIFO empty mask + pub mod NPTXFEM { + /// Offset (5 bits) + pub const offset: u32 = 5; + /// Mask (1 bit: 1 << 5) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Global nonperiodic IN NAK effective mask + pub mod GINAKEFFM { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Global OUT NAK effective mask + pub mod GONAKEFFM { + /// Offset (7 bits) + pub const offset: u32 = 7; + /// Mask (1 bit: 1 << 7) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Early suspend mask + pub mod ESUSPM { + /// Offset (10 bits) + pub const offset: u32 = 10; + /// Mask (1 bit: 1 << 10) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// USB suspend mask + pub mod USBSUSPM { + /// Offset (11 bits) + pub const offset: u32 = 11; + /// Mask (1 bit: 1 << 11) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// USB reset mask + pub mod USBRST { + /// Offset (12 bits) + pub const offset: u32 = 12; + /// Mask (1 bit: 1 << 12) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Enumeration done mask + pub mod ENUMDNEM { + /// Offset (13 bits) + pub const offset: u32 = 13; + /// Mask (1 bit: 1 << 13) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Isochronous OUT packet dropped interrupt mask + pub mod ISOODRPM { + /// Offset (14 bits) + pub const offset: u32 = 14; + /// Mask (1 bit: 1 << 14) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// End of periodic frame interrupt mask + pub mod EOPFM { + /// Offset (15 bits) + pub const offset: u32 = 15; + /// Mask (1 bit: 1 << 15) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint mismatch interrupt mask + pub mod EPMISM { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (1 bit: 1 << 17) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN endpoints interrupt mask + pub mod IEPINT { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (1 bit: 1 << 18) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// OUT endpoints interrupt mask + pub mod OEPINT { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (1 bit: 1 << 19) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Incomplete isochronous IN transfer mask + pub mod IISOIXFRM { + /// Offset (20 bits) + pub const offset: u32 = 20; + /// Mask (1 bit: 1 << 20) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Incomplete periodic transfer mask + pub mod PXFRM_IISOOXFRM { + /// Offset (21 bits) + pub const offset: u32 = 21; + /// Mask (1 bit: 1 << 21) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Data fetch suspended mask + pub mod FSUSPM { + /// Offset (22 bits) + pub const offset: u32 = 22; + /// Mask (1 bit: 1 << 22) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host port interrupt mask + pub mod PRTIM { + /// Offset (24 bits) + pub const offset: u32 = 24; + /// Mask (1 bit: 1 << 24) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host channels interrupt mask + pub mod HCIM { + /// Offset (25 bits) + pub const offset: u32 = 25; + /// Mask (1 bit: 1 << 25) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Periodic TxFIFO empty mask + pub mod PTXFEM { + /// Offset (26 bits) + pub const offset: u32 = 26; + /// Mask (1 bit: 1 << 26) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Connector ID status change mask + pub mod CIDSCHGM { + /// Offset (28 bits) + pub const offset: u32 = 28; + /// Mask (1 bit: 1 << 28) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Disconnect detected interrupt mask + pub mod DISCINT { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (1 bit: 1 << 29) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Session request/new session detected interrupt mask + pub mod SRQIM { + /// Offset (30 bits) + pub const offset: u32 = 30; + /// Mask (1 bit: 1 << 30) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Resume/remote wakeup detected interrupt mask + pub mod WUIM { + /// Offset (31 bits) + pub const offset: u32 = 31; + /// Mask (1 bit: 1 << 31) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// GRXSTSR_Host and GRXSTSR_Peripheral +/// GRXSTSR_Host: OTG_HS Receive status debug read register (host mode) +/// GRXSTSR_Peripheral: OTG_HS Receive status debug read register (peripheral mode mode) +pub mod GRXSTSR { + + /// Channel number + pub mod CHNUM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (4 bits: 0b1111 << 0) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Byte count + pub mod BCNT { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (11 bits: 0x7ff << 4) + pub const mask: u32 = 0x7ff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Data PID + pub mod DPID { + /// Offset (15 bits) + pub const offset: u32 = 15; + /// Mask (2 bits: 0b11 << 15) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Packet status + pub mod PKTSTS { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (4 bits: 0b1111 << 17) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint number + pub mod EPNUM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (4 bits: 0b1111 << 0) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Frame number + pub mod FRMNUM { + /// Offset (21 bits) + pub const offset: u32 = 21; + /// Mask (4 bits: 0b1111 << 21) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// GRXSTSP_Host and GRXSTSP_Peripheral +/// GRXSTSP_Host: OTG_HS status read and pop register (host mode) +/// GRXSTSP_Peripheral: OTG_HS status read and pop register (peripheral mode) +pub mod GRXSTSP { + pub use super::GRXSTSR::BCNT; + pub use super::GRXSTSR::CHNUM; + pub use super::GRXSTSR::DPID; + pub use super::GRXSTSR::EPNUM; + pub use super::GRXSTSR::FRMNUM; + pub use super::GRXSTSR::PKTSTS; +} + +/// OTG_HS Receive FIFO size register +pub mod GRXFSIZ { + + /// RxFIFO depth + pub mod RXFD { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// GNPTXFSIZ and TX0FSIZ +/// GNPTXFSIZ: OTG_HS nonperiodic transmit FIFO size register (host mode) +/// TX0FSIZ: Endpoint 0 transmit FIFO size (peripheral mode) +pub mod GNPTXFSIZ { + + /// Nonperiodic transmit RAM start address + pub mod NPTXFSA { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Nonperiodic TxFIFO depth + pub mod NPTXFD { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (16 bits: 0xffff << 16) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint 0 transmit RAM start address + pub mod TX0FSA { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint 0 TxFIFO depth + pub mod TX0FD { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (16 bits: 0xffff << 16) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS nonperiodic transmit FIFO/queue status register +pub mod GNPTXSTS { + + /// Nonperiodic TxFIFO space available + pub mod NPTXFSAV { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Nonperiodic transmit request queue space available + pub mod NPTQXSAV { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (8 bits: 0xff << 16) + pub const mask: u32 = 0xff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Top of the nonperiodic transmit request queue + pub mod NPTXQTOP { + /// Offset (24 bits) + pub const offset: u32 = 24; + /// Mask (7 bits: 0x7f << 24) + pub const mask: u32 = 0x7f << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS PHY Control Register +pub mod PHYCR { + + /// Data + pub mod DATA { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (8 bits: 0xff << 0) + pub const mask: u32 = 0xff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Address + pub mod ADDR { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (8 bits: 0xff << 16) + pub const mask: u32 = 0xff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Read/Write control + pub mod RW { + /// Offset (22 bits) + pub const offset: u32 = 22; + /// Mask (1 bit: 1 << 22) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// New Read/Write transaction start + pub mod NEW { + /// Offset (25 bits) + pub const offset: u32 = 25; + /// Mask (1 bit: 1 << 25) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Busy status flag + pub mod BUSY { + /// Offset (26 bits) + pub const offset: u32 = 26; + /// Mask (1 bit: 1 << 26) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Done status flag + pub mod DONE { + /// Offset (27 bits) + pub const offset: u32 = 27; + /// Mask (1 bit: 1 << 27) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS general core configuration register +pub mod GCCFG { + + /// Power down + pub mod PWRDWN { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (1 bit: 1 << 16) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Enable I2C bus connection for the external I2C PHY interface + pub mod I2CPADEN { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (1 bit: 1 << 17) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Enable the VBUS sensing device + pub mod VBUSASEN { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (1 bit: 1 << 18) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Enable the VBUS sensing device + pub mod VBUSBSEN { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (1 bit: 1 << 19) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// SOF output enable + pub mod SOFOUTEN { + /// Offset (20 bits) + pub const offset: u32 = 20; + /// Mask (1 bit: 1 << 20) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// VBUS sensing disable option + pub mod NOVBUSSENS { + /// Offset (21 bits) + pub const offset: u32 = 21; + /// Mask (1 bit: 1 << 21) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS core ID register +pub mod CID { + + /// Product ID field + pub mod PRODUCT_ID { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (32 bits: 0xffffffff << 0) + pub const mask: u32 = 0xffffffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS Host periodic transmit FIFO size register +pub mod HPTXFSIZ { + + /// Host periodic TxFIFO start address + pub mod PTXSA { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Host periodic TxFIFO depth + pub mod PTXFD { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (16 bits: 0xffff << 16) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device IN endpoint transmit FIFO size register +pub mod DIEPTXF1 { + + /// IN endpoint FIFOx transmit RAM start address + pub mod INEPTXSA { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// IN endpoint TxFIFO depth + pub mod INEPTXFD { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (16 bits: 0xffff << 16) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS device IN endpoint transmit FIFO size register +pub mod DIEPTXF2 { + pub use super::DIEPTXF1::INEPTXFD; + pub use super::DIEPTXF1::INEPTXSA; +} + +/// OTG_HS device IN endpoint transmit FIFO size register +pub mod DIEPTXF3 { + pub use super::DIEPTXF1::INEPTXFD; + pub use super::DIEPTXF1::INEPTXSA; +} + +/// OTG_HS device IN endpoint transmit FIFO size register +pub mod DIEPTXF4 { + pub use super::DIEPTXF1::INEPTXFD; + pub use super::DIEPTXF1::INEPTXSA; +} + +/// OTG_HS device IN endpoint transmit FIFO size register +pub mod DIEPTXF5 { + pub use super::DIEPTXF1::INEPTXFD; + pub use super::DIEPTXF1::INEPTXSA; +} +#[repr(C)] +pub struct RegisterBlock { + /// OTG_HS control and status register + pub GOTGCTL: RWRegister, + + /// OTG_HS interrupt register + pub GOTGINT: RWRegister, + + /// OTG_HS AHB configuration register + pub GAHBCFG: RWRegister, + + /// OTG_HS USB configuration register + pub GUSBCFG: RWRegister, + + /// OTG_HS reset register + pub GRSTCTL: RWRegister, + + /// OTG_HS core interrupt register + pub GINTSTS: RWRegister, + + /// OTG_HS interrupt mask register + pub GINTMSK: RWRegister, + + /// GRXSTSR_Host and GRXSTSR_Peripheral + /// GRXSTSR_Host: OTG_HS Receive status debug read register (host mode) + /// GRXSTSR_Peripheral: OTG_HS Receive status debug read register (peripheral mode mode) + pub GRXSTSR: RWRegister, + + /// GRXSTSP_Host and GRXSTSP_Peripheral + /// GRXSTSP_Host: OTG_HS status read and pop register (host mode) + /// GRXSTSP_Peripheral: OTG_HS status read and pop register (peripheral mode) + pub GRXSTSP: RWRegister, + + /// OTG_HS Receive FIFO size register + pub GRXFSIZ: RWRegister, + + /// GNPTXFSIZ and TX0FSIZ + /// GNPTXFSIZ: OTG_HS nonperiodic transmit FIFO size register (host mode) + /// TX0FSIZ: Endpoint 0 transmit FIFO size (peripheral mode) + pub GNPTXFSIZ: RWRegister, + + /// OTG_HS nonperiodic transmit FIFO/queue status register + pub GNPTXSTS: RORegister, + + _reserved1: u32, + + /// OTG_HS PHY Control Register + pub PHYCR: RWRegister, + + /// OTG_HS general core configuration register + pub GCCFG: RWRegister, + + /// OTG_HS core ID register + pub CID: RWRegister, + + _reserved2: [u32; 48], + + /// OTG_HS Host periodic transmit FIFO size register + pub HPTXFSIZ: RWRegister, + + /// OTG_HS device IN endpoint transmit FIFO size register + pub DIEPTXF1: RWRegister, + + /// OTG_HS device IN endpoint transmit FIFO size register + pub DIEPTXF2: RWRegister, + + /// OTG_HS device IN endpoint transmit FIFO size register + pub DIEPTXF3: RWRegister, + + /// OTG_HS device IN endpoint transmit FIFO size register + pub DIEPTXF4: RWRegister, + + /// OTG_HS device IN endpoint transmit FIFO size register + pub DIEPTXF5: RWRegister, +} +pub struct ResetValues { + pub GOTGCTL: u32, + pub GOTGINT: u32, + pub GAHBCFG: u32, + pub GUSBCFG: u32, + pub GRSTCTL: u32, + pub GINTSTS: u32, + pub GINTMSK: u32, + pub GRXSTSR: u32, + pub GRXSTSP: u32, + pub GRXFSIZ: u32, + pub GNPTXFSIZ: u32, + pub GNPTXSTS: u32, + pub PHYCR: u32, + pub GCCFG: u32, + pub CID: u32, + pub HPTXFSIZ: u32, + pub DIEPTXF1: u32, + pub DIEPTXF2: u32, + pub DIEPTXF3: u32, + pub DIEPTXF4: u32, + pub DIEPTXF5: u32, +} +#[cfg(not(feature = "nosync"))] +pub struct Instance { + pub(crate) addr: u32, + pub(crate) _marker: PhantomData<*const RegisterBlock>, +} +#[cfg(not(feature = "nosync"))] +impl ::core::ops::Deref for Instance { + type Target = RegisterBlock; + #[inline(always)] + fn deref(&self) -> &RegisterBlock { + unsafe { &*(self.addr as *const _) } + } +} +#[cfg(feature = "rtfm")] +unsafe impl Send for Instance {} diff --git a/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_hs_host.rs b/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_hs_host.rs new file mode 100644 index 0000000..d143e08 --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/ral/peripherals/otg_hs_host.rs @@ -0,0 +1,2035 @@ +#![allow(non_snake_case, non_upper_case_globals)] +#![allow(non_camel_case_types)] +//! USB on the go high speed +//! +//! Used by: stm32f405, stm32f407, stm32f427, stm32f429, stm32f446, stm32f469 + +use super::super::register::{RORegister, RWRegister, UnsafeRWRegister}; +#[cfg(not(feature = "nosync"))] +use core::marker::PhantomData; + +/// OTG_HS host configuration register +pub mod HCFG { + + /// FS/LS PHY clock select + pub mod FSLSPCS { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (2 bits: 0b11 << 0) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// FS- and LS-only support + pub mod FSLSS { + /// Offset (2 bits) + pub const offset: u32 = 2; + /// Mask (1 bit: 1 << 2) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS Host frame interval register +pub mod HFIR { + + /// Frame interval + pub mod FRIVL { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS host frame number/frame time remaining register +pub mod HFNUM { + + /// Frame number + pub mod FRNUM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Frame time remaining + pub mod FTREM { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (16 bits: 0xffff << 16) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS_Host periodic transmit FIFO/queue status register +pub mod HPTXSTS { + + /// Periodic transmit data FIFO space available + pub mod PTXFSAVL { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Periodic transmit request queue space available + pub mod PTXQSAV { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (8 bits: 0xff << 16) + pub const mask: u32 = 0xff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Top of the periodic transmit request queue + pub mod PTXQTOP { + /// Offset (24 bits) + pub const offset: u32 = 24; + /// Mask (8 bits: 0xff << 24) + pub const mask: u32 = 0xff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS Host all channels interrupt register +pub mod HAINT { + + /// Channel interrupts + pub mod HAINT { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS host all channels interrupt mask register +pub mod HAINTMSK { + + /// Channel interrupt mask + pub mod HAINTM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (16 bits: 0xffff << 0) + pub const mask: u32 = 0xffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS host port control and status register +pub mod HPRT { + + /// Port connect status + pub mod PCSTS { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port connect detected + pub mod PCDET { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port enable + pub mod PENA { + /// Offset (2 bits) + pub const offset: u32 = 2; + /// Mask (1 bit: 1 << 2) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port enable/disable change + pub mod PENCHNG { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port overcurrent active + pub mod POCA { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port overcurrent change + pub mod POCCHNG { + /// Offset (5 bits) + pub const offset: u32 = 5; + /// Mask (1 bit: 1 << 5) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port resume + pub mod PRES { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port suspend + pub mod PSUSP { + /// Offset (7 bits) + pub const offset: u32 = 7; + /// Mask (1 bit: 1 << 7) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port reset + pub mod PRST { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port line status + pub mod PLSTS { + /// Offset (10 bits) + pub const offset: u32 = 10; + /// Mask (2 bits: 0b11 << 10) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port power + pub mod PPWR { + /// Offset (12 bits) + pub const offset: u32 = 12; + /// Mask (1 bit: 1 << 12) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port test control + pub mod PTCTL { + /// Offset (13 bits) + pub const offset: u32 = 13; + /// Mask (4 bits: 0b1111 << 13) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Port speed + pub mod PSPD { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (2 bits: 0b11 << 17) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS host channel-0 characteristics register +pub mod HCCHAR0 { + + /// Maximum packet size + pub mod MPSIZ { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (11 bits: 0x7ff << 0) + pub const mask: u32 = 0x7ff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint number + pub mod EPNUM { + /// Offset (11 bits) + pub const offset: u32 = 11; + /// Mask (4 bits: 0b1111 << 11) + pub const mask: u32 = 0b1111 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint direction + pub mod EPDIR { + /// Offset (15 bits) + pub const offset: u32 = 15; + /// Mask (1 bit: 1 << 15) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Low-speed device + pub mod LSDEV { + /// Offset (17 bits) + pub const offset: u32 = 17; + /// Mask (1 bit: 1 << 17) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Endpoint type + pub mod EPTYP { + /// Offset (18 bits) + pub const offset: u32 = 18; + /// Mask (2 bits: 0b11 << 18) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Multi Count (MC) / Error Count (EC) + pub mod MC { + /// Offset (20 bits) + pub const offset: u32 = 20; + /// Mask (2 bits: 0b11 << 20) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Device address + pub mod DAD { + /// Offset (22 bits) + pub const offset: u32 = 22; + /// Mask (7 bits: 0x7f << 22) + pub const mask: u32 = 0x7f << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Odd frame + pub mod ODDFRM { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (1 bit: 1 << 29) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Channel disable + pub mod CHDIS { + /// Offset (30 bits) + pub const offset: u32 = 30; + /// Mask (1 bit: 1 << 30) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Channel enable + pub mod CHENA { + /// Offset (31 bits) + pub const offset: u32 = 31; + /// Mask (1 bit: 1 << 31) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS host channel-1 characteristics register +pub mod HCCHAR1 { + pub use super::HCCHAR0::CHDIS; + pub use super::HCCHAR0::CHENA; + pub use super::HCCHAR0::DAD; + pub use super::HCCHAR0::EPDIR; + pub use super::HCCHAR0::EPNUM; + pub use super::HCCHAR0::EPTYP; + pub use super::HCCHAR0::LSDEV; + pub use super::HCCHAR0::MC; + pub use super::HCCHAR0::MPSIZ; + pub use super::HCCHAR0::ODDFRM; +} + +/// OTG_HS host channel-2 characteristics register +pub mod HCCHAR2 { + pub use super::HCCHAR0::CHDIS; + pub use super::HCCHAR0::CHENA; + pub use super::HCCHAR0::DAD; + pub use super::HCCHAR0::EPDIR; + pub use super::HCCHAR0::EPNUM; + pub use super::HCCHAR0::EPTYP; + pub use super::HCCHAR0::LSDEV; + pub use super::HCCHAR0::MC; + pub use super::HCCHAR0::MPSIZ; + pub use super::HCCHAR0::ODDFRM; +} + +/// OTG_HS host channel-3 characteristics register +pub mod HCCHAR3 { + pub use super::HCCHAR0::CHDIS; + pub use super::HCCHAR0::CHENA; + pub use super::HCCHAR0::DAD; + pub use super::HCCHAR0::EPDIR; + pub use super::HCCHAR0::EPNUM; + pub use super::HCCHAR0::EPTYP; + pub use super::HCCHAR0::LSDEV; + pub use super::HCCHAR0::MC; + pub use super::HCCHAR0::MPSIZ; + pub use super::HCCHAR0::ODDFRM; +} + +/// OTG_HS host channel-4 characteristics register +pub mod HCCHAR4 { + pub use super::HCCHAR0::CHDIS; + pub use super::HCCHAR0::CHENA; + pub use super::HCCHAR0::DAD; + pub use super::HCCHAR0::EPDIR; + pub use super::HCCHAR0::EPNUM; + pub use super::HCCHAR0::EPTYP; + pub use super::HCCHAR0::LSDEV; + pub use super::HCCHAR0::MC; + pub use super::HCCHAR0::MPSIZ; + pub use super::HCCHAR0::ODDFRM; +} + +/// OTG_HS host channel-5 characteristics register +pub mod HCCHAR5 { + pub use super::HCCHAR0::CHDIS; + pub use super::HCCHAR0::CHENA; + pub use super::HCCHAR0::DAD; + pub use super::HCCHAR0::EPDIR; + pub use super::HCCHAR0::EPNUM; + pub use super::HCCHAR0::EPTYP; + pub use super::HCCHAR0::LSDEV; + pub use super::HCCHAR0::MC; + pub use super::HCCHAR0::MPSIZ; + pub use super::HCCHAR0::ODDFRM; +} + +/// OTG_HS host channel-6 characteristics register +pub mod HCCHAR6 { + pub use super::HCCHAR0::CHDIS; + pub use super::HCCHAR0::CHENA; + pub use super::HCCHAR0::DAD; + pub use super::HCCHAR0::EPDIR; + pub use super::HCCHAR0::EPNUM; + pub use super::HCCHAR0::EPTYP; + pub use super::HCCHAR0::LSDEV; + pub use super::HCCHAR0::MC; + pub use super::HCCHAR0::MPSIZ; + pub use super::HCCHAR0::ODDFRM; +} + +/// OTG_HS host channel-7 characteristics register +pub mod HCCHAR7 { + pub use super::HCCHAR0::CHDIS; + pub use super::HCCHAR0::CHENA; + pub use super::HCCHAR0::DAD; + pub use super::HCCHAR0::EPDIR; + pub use super::HCCHAR0::EPNUM; + pub use super::HCCHAR0::EPTYP; + pub use super::HCCHAR0::LSDEV; + pub use super::HCCHAR0::MC; + pub use super::HCCHAR0::MPSIZ; + pub use super::HCCHAR0::ODDFRM; +} + +/// OTG_HS host channel-8 characteristics register +pub mod HCCHAR8 { + pub use super::HCCHAR0::CHDIS; + pub use super::HCCHAR0::CHENA; + pub use super::HCCHAR0::DAD; + pub use super::HCCHAR0::EPDIR; + pub use super::HCCHAR0::EPNUM; + pub use super::HCCHAR0::EPTYP; + pub use super::HCCHAR0::LSDEV; + pub use super::HCCHAR0::MC; + pub use super::HCCHAR0::MPSIZ; + pub use super::HCCHAR0::ODDFRM; +} + +/// OTG_HS host channel-9 characteristics register +pub mod HCCHAR9 { + pub use super::HCCHAR0::CHDIS; + pub use super::HCCHAR0::CHENA; + pub use super::HCCHAR0::DAD; + pub use super::HCCHAR0::EPDIR; + pub use super::HCCHAR0::EPNUM; + pub use super::HCCHAR0::EPTYP; + pub use super::HCCHAR0::LSDEV; + pub use super::HCCHAR0::MC; + pub use super::HCCHAR0::MPSIZ; + pub use super::HCCHAR0::ODDFRM; +} + +/// OTG_HS host channel-10 characteristics register +pub mod HCCHAR10 { + pub use super::HCCHAR0::CHDIS; + pub use super::HCCHAR0::CHENA; + pub use super::HCCHAR0::DAD; + pub use super::HCCHAR0::EPDIR; + pub use super::HCCHAR0::EPNUM; + pub use super::HCCHAR0::EPTYP; + pub use super::HCCHAR0::LSDEV; + pub use super::HCCHAR0::MC; + pub use super::HCCHAR0::MPSIZ; + pub use super::HCCHAR0::ODDFRM; +} + +/// OTG_HS host channel-11 characteristics register +pub mod HCCHAR11 { + pub use super::HCCHAR0::CHDIS; + pub use super::HCCHAR0::CHENA; + pub use super::HCCHAR0::DAD; + pub use super::HCCHAR0::EPDIR; + pub use super::HCCHAR0::EPNUM; + pub use super::HCCHAR0::EPTYP; + pub use super::HCCHAR0::LSDEV; + pub use super::HCCHAR0::MC; + pub use super::HCCHAR0::MPSIZ; + pub use super::HCCHAR0::ODDFRM; +} + +/// OTG_HS host channel-0 split control register +pub mod HCSPLT0 { + + /// Port address + pub mod PRTADDR { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (7 bits: 0x7f << 0) + pub const mask: u32 = 0x7f << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Hub address + pub mod HUBADDR { + /// Offset (7 bits) + pub const offset: u32 = 7; + /// Mask (7 bits: 0x7f << 7) + pub const mask: u32 = 0x7f << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// XACTPOS + pub mod XACTPOS { + /// Offset (14 bits) + pub const offset: u32 = 14; + /// Mask (2 bits: 0b11 << 14) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Do complete split + pub mod COMPLSPLT { + /// Offset (16 bits) + pub const offset: u32 = 16; + /// Mask (1 bit: 1 << 16) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Split enable + pub mod SPLITEN { + /// Offset (31 bits) + pub const offset: u32 = 31; + /// Mask (1 bit: 1 << 31) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS host channel-1 split control register +pub mod HCSPLT1 { + pub use super::HCSPLT0::COMPLSPLT; + pub use super::HCSPLT0::HUBADDR; + pub use super::HCSPLT0::PRTADDR; + pub use super::HCSPLT0::SPLITEN; + pub use super::HCSPLT0::XACTPOS; +} + +/// OTG_HS host channel-2 split control register +pub mod HCSPLT2 { + pub use super::HCSPLT0::COMPLSPLT; + pub use super::HCSPLT0::HUBADDR; + pub use super::HCSPLT0::PRTADDR; + pub use super::HCSPLT0::SPLITEN; + pub use super::HCSPLT0::XACTPOS; +} + +/// OTG_HS host channel-3 split control register +pub mod HCSPLT3 { + pub use super::HCSPLT0::COMPLSPLT; + pub use super::HCSPLT0::HUBADDR; + pub use super::HCSPLT0::PRTADDR; + pub use super::HCSPLT0::SPLITEN; + pub use super::HCSPLT0::XACTPOS; +} + +/// OTG_HS host channel-4 split control register +pub mod HCSPLT4 { + pub use super::HCSPLT0::COMPLSPLT; + pub use super::HCSPLT0::HUBADDR; + pub use super::HCSPLT0::PRTADDR; + pub use super::HCSPLT0::SPLITEN; + pub use super::HCSPLT0::XACTPOS; +} + +/// OTG_HS host channel-5 split control register +pub mod HCSPLT5 { + pub use super::HCSPLT0::COMPLSPLT; + pub use super::HCSPLT0::HUBADDR; + pub use super::HCSPLT0::PRTADDR; + pub use super::HCSPLT0::SPLITEN; + pub use super::HCSPLT0::XACTPOS; +} + +/// OTG_HS host channel-6 split control register +pub mod HCSPLT6 { + pub use super::HCSPLT0::COMPLSPLT; + pub use super::HCSPLT0::HUBADDR; + pub use super::HCSPLT0::PRTADDR; + pub use super::HCSPLT0::SPLITEN; + pub use super::HCSPLT0::XACTPOS; +} + +/// OTG_HS host channel-7 split control register +pub mod HCSPLT7 { + pub use super::HCSPLT0::COMPLSPLT; + pub use super::HCSPLT0::HUBADDR; + pub use super::HCSPLT0::PRTADDR; + pub use super::HCSPLT0::SPLITEN; + pub use super::HCSPLT0::XACTPOS; +} + +/// OTG_HS host channel-8 split control register +pub mod HCSPLT8 { + pub use super::HCSPLT0::COMPLSPLT; + pub use super::HCSPLT0::HUBADDR; + pub use super::HCSPLT0::PRTADDR; + pub use super::HCSPLT0::SPLITEN; + pub use super::HCSPLT0::XACTPOS; +} + +/// OTG_HS host channel-9 split control register +pub mod HCSPLT9 { + pub use super::HCSPLT0::COMPLSPLT; + pub use super::HCSPLT0::HUBADDR; + pub use super::HCSPLT0::PRTADDR; + pub use super::HCSPLT0::SPLITEN; + pub use super::HCSPLT0::XACTPOS; +} + +/// OTG_HS host channel-10 split control register +pub mod HCSPLT10 { + pub use super::HCSPLT0::COMPLSPLT; + pub use super::HCSPLT0::HUBADDR; + pub use super::HCSPLT0::PRTADDR; + pub use super::HCSPLT0::SPLITEN; + pub use super::HCSPLT0::XACTPOS; +} + +/// OTG_HS host channel-11 split control register +pub mod HCSPLT11 { + pub use super::HCSPLT0::COMPLSPLT; + pub use super::HCSPLT0::HUBADDR; + pub use super::HCSPLT0::PRTADDR; + pub use super::HCSPLT0::SPLITEN; + pub use super::HCSPLT0::XACTPOS; +} + +/// OTG_HS host channel-11 interrupt register +pub mod HCINT0 { + + /// Transfer completed + pub mod XFRC { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Channel halted + pub mod CHH { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// AHB error + pub mod AHBERR { + /// Offset (2 bits) + pub const offset: u32 = 2; + /// Mask (1 bit: 1 << 2) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// STALL response received interrupt + pub mod STALL { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// NAK response received interrupt + pub mod NAK { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// ACK response received/transmitted interrupt + pub mod ACK { + /// Offset (5 bits) + pub const offset: u32 = 5; + /// Mask (1 bit: 1 << 5) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Response received interrupt + pub mod NYET { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Transaction error + pub mod TXERR { + /// Offset (7 bits) + pub const offset: u32 = 7; + /// Mask (1 bit: 1 << 7) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Babble error + pub mod BBERR { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Frame overrun + pub mod FRMOR { + /// Offset (9 bits) + pub const offset: u32 = 9; + /// Mask (1 bit: 1 << 9) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Data toggle error + pub mod DTERR { + /// Offset (10 bits) + pub const offset: u32 = 10; + /// Mask (1 bit: 1 << 10) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS host channel-1 interrupt register +pub mod HCINT1 { + pub use super::HCINT0::ACK; + pub use super::HCINT0::AHBERR; + pub use super::HCINT0::BBERR; + pub use super::HCINT0::CHH; + pub use super::HCINT0::DTERR; + pub use super::HCINT0::FRMOR; + pub use super::HCINT0::NAK; + pub use super::HCINT0::NYET; + pub use super::HCINT0::STALL; + pub use super::HCINT0::TXERR; + pub use super::HCINT0::XFRC; +} + +/// OTG_HS host channel-2 interrupt register +pub mod HCINT2 { + pub use super::HCINT0::ACK; + pub use super::HCINT0::AHBERR; + pub use super::HCINT0::BBERR; + pub use super::HCINT0::CHH; + pub use super::HCINT0::DTERR; + pub use super::HCINT0::FRMOR; + pub use super::HCINT0::NAK; + pub use super::HCINT0::NYET; + pub use super::HCINT0::STALL; + pub use super::HCINT0::TXERR; + pub use super::HCINT0::XFRC; +} + +/// OTG_HS host channel-3 interrupt register +pub mod HCINT3 { + pub use super::HCINT0::ACK; + pub use super::HCINT0::AHBERR; + pub use super::HCINT0::BBERR; + pub use super::HCINT0::CHH; + pub use super::HCINT0::DTERR; + pub use super::HCINT0::FRMOR; + pub use super::HCINT0::NAK; + pub use super::HCINT0::NYET; + pub use super::HCINT0::STALL; + pub use super::HCINT0::TXERR; + pub use super::HCINT0::XFRC; +} + +/// OTG_HS host channel-4 interrupt register +pub mod HCINT4 { + pub use super::HCINT0::ACK; + pub use super::HCINT0::AHBERR; + pub use super::HCINT0::BBERR; + pub use super::HCINT0::CHH; + pub use super::HCINT0::DTERR; + pub use super::HCINT0::FRMOR; + pub use super::HCINT0::NAK; + pub use super::HCINT0::NYET; + pub use super::HCINT0::STALL; + pub use super::HCINT0::TXERR; + pub use super::HCINT0::XFRC; +} + +/// OTG_HS host channel-5 interrupt register +pub mod HCINT5 { + pub use super::HCINT0::ACK; + pub use super::HCINT0::AHBERR; + pub use super::HCINT0::BBERR; + pub use super::HCINT0::CHH; + pub use super::HCINT0::DTERR; + pub use super::HCINT0::FRMOR; + pub use super::HCINT0::NAK; + pub use super::HCINT0::NYET; + pub use super::HCINT0::STALL; + pub use super::HCINT0::TXERR; + pub use super::HCINT0::XFRC; +} + +/// OTG_HS host channel-6 interrupt register +pub mod HCINT6 { + pub use super::HCINT0::ACK; + pub use super::HCINT0::AHBERR; + pub use super::HCINT0::BBERR; + pub use super::HCINT0::CHH; + pub use super::HCINT0::DTERR; + pub use super::HCINT0::FRMOR; + pub use super::HCINT0::NAK; + pub use super::HCINT0::NYET; + pub use super::HCINT0::STALL; + pub use super::HCINT0::TXERR; + pub use super::HCINT0::XFRC; +} + +/// OTG_HS host channel-7 interrupt register +pub mod HCINT7 { + pub use super::HCINT0::ACK; + pub use super::HCINT0::AHBERR; + pub use super::HCINT0::BBERR; + pub use super::HCINT0::CHH; + pub use super::HCINT0::DTERR; + pub use super::HCINT0::FRMOR; + pub use super::HCINT0::NAK; + pub use super::HCINT0::NYET; + pub use super::HCINT0::STALL; + pub use super::HCINT0::TXERR; + pub use super::HCINT0::XFRC; +} + +/// OTG_HS host channel-8 interrupt register +pub mod HCINT8 { + pub use super::HCINT0::ACK; + pub use super::HCINT0::AHBERR; + pub use super::HCINT0::BBERR; + pub use super::HCINT0::CHH; + pub use super::HCINT0::DTERR; + pub use super::HCINT0::FRMOR; + pub use super::HCINT0::NAK; + pub use super::HCINT0::NYET; + pub use super::HCINT0::STALL; + pub use super::HCINT0::TXERR; + pub use super::HCINT0::XFRC; +} + +/// OTG_HS host channel-9 interrupt register +pub mod HCINT9 { + pub use super::HCINT0::ACK; + pub use super::HCINT0::AHBERR; + pub use super::HCINT0::BBERR; + pub use super::HCINT0::CHH; + pub use super::HCINT0::DTERR; + pub use super::HCINT0::FRMOR; + pub use super::HCINT0::NAK; + pub use super::HCINT0::NYET; + pub use super::HCINT0::STALL; + pub use super::HCINT0::TXERR; + pub use super::HCINT0::XFRC; +} + +/// OTG_HS host channel-10 interrupt register +pub mod HCINT10 { + pub use super::HCINT0::ACK; + pub use super::HCINT0::AHBERR; + pub use super::HCINT0::BBERR; + pub use super::HCINT0::CHH; + pub use super::HCINT0::DTERR; + pub use super::HCINT0::FRMOR; + pub use super::HCINT0::NAK; + pub use super::HCINT0::NYET; + pub use super::HCINT0::STALL; + pub use super::HCINT0::TXERR; + pub use super::HCINT0::XFRC; +} + +/// OTG_HS host channel-11 interrupt register +pub mod HCINT11 { + pub use super::HCINT0::ACK; + pub use super::HCINT0::AHBERR; + pub use super::HCINT0::BBERR; + pub use super::HCINT0::CHH; + pub use super::HCINT0::DTERR; + pub use super::HCINT0::FRMOR; + pub use super::HCINT0::NAK; + pub use super::HCINT0::NYET; + pub use super::HCINT0::STALL; + pub use super::HCINT0::TXERR; + pub use super::HCINT0::XFRC; +} + +/// OTG_HS host channel-11 interrupt mask register +pub mod HCINTMSK0 { + + /// Transfer completed mask + pub mod XFRCM { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (1 bit: 1 << 0) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Channel halted mask + pub mod CHHM { + /// Offset (1 bits) + pub const offset: u32 = 1; + /// Mask (1 bit: 1 << 1) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// AHB error + pub mod AHBERR { + /// Offset (2 bits) + pub const offset: u32 = 2; + /// Mask (1 bit: 1 << 2) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// STALL response received interrupt mask + pub mod STALLM { + /// Offset (3 bits) + pub const offset: u32 = 3; + /// Mask (1 bit: 1 << 3) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// NAK response received interrupt mask + pub mod NAKM { + /// Offset (4 bits) + pub const offset: u32 = 4; + /// Mask (1 bit: 1 << 4) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// ACK response received/transmitted interrupt mask + pub mod ACKM { + /// Offset (5 bits) + pub const offset: u32 = 5; + /// Mask (1 bit: 1 << 5) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// response received interrupt mask + pub mod NYET { + /// Offset (6 bits) + pub const offset: u32 = 6; + /// Mask (1 bit: 1 << 6) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Transaction error mask + pub mod TXERRM { + /// Offset (7 bits) + pub const offset: u32 = 7; + /// Mask (1 bit: 1 << 7) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Babble error mask + pub mod BBERRM { + /// Offset (8 bits) + pub const offset: u32 = 8; + /// Mask (1 bit: 1 << 8) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Frame overrun mask + pub mod FRMORM { + /// Offset (9 bits) + pub const offset: u32 = 9; + /// Mask (1 bit: 1 << 9) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Data toggle error mask + pub mod DTERRM { + /// Offset (10 bits) + pub const offset: u32 = 10; + /// Mask (1 bit: 1 << 10) + pub const mask: u32 = 1 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS host channel-1 interrupt mask register +pub mod HCINTMSK1 { + pub use super::HCINTMSK0::ACKM; + pub use super::HCINTMSK0::AHBERR; + pub use super::HCINTMSK0::BBERRM; + pub use super::HCINTMSK0::CHHM; + pub use super::HCINTMSK0::DTERRM; + pub use super::HCINTMSK0::FRMORM; + pub use super::HCINTMSK0::NAKM; + pub use super::HCINTMSK0::NYET; + pub use super::HCINTMSK0::STALLM; + pub use super::HCINTMSK0::TXERRM; + pub use super::HCINTMSK0::XFRCM; +} + +/// OTG_HS host channel-2 interrupt mask register +pub mod HCINTMSK2 { + pub use super::HCINTMSK0::ACKM; + pub use super::HCINTMSK0::AHBERR; + pub use super::HCINTMSK0::BBERRM; + pub use super::HCINTMSK0::CHHM; + pub use super::HCINTMSK0::DTERRM; + pub use super::HCINTMSK0::FRMORM; + pub use super::HCINTMSK0::NAKM; + pub use super::HCINTMSK0::NYET; + pub use super::HCINTMSK0::STALLM; + pub use super::HCINTMSK0::TXERRM; + pub use super::HCINTMSK0::XFRCM; +} + +/// OTG_HS host channel-3 interrupt mask register +pub mod HCINTMSK3 { + pub use super::HCINTMSK0::ACKM; + pub use super::HCINTMSK0::AHBERR; + pub use super::HCINTMSK0::BBERRM; + pub use super::HCINTMSK0::CHHM; + pub use super::HCINTMSK0::DTERRM; + pub use super::HCINTMSK0::FRMORM; + pub use super::HCINTMSK0::NAKM; + pub use super::HCINTMSK0::NYET; + pub use super::HCINTMSK0::STALLM; + pub use super::HCINTMSK0::TXERRM; + pub use super::HCINTMSK0::XFRCM; +} + +/// OTG_HS host channel-4 interrupt mask register +pub mod HCINTMSK4 { + pub use super::HCINTMSK0::ACKM; + pub use super::HCINTMSK0::AHBERR; + pub use super::HCINTMSK0::BBERRM; + pub use super::HCINTMSK0::CHHM; + pub use super::HCINTMSK0::DTERRM; + pub use super::HCINTMSK0::FRMORM; + pub use super::HCINTMSK0::NAKM; + pub use super::HCINTMSK0::NYET; + pub use super::HCINTMSK0::STALLM; + pub use super::HCINTMSK0::TXERRM; + pub use super::HCINTMSK0::XFRCM; +} + +/// OTG_HS host channel-5 interrupt mask register +pub mod HCINTMSK5 { + pub use super::HCINTMSK0::ACKM; + pub use super::HCINTMSK0::AHBERR; + pub use super::HCINTMSK0::BBERRM; + pub use super::HCINTMSK0::CHHM; + pub use super::HCINTMSK0::DTERRM; + pub use super::HCINTMSK0::FRMORM; + pub use super::HCINTMSK0::NAKM; + pub use super::HCINTMSK0::NYET; + pub use super::HCINTMSK0::STALLM; + pub use super::HCINTMSK0::TXERRM; + pub use super::HCINTMSK0::XFRCM; +} + +/// OTG_HS host channel-6 interrupt mask register +pub mod HCINTMSK6 { + pub use super::HCINTMSK0::ACKM; + pub use super::HCINTMSK0::AHBERR; + pub use super::HCINTMSK0::BBERRM; + pub use super::HCINTMSK0::CHHM; + pub use super::HCINTMSK0::DTERRM; + pub use super::HCINTMSK0::FRMORM; + pub use super::HCINTMSK0::NAKM; + pub use super::HCINTMSK0::NYET; + pub use super::HCINTMSK0::STALLM; + pub use super::HCINTMSK0::TXERRM; + pub use super::HCINTMSK0::XFRCM; +} + +/// OTG_HS host channel-7 interrupt mask register +pub mod HCINTMSK7 { + pub use super::HCINTMSK0::ACKM; + pub use super::HCINTMSK0::AHBERR; + pub use super::HCINTMSK0::BBERRM; + pub use super::HCINTMSK0::CHHM; + pub use super::HCINTMSK0::DTERRM; + pub use super::HCINTMSK0::FRMORM; + pub use super::HCINTMSK0::NAKM; + pub use super::HCINTMSK0::NYET; + pub use super::HCINTMSK0::STALLM; + pub use super::HCINTMSK0::TXERRM; + pub use super::HCINTMSK0::XFRCM; +} + +/// OTG_HS host channel-8 interrupt mask register +pub mod HCINTMSK8 { + pub use super::HCINTMSK0::ACKM; + pub use super::HCINTMSK0::AHBERR; + pub use super::HCINTMSK0::BBERRM; + pub use super::HCINTMSK0::CHHM; + pub use super::HCINTMSK0::DTERRM; + pub use super::HCINTMSK0::FRMORM; + pub use super::HCINTMSK0::NAKM; + pub use super::HCINTMSK0::NYET; + pub use super::HCINTMSK0::STALLM; + pub use super::HCINTMSK0::TXERRM; + pub use super::HCINTMSK0::XFRCM; +} + +/// OTG_HS host channel-9 interrupt mask register +pub mod HCINTMSK9 { + pub use super::HCINTMSK0::ACKM; + pub use super::HCINTMSK0::AHBERR; + pub use super::HCINTMSK0::BBERRM; + pub use super::HCINTMSK0::CHHM; + pub use super::HCINTMSK0::DTERRM; + pub use super::HCINTMSK0::FRMORM; + pub use super::HCINTMSK0::NAKM; + pub use super::HCINTMSK0::NYET; + pub use super::HCINTMSK0::STALLM; + pub use super::HCINTMSK0::TXERRM; + pub use super::HCINTMSK0::XFRCM; +} + +/// OTG_HS host channel-10 interrupt mask register +pub mod HCINTMSK10 { + pub use super::HCINTMSK0::ACKM; + pub use super::HCINTMSK0::AHBERR; + pub use super::HCINTMSK0::BBERRM; + pub use super::HCINTMSK0::CHHM; + pub use super::HCINTMSK0::DTERRM; + pub use super::HCINTMSK0::FRMORM; + pub use super::HCINTMSK0::NAKM; + pub use super::HCINTMSK0::NYET; + pub use super::HCINTMSK0::STALLM; + pub use super::HCINTMSK0::TXERRM; + pub use super::HCINTMSK0::XFRCM; +} + +/// OTG_HS host channel-11 interrupt mask register +pub mod HCINTMSK11 { + pub use super::HCINTMSK0::ACKM; + pub use super::HCINTMSK0::AHBERR; + pub use super::HCINTMSK0::BBERRM; + pub use super::HCINTMSK0::CHHM; + pub use super::HCINTMSK0::DTERRM; + pub use super::HCINTMSK0::FRMORM; + pub use super::HCINTMSK0::NAKM; + pub use super::HCINTMSK0::NYET; + pub use super::HCINTMSK0::STALLM; + pub use super::HCINTMSK0::TXERRM; + pub use super::HCINTMSK0::XFRCM; +} + +/// OTG_HS host channel-11 transfer size register +pub mod HCTSIZ0 { + + /// Transfer size + pub mod XFRSIZ { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (19 bits: 0x7ffff << 0) + pub const mask: u32 = 0x7ffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Packet count + pub mod PKTCNT { + /// Offset (19 bits) + pub const offset: u32 = 19; + /// Mask (10 bits: 0x3ff << 19) + pub const mask: u32 = 0x3ff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } + + /// Data PID + pub mod DPID { + /// Offset (29 bits) + pub const offset: u32 = 29; + /// Mask (2 bits: 0b11 << 29) + pub const mask: u32 = 0b11 << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS host channel-1 transfer size register +pub mod HCTSIZ1 { + pub use super::HCTSIZ0::DPID; + pub use super::HCTSIZ0::PKTCNT; + pub use super::HCTSIZ0::XFRSIZ; +} + +/// OTG_HS host channel-2 transfer size register +pub mod HCTSIZ2 { + pub use super::HCTSIZ0::DPID; + pub use super::HCTSIZ0::PKTCNT; + pub use super::HCTSIZ0::XFRSIZ; +} + +/// OTG_HS host channel-3 transfer size register +pub mod HCTSIZ3 { + pub use super::HCTSIZ0::DPID; + pub use super::HCTSIZ0::PKTCNT; + pub use super::HCTSIZ0::XFRSIZ; +} + +/// OTG_HS host channel-4 transfer size register +pub mod HCTSIZ4 { + pub use super::HCTSIZ0::DPID; + pub use super::HCTSIZ0::PKTCNT; + pub use super::HCTSIZ0::XFRSIZ; +} + +/// OTG_HS host channel-5 transfer size register +pub mod HCTSIZ5 { + pub use super::HCTSIZ0::DPID; + pub use super::HCTSIZ0::PKTCNT; + pub use super::HCTSIZ0::XFRSIZ; +} + +/// OTG_HS host channel-6 transfer size register +pub mod HCTSIZ6 { + pub use super::HCTSIZ0::DPID; + pub use super::HCTSIZ0::PKTCNT; + pub use super::HCTSIZ0::XFRSIZ; +} + +/// OTG_HS host channel-7 transfer size register +pub mod HCTSIZ7 { + pub use super::HCTSIZ0::DPID; + pub use super::HCTSIZ0::PKTCNT; + pub use super::HCTSIZ0::XFRSIZ; +} + +/// OTG_HS host channel-8 transfer size register +pub mod HCTSIZ8 { + pub use super::HCTSIZ0::DPID; + pub use super::HCTSIZ0::PKTCNT; + pub use super::HCTSIZ0::XFRSIZ; +} + +/// OTG_HS host channel-9 transfer size register +pub mod HCTSIZ9 { + pub use super::HCTSIZ0::DPID; + pub use super::HCTSIZ0::PKTCNT; + pub use super::HCTSIZ0::XFRSIZ; +} + +/// OTG_HS host channel-10 transfer size register +pub mod HCTSIZ10 { + pub use super::HCTSIZ0::DPID; + pub use super::HCTSIZ0::PKTCNT; + pub use super::HCTSIZ0::XFRSIZ; +} + +/// OTG_HS host channel-11 transfer size register +pub mod HCTSIZ11 { + pub use super::HCTSIZ0::DPID; + pub use super::HCTSIZ0::PKTCNT; + pub use super::HCTSIZ0::XFRSIZ; +} + +/// OTG_HS host channel-0 DMA address register +pub mod HCDMA0 { + + /// DMA address + pub mod DMAADDR { + /// Offset (0 bits) + pub const offset: u32 = 0; + /// Mask (32 bits: 0xffffffff << 0) + pub const mask: u32 = 0xffffffff << offset; + /// Read-only values (empty) + pub mod R {} + /// Write-only values (empty) + pub mod W {} + /// Read-write values (empty) + pub mod RW {} + } +} + +/// OTG_HS host channel-1 DMA address register +pub mod HCDMA1 { + pub use super::HCDMA0::DMAADDR; +} + +/// OTG_HS host channel-2 DMA address register +pub mod HCDMA2 { + pub use super::HCDMA0::DMAADDR; +} + +/// OTG_HS host channel-3 DMA address register +pub mod HCDMA3 { + pub use super::HCDMA0::DMAADDR; +} + +/// OTG_HS host channel-4 DMA address register +pub mod HCDMA4 { + pub use super::HCDMA0::DMAADDR; +} + +/// OTG_HS host channel-5 DMA address register +pub mod HCDMA5 { + pub use super::HCDMA0::DMAADDR; +} + +/// OTG_HS host channel-6 DMA address register +pub mod HCDMA6 { + pub use super::HCDMA0::DMAADDR; +} + +/// OTG_HS host channel-7 DMA address register +pub mod HCDMA7 { + pub use super::HCDMA0::DMAADDR; +} + +/// OTG_HS host channel-8 DMA address register +pub mod HCDMA8 { + pub use super::HCDMA0::DMAADDR; +} + +/// OTG_HS host channel-9 DMA address register +pub mod HCDMA9 { + pub use super::HCDMA0::DMAADDR; +} + +/// OTG_HS host channel-10 DMA address register +pub mod HCDMA10 { + pub use super::HCDMA0::DMAADDR; +} + +/// OTG_HS host channel-11 DMA address register +pub mod HCDMA11 { + pub use super::HCDMA0::DMAADDR; +} +#[repr(C)] +pub struct RegisterBlock { + /// OTG_HS host configuration register + pub HCFG: RWRegister, + + /// OTG_HS Host frame interval register + pub HFIR: RWRegister, + + /// OTG_HS host frame number/frame time remaining register + pub HFNUM: RORegister, + + _reserved1: [u32; 1], + + /// OTG_HS_Host periodic transmit FIFO/queue status register + pub HPTXSTS: RWRegister, + + /// OTG_HS Host all channels interrupt register + pub HAINT: RORegister, + + /// OTG_HS host all channels interrupt mask register + pub HAINTMSK: RWRegister, + + _reserved2: [u32; 9], + + /// OTG_HS host port control and status register + pub HPRT: RWRegister, + + _reserved3: [u32; 47], + + /// OTG_HS host channel-0 characteristics register + pub HCCHAR0: RWRegister, + + /// OTG_HS host channel-0 split control register + pub HCSPLT0: RWRegister, + + /// OTG_HS host channel-11 interrupt register + pub HCINT0: RWRegister, + + /// OTG_HS host channel-11 interrupt mask register + pub HCINTMSK0: RWRegister, + + /// OTG_HS host channel-11 transfer size register + pub HCTSIZ0: RWRegister, + + /// OTG_HS host channel-0 DMA address register + pub HCDMA0: UnsafeRWRegister, + + _reserved4: [u32; 2], + + /// OTG_HS host channel-1 characteristics register + pub HCCHAR1: RWRegister, + + /// OTG_HS host channel-1 split control register + pub HCSPLT1: RWRegister, + + /// OTG_HS host channel-1 interrupt register + pub HCINT1: RWRegister, + + /// OTG_HS host channel-1 interrupt mask register + pub HCINTMSK1: RWRegister, + + /// OTG_HS host channel-1 transfer size register + pub HCTSIZ1: RWRegister, + + /// OTG_HS host channel-1 DMA address register + pub HCDMA1: UnsafeRWRegister, + + _reserved5: [u32; 2], + + /// OTG_HS host channel-2 characteristics register + pub HCCHAR2: RWRegister, + + /// OTG_HS host channel-2 split control register + pub HCSPLT2: RWRegister, + + /// OTG_HS host channel-2 interrupt register + pub HCINT2: RWRegister, + + /// OTG_HS host channel-2 interrupt mask register + pub HCINTMSK2: RWRegister, + + /// OTG_HS host channel-2 transfer size register + pub HCTSIZ2: RWRegister, + + /// OTG_HS host channel-2 DMA address register + pub HCDMA2: UnsafeRWRegister, + + _reserved6: [u32; 2], + + /// OTG_HS host channel-3 characteristics register + pub HCCHAR3: RWRegister, + + /// OTG_HS host channel-3 split control register + pub HCSPLT3: RWRegister, + + /// OTG_HS host channel-3 interrupt register + pub HCINT3: RWRegister, + + /// OTG_HS host channel-3 interrupt mask register + pub HCINTMSK3: RWRegister, + + /// OTG_HS host channel-3 transfer size register + pub HCTSIZ3: RWRegister, + + /// OTG_HS host channel-3 DMA address register + pub HCDMA3: UnsafeRWRegister, + + _reserved7: [u32; 2], + + /// OTG_HS host channel-4 characteristics register + pub HCCHAR4: RWRegister, + + /// OTG_HS host channel-4 split control register + pub HCSPLT4: RWRegister, + + /// OTG_HS host channel-4 interrupt register + pub HCINT4: RWRegister, + + /// OTG_HS host channel-4 interrupt mask register + pub HCINTMSK4: RWRegister, + + /// OTG_HS host channel-4 transfer size register + pub HCTSIZ4: RWRegister, + + /// OTG_HS host channel-4 DMA address register + pub HCDMA4: UnsafeRWRegister, + + _reserved8: [u32; 2], + + /// OTG_HS host channel-5 characteristics register + pub HCCHAR5: RWRegister, + + /// OTG_HS host channel-5 split control register + pub HCSPLT5: RWRegister, + + /// OTG_HS host channel-5 interrupt register + pub HCINT5: RWRegister, + + /// OTG_HS host channel-5 interrupt mask register + pub HCINTMSK5: RWRegister, + + /// OTG_HS host channel-5 transfer size register + pub HCTSIZ5: RWRegister, + + /// OTG_HS host channel-5 DMA address register + pub HCDMA5: UnsafeRWRegister, + + _reserved9: [u32; 2], + + /// OTG_HS host channel-6 characteristics register + pub HCCHAR6: RWRegister, + + /// OTG_HS host channel-6 split control register + pub HCSPLT6: RWRegister, + + /// OTG_HS host channel-6 interrupt register + pub HCINT6: RWRegister, + + /// OTG_HS host channel-6 interrupt mask register + pub HCINTMSK6: RWRegister, + + /// OTG_HS host channel-6 transfer size register + pub HCTSIZ6: RWRegister, + + /// OTG_HS host channel-6 DMA address register + pub HCDMA6: UnsafeRWRegister, + + _reserved10: [u32; 2], + + /// OTG_HS host channel-7 characteristics register + pub HCCHAR7: RWRegister, + + /// OTG_HS host channel-7 split control register + pub HCSPLT7: RWRegister, + + /// OTG_HS host channel-7 interrupt register + pub HCINT7: RWRegister, + + /// OTG_HS host channel-7 interrupt mask register + pub HCINTMSK7: RWRegister, + + /// OTG_HS host channel-7 transfer size register + pub HCTSIZ7: RWRegister, + + /// OTG_HS host channel-7 DMA address register + pub HCDMA7: UnsafeRWRegister, + + _reserved11: [u32; 2], + + /// OTG_HS host channel-8 characteristics register + pub HCCHAR8: RWRegister, + + /// OTG_HS host channel-8 split control register + pub HCSPLT8: RWRegister, + + /// OTG_HS host channel-8 interrupt register + pub HCINT8: RWRegister, + + /// OTG_HS host channel-8 interrupt mask register + pub HCINTMSK8: RWRegister, + + /// OTG_HS host channel-8 transfer size register + pub HCTSIZ8: RWRegister, + + /// OTG_HS host channel-8 DMA address register + pub HCDMA8: UnsafeRWRegister, + + _reserved12: [u32; 2], + + /// OTG_HS host channel-9 characteristics register + pub HCCHAR9: RWRegister, + + /// OTG_HS host channel-9 split control register + pub HCSPLT9: RWRegister, + + /// OTG_HS host channel-9 interrupt register + pub HCINT9: RWRegister, + + /// OTG_HS host channel-9 interrupt mask register + pub HCINTMSK9: RWRegister, + + /// OTG_HS host channel-9 transfer size register + pub HCTSIZ9: RWRegister, + + /// OTG_HS host channel-9 DMA address register + pub HCDMA9: UnsafeRWRegister, + + _reserved13: [u32; 2], + + /// OTG_HS host channel-10 characteristics register + pub HCCHAR10: RWRegister, + + /// OTG_HS host channel-10 split control register + pub HCSPLT10: RWRegister, + + /// OTG_HS host channel-10 interrupt register + pub HCINT10: RWRegister, + + /// OTG_HS host channel-10 interrupt mask register + pub HCINTMSK10: RWRegister, + + /// OTG_HS host channel-10 transfer size register + pub HCTSIZ10: RWRegister, + + /// OTG_HS host channel-10 DMA address register + pub HCDMA10: UnsafeRWRegister, + + _reserved14: [u32; 2], + + /// OTG_HS host channel-11 characteristics register + pub HCCHAR11: RWRegister, + + /// OTG_HS host channel-11 split control register + pub HCSPLT11: RWRegister, + + /// OTG_HS host channel-11 interrupt register + pub HCINT11: RWRegister, + + /// OTG_HS host channel-11 interrupt mask register + pub HCINTMSK11: RWRegister, + + /// OTG_HS host channel-11 transfer size register + pub HCTSIZ11: RWRegister, + + /// OTG_HS host channel-11 DMA address register + pub HCDMA11: UnsafeRWRegister, +} +pub struct ResetValues { + pub HCFG: u32, + pub HFIR: u32, + pub HFNUM: u32, + pub HPTXSTS: u32, + pub HAINT: u32, + pub HAINTMSK: u32, + pub HPRT: u32, + pub HCCHAR0: u32, + pub HCSPLT0: u32, + pub HCINT0: u32, + pub HCINTMSK0: u32, + pub HCTSIZ0: u32, + pub HCDMA0: u32, + pub HCCHAR1: u32, + pub HCSPLT1: u32, + pub HCINT1: u32, + pub HCINTMSK1: u32, + pub HCTSIZ1: u32, + pub HCDMA1: u32, + pub HCCHAR2: u32, + pub HCSPLT2: u32, + pub HCINT2: u32, + pub HCINTMSK2: u32, + pub HCTSIZ2: u32, + pub HCDMA2: u32, + pub HCCHAR3: u32, + pub HCSPLT3: u32, + pub HCINT3: u32, + pub HCINTMSK3: u32, + pub HCTSIZ3: u32, + pub HCDMA3: u32, + pub HCCHAR4: u32, + pub HCSPLT4: u32, + pub HCINT4: u32, + pub HCINTMSK4: u32, + pub HCTSIZ4: u32, + pub HCDMA4: u32, + pub HCCHAR5: u32, + pub HCSPLT5: u32, + pub HCINT5: u32, + pub HCINTMSK5: u32, + pub HCTSIZ5: u32, + pub HCDMA5: u32, + pub HCCHAR6: u32, + pub HCSPLT6: u32, + pub HCINT6: u32, + pub HCINTMSK6: u32, + pub HCTSIZ6: u32, + pub HCDMA6: u32, + pub HCCHAR7: u32, + pub HCSPLT7: u32, + pub HCINT7: u32, + pub HCINTMSK7: u32, + pub HCTSIZ7: u32, + pub HCDMA7: u32, + pub HCCHAR8: u32, + pub HCSPLT8: u32, + pub HCINT8: u32, + pub HCINTMSK8: u32, + pub HCTSIZ8: u32, + pub HCDMA8: u32, + pub HCCHAR9: u32, + pub HCSPLT9: u32, + pub HCINT9: u32, + pub HCINTMSK9: u32, + pub HCTSIZ9: u32, + pub HCDMA9: u32, + pub HCCHAR10: u32, + pub HCSPLT10: u32, + pub HCINT10: u32, + pub HCINTMSK10: u32, + pub HCTSIZ10: u32, + pub HCDMA10: u32, + pub HCCHAR11: u32, + pub HCSPLT11: u32, + pub HCINT11: u32, + pub HCINTMSK11: u32, + pub HCTSIZ11: u32, + pub HCDMA11: u32, +} +#[cfg(not(feature = "nosync"))] +pub struct Instance { + pub(crate) addr: u32, + pub(crate) _marker: PhantomData<*const RegisterBlock>, +} +#[cfg(not(feature = "nosync"))] +impl ::core::ops::Deref for Instance { + type Target = RegisterBlock; + #[inline(always)] + fn deref(&self) -> &RegisterBlock { + unsafe { &*(self.addr as *const _) } + } +} +#[cfg(feature = "rtfm")] +unsafe impl Send for Instance {} diff --git a/firmware/vendor/synopsys-usb-otg/src/ral/register.rs b/firmware/vendor/synopsys-usb-otg/src/ral/register.rs new file mode 100644 index 0000000..4584e7c --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/ral/register.rs @@ -0,0 +1,610 @@ +#![allow(unused)] +use core::cell::UnsafeCell; + +/// A read-write register of type T. +/// +/// Contains one value of type T and provides volatile read/write functions to it. +/// +/// # Safety +/// This register should be used where reads and writes to this peripheral register do not +/// lead to memory unsafety. For example, it is a poor choice for a DMA target, but less +/// worrisome for a GPIO output data register. +/// +/// Access to this register must be synchronised; if multiple threads (or the main thread and an +/// interrupt service routine) are accessing it simultaneously you may encounter data races. +pub struct RWRegister { + register: UnsafeCell, +} + +impl RWRegister { + /// Reads the value of the register. + #[inline(always)] + pub fn read(&self) -> T { + unsafe { ::core::ptr::read_volatile(self.register.get()) } + } + + /// Writes a new value to the register. + #[inline(always)] + pub fn write(&self, val: T) { + unsafe { ::core::ptr::write_volatile(self.register.get(), val) } + } +} + +/// A read-write register of type T, where read/write access is unsafe. +/// +/// Contains one value of type T and provides volatile read/write functions to it. +/// +/// # Safety +/// This register should be used where reads and writes to this peripheral may invoke +/// undefined behaviour or memory unsafety. For example, any registers you write a memory +/// address into. +/// +/// Access to this register must be synchronised; if multiple threads (or the main thread and an +/// interrupt service routine) are accessing it simultaneously you may encounter data races. +pub struct UnsafeRWRegister { + register: UnsafeCell, +} + +impl UnsafeRWRegister { + /// Reads the value of the register. + #[inline(always)] + pub unsafe fn read(&self) -> T { + ::core::ptr::read_volatile(self.register.get()) + } + + /// Writes a new value to the register. + #[inline(always)] + pub unsafe fn write(&self, val: T) { + ::core::ptr::write_volatile(self.register.get(), val) + } +} + +/// A read-only register of type T. +/// +/// Contains one value of type T and provides a volatile read function to it. +/// +/// # Safety +/// This register should be used where reads and writes to this peripheral register do not +/// lead to memory unsafety. +/// +/// Access to this register must be synchronised; if multiple threads (or the main thread and an +/// interrupt service routine) are accessing it simultaneously you may encounter data races. +pub struct RORegister { + register: UnsafeCell, +} + +impl RORegister { + /// Reads the value of the register. + #[inline(always)] + pub fn read(&self) -> T { + unsafe { ::core::ptr::read_volatile(self.register.get()) } + } +} + +/// A read-only register of type T, where read access is unsafe. +/// +/// Contains one value of type T and provides a volatile read function to it. +/// +/// # Safety +/// This register should be used where reads to this peripheral may invoke +/// undefined behaviour or memory unsafety. +/// +/// Access to this register must be synchronised; if multiple threads (or the main thread and an +/// interrupt service routine) are accessing it simultaneously you may encounter data races. +pub struct UnsafeRORegister { + register: UnsafeCell, +} + +impl UnsafeRORegister { + /// Reads the value of the register. + #[inline(always)] + pub unsafe fn read(&self) -> T { + ::core::ptr::read_volatile(self.register.get()) + } +} + +/// A write-only register of type T. +/// +/// Contains one value of type T and provides a volatile write function to it. +/// +/// # Safety +/// This register should be used where writes to this peripheral register do not lead to memory +/// unsafety. +/// +/// Access to this register must be synchronised; if multiple threads (or the main thread and an +/// interrupt service routine) are accessing it simultaneously you may encounter data races. +pub struct WORegister { + register: UnsafeCell, +} + +impl WORegister { + /// Writes a new value to the register. + #[inline(always)] + pub fn write(&self, val: T) { + unsafe { ::core::ptr::write_volatile(self.register.get(), val) } + } +} + +/// A write-only register of type T, where write access is unsafe. +/// +/// Contains one value of type T and provides a volatile write function to it. +/// +/// # Safety +/// This register should be used where reads and writes to this peripheral may invoke +/// undefined behaviour or memory unsafety. +/// +/// Access to this register must be synchronised; if multiple threads (or the main thread and an +/// interrupt service routine) are accessing it simultaneously you may encounter data races. +pub struct UnsafeWORegister { + register: UnsafeCell, +} + +impl UnsafeWORegister { + /// Writes a new value to the register. + #[inline(always)] + pub unsafe fn write(&self, val: T) { + ::core::ptr::write_volatile(self.register.get(), val) + } +} + +/// Write to a RWRegister or UnsafeRWRegister. +/// +/// # Examples +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// // Safely acquire the peripheral instance (will panic if already acquired) +/// let gpioa = stm32ral::gpio::GPIOA::take().unwrap(); +/// +/// // Write some value to the register. +/// write_reg!(stm32ral::gpio, gpioa, ODR, 1<<3); +/// +/// // Write values to specific fields. Unspecified fields are written to 0. +/// write_reg!(stm32ral::gpio, gpioa, MODER, MODER3: Output, MODER4: Analog); +/// +/// // Unsafe access without requiring you to first `take()` the instance +/// unsafe { write_reg!(stm32ral::gpio, GPIOA, MODER, MODER3: Output, MODER4: Analog) }; +/// # } +/// ``` +/// +/// # Usage +/// Like `modify_reg!`, this macro can be used in two ways, either with a single value to write to +/// the whole register, or with multiple fields each with their own value. +/// +/// In both cases, the first arguments are: +/// * the path to the peripheral module: `stm32ral::gpio`, +/// * a reference to the instance of that peripheral: 'gpioa' (anything which dereferences to +/// `RegisterBlock`, such as `Instance`, `&Instance`, `&RegisterBlock`, or +/// `*const RegisterBlock`), +/// * the register you wish you access: `MODER` (a field on the `RegisterBlock`). +/// +/// In the single-value usage, the final argument is just the value to write: +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// # let gpioa = stm32ral::gpio::GPIOA::take().unwrap(); +/// // Turn on PA3 (and turn everything else off). +/// write_reg!(stm32ral::gpio, gpioa, ODR, 1<<3); +/// # } +/// ``` +/// +/// Otherwise, the remaining arguments are each `Field: Value` pairs: +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// // Set PA3 to Output, PA4 to Analog, and everything else to 0 (which is Input). +/// # let gpioa = stm32ral::gpio::GPIOA::take().unwrap(); +/// write_reg!(stm32ral::gpio, gpioa, MODER, MODER3: 0b01, MODER4: 0b11); +/// # } +/// ``` +/// For fields with annotated values, you can also specify a named value: +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// // As above, but with named values. +/// # let gpioa = stm32ral::gpio::GPIOA::take().unwrap(); +/// write_reg!(stm32ral::gpio, gpioa, MODER, MODER3: Output, MODER4: Analog); +/// # } +/// ``` +/// +/// This macro expands to calling `(*$instance).$register.write(value)`, +/// where in the second usage, the value is computed as the bitwise OR of +/// each field value, which are masked and shifted appropriately for the given field. +/// The named values are brought into scope by `use $peripheral::$register::$field::*` for +/// each field. The same constants could just be specified manually: +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// // As above, but being explicit about named values. +/// # let gpioa = stm32ral::gpio::GPIOA::take().unwrap(); +/// write_reg!(stm32ral::gpio, gpioa, MODER, MODER3: stm32ral::gpio::MODER::MODER3::RW::Output, +/// MODER4: stm32ral::gpio::MODER::MODER4::RW::Analog); +/// # } +/// ``` +/// +/// The fully expanded form is equivalent to: +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// // As above, but expanded. +/// # let gpioa = stm32ral::gpio::GPIOA::take().unwrap(); +/// (*gpioa).MODER.write( +/// ((stm32ral::gpio::MODER::MODER3::RW::Output << stm32ral::gpio::MODER::MODER3::offset) +/// & stm32ral::gpio::MODER::MODER3::mask) +/// | +/// ((stm32ral::gpio::MODER::MODER4::RW::Analog << stm32ral::gpio::MODER::MODER4::offset) +/// & stm32ral::gpio::MODER::MODER4::mask) +/// ); +/// # } +/// ``` +/// +/// # Safety +/// This macro will require an unsafe function or block when used with an UnsafeRWRegister, +/// but not if used with RWRegister. +/// +/// When run in an unsafe context, peripheral instances are directly accessible without requiring +/// having called `take()` beforehand: +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// unsafe { write_reg!(stm32ral::gpio, GPIOA, MODER, MODER3: Output, MODER4: Analog) }; +/// # } +/// ``` +/// This works because `GPIOA` is a `*const RegisterBlock` in the `stm32ral::gpio` module; +/// and the macro brings such constants into scope and then dereferences the provided reference. +#[macro_export] +#[doc(hidden)] +macro_rules! write_reg { + ( $periph:path, $instance:expr, $reg:ident, $( $field:ident : $value:expr ),+ ) => {{ + #[allow(unused_imports)] + use $periph::{*}; + #[allow(unused_imports)] + (*$instance).$reg.write( + $({ use $periph::{$reg::$field::{mask, offset, W::*, RW::*}}; ($value << offset) & mask }) | * + ); + }}; + ( $periph:path, $instance:expr, $reg:ident, $value:expr ) => {{ + #[allow(unused_imports)] + use $periph::{*}; + (*$instance).$reg.write($value); + }}; +} + +/// Modify a RWRegister or UnsafeRWRegister. +/// +/// # Examples +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// // Safely acquire the peripheral instance (will panic if already acquired) +/// let gpioa = stm32ral::gpio::GPIOA::take().unwrap(); +/// +/// // Update the register to ensure bit 3 is set. +/// modify_reg!(stm32ral::gpio, gpioa, ODR, |reg| reg | (1<<3)); +/// +/// // Write values to specific fields. Unspecified fields are left unchanged. +/// modify_reg!(stm32ral::gpio, gpioa, MODER, MODER3: Output, MODER4: Analog); +/// +/// // Unsafe access without requiring you to first `take()` the instance +/// unsafe { modify_reg!(stm32ral::gpio, GPIOA, MODER, MODER3: Output, MODER4: Analog) }; +/// # } +/// ``` +/// +/// # Usage +/// Like `write_reg!`, this macro can be used in two ways, either with a modification of the entire +/// register, or by specifying which fields to change and what value to change them to. +/// +/// In both cases, the first arguments are: +/// * the path to the peripheral module: `stm32ral::gpio`, +/// * a reference to the instance of that peripheral: 'gpioa' (anything which dereferences to +/// `RegisterBlock`, such as `Instance`, `&Instance`, `&RegisterBlock`, or +/// `*const RegisterBlock`), +/// * the register you wish you access: `MODER` (a field on the `RegisterBlock`). +/// +/// In the whole-register usage, the final argument is a closure that accepts the current value +/// of the register and returns the new value to write: +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// # let gpioa = stm32ral::gpio::GPIOA::take().unwrap(); +/// // Turn on PA3 without affecting anything else. +/// modify_reg!(stm32ral::gpio, gpioa, ODR, |reg| reg | (1<<3)); +/// # } +/// ``` +/// +/// Otherwise, the remaining arguments are `Field: Value` pairs: +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// # let gpioa = stm32ral::gpio::GPIOA::take().unwrap(); +/// // Set PA3 to Output, PA4 to Analog, and leave everything else unchanged. +/// modify_reg!(stm32ral::gpio, gpioa, MODER, MODER3: 0b01, MODER4: 0b11); +/// # } +/// ``` +/// +/// For fields with annotated values, you can also specify a named value: +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// # let gpioa = stm32ral::gpio::GPIOA::take().unwrap(); +/// // As above, but with named values. +/// modify_reg!(stm32ral::gpio, gpioa, MODER, MODER3: Output, MODER4: Analog); +/// # } +/// ``` +/// +/// This macro expands to calling `(*instance).register.write(value)`. +/// When called with a closure, `(*instance).register.read()` is called, the result +/// passed in to the closure, and the return value of the closure is used for `value`. +/// When called with `Field: Value` arguments, the current value is read and then masked +/// according to the specified fields, and then ORd with the OR of each field value, +/// each masked and shifted appropriately for the field. The named values are brought into scope +/// by `use peripheral::register::field::*` for each field. The same constants could just be +/// specified manually: +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// # let gpioa = stm32ral::gpio::GPIOA::take().unwrap(); +/// // As above, but being explicit about named values. +/// modify_reg!(stm32ral::gpio, gpioa, MODER, MODER3: stm32ral::gpio::MODER::MODER3::RW::Output, +/// MODER4: stm32ral::gpio::MODER::MODER4::RW::Analog); +/// # } +/// ``` +/// +/// The fully expanded form is equivalent to: +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// # let gpioa = stm32ral::gpio::GPIOA::take().unwrap(); +/// // As above, but expanded. +/// (*gpioa).MODER.write( +/// ( +/// // First read the current value... +/// (*gpioa).MODER.read() +/// // Then AND it with an appropriate mask... +/// & +/// !( stm32ral::gpio::MODER::MODER3::mask | stm32ral::gpio::MODER::MODER4::mask ) +/// ) +/// // Then OR with each field value. +/// | +/// ((stm32ral::gpio::MODER::MODER3::RW::Output << stm32ral::gpio::MODER::MODER3::offset) +/// & stm32ral::gpio::MODER::MODER3::mask) +/// | +/// ((stm32ral::gpio::MODER::MODER4::RW::Analog << stm32ral::gpio::MODER::MODER3::offset) +/// & stm32ral::gpio::MODER::MODER3::mask) +/// ); +/// # } +/// ``` +/// +/// # Safety +/// This macro will require an unsafe function or block when used with an UnsafeRWRegister, +/// but not if used with RWRegister. +/// +/// When run in an unsafe context, peripheral instances are directly accessible without requiring +/// having called `take()` beforehand: +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// unsafe { modify_reg!(stm32ral::gpio, GPIOA, MODER, MODER3: Output, MODER4: Analog) }; +/// # } +/// ``` +/// This works because `GPIOA` is a `*const RegisterBlock` in the `stm32ral::gpio` module; +/// and the macro brings such constants into scope and then dereferences the provided reference. +#[macro_export] +#[doc(hidden)] +macro_rules! modify_reg { + ( $periph:path, $instance:expr, $reg:ident, $( $field:ident : $value:expr ),+ ) => {{ + #[allow(unused_imports)] + use $periph::{*}; + #[allow(unused_imports)] + (*$instance).$reg.write( + ((*$instance).$reg.read() & !( $({ use $periph::{$reg::$field::mask}; mask }) | * )) + | $({ use $periph::{$reg::$field::{mask, offset, W::*, RW::*}}; ($value << offset) & mask }) | *); + }}; + ( $periph:path, $instance:expr, $reg:ident, $fn:expr ) => {{ + #[allow(unused_imports)] + use $periph::{*}; + (*$instance).$reg.write($fn((*$instance).$reg.read())); + }}; +} + +/// Read the value from a RORegister, RWRegister, UnsafeRORegister, or UnsafeRWRegister. +/// +/// # Examples +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// // Safely acquire the peripheral instance (will panic if already acquired) +/// let gpioa = stm32ral::gpio::GPIOA::take().unwrap(); +/// +/// // Read the whole register. +/// let val = read_reg!(stm32ral::gpio, gpioa, IDR); +/// +/// // Read one field from the register. +/// let val = read_reg!(stm32ral::gpio, gpioa, IDR, IDR2); +/// +/// // Read multiple fields from the register. +/// let (val1, val2, val3) = read_reg!(stm32ral::gpio, gpioa, IDR, IDR0, IDR1, IDR2); +/// +/// // Check if one field is equal to a specific value, with the field's named values in scope. +/// while read_reg!(stm32ral::gpio, gpioa, IDR, IDR2 == High) {} +/// +/// // Unsafe access without requiring you to first `take()` the instance +/// let val = unsafe { read_reg!(stm32ral::gpio, GPIOA, IDR) }; +/// # } +/// ``` +/// +/// # Usage +/// Like `write_reg!`, this macro can be used multiple ways, either reading the entire register or +/// reading a one or more fields from it and potentially performing a comparison with one field. +/// +/// In all cases, the first arguments are: +/// * the path to the peripheral module: `stm32ral::gpio`, +/// * a reference to the instance of that peripheral: 'gpioa' (anything which dereferences to +/// `RegisterBlock`, such as `Instance`, `&Instance`, `&RegisterBlock`, or +/// `*const RegisterBlock`), +/// * the register you wish to access: `IDR` (a field on the `RegisterBlock`). +/// +/// In the whole-register usage, the macro simply returns the register's value: +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// # let gpioa = stm32ral::gpio::GPIOA::take().unwrap(); +/// // Read the entire value of GPIOA.IDR into `val`. +/// let val = read_reg!(stm32ral::gpio, gpioa, IDR); +/// # } +/// ``` +/// +/// For reading individual fields, the macro masks and shifts appropriately: +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// # let gpioa = stm32ral::gpio::GPIOA::take().unwrap(); +/// // Read just the value of the field GPIOA.IDR2 into `val`. +/// let val = read_reg!(stm32ral::gpio, gpioa, IDR, IDR2); +/// +/// // As above, but expanded for exposition: +/// let val = ((*gpioa).IDR.read() & stm32ral::gpio::IDR::IDR2::mask) +/// >> stm32ral::gpio::IDR::IDR2::offset; +/// +/// // Read multiple fields +/// let (val1, val2) = read_reg!(stm32ral::gpio, gpioa, IDR, IDR2, IDR3); +/// +/// // As above, but expanded for exposition: +/// let (val1, val2) = { let val = (*gpioa).IDR.read(); +/// ((val & stm32ral::gpio::IDR::IDR2::mask) >> stm32ral::gpio::IDR::IDR2::offset, +/// (val & stm32ral::gpio::IDR::IDR3::mask) >> stm32ral::gpio::IDR::IDR3::offset, +/// )}; +/// # } +/// ``` +/// +/// For comparing a single field, the macro masks and shifts and then performs the comparison: +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// # let gpioa = stm32ral::gpio::GPIOA::take().unwrap(); +/// # let rcc = stm32ral::rcc::RCC::take().unwrap(); +/// // Loop while PA2 is High. +/// while read_reg!(stm32ral::gpio, gpioa, IDR, IDR2 == High) {} +/// +/// // Only proceed if the clock is not the HSI. +/// if read_reg!(stm32ral::rcc, rcc, CFGR, SWS != HSI) { } +/// +/// // Equivalent expansion: +/// if (((*rcc).CFGR.read() & stm32ral::rcc::CFGR::SWS::mask) +/// >> stm32ral::rcc::CFGR::SWS::offset) != stm32ral::rcc::CFGR::SWS::R::HSI { } +/// # } +/// ``` +/// +/// # Safety +/// This macro will require an unsafe function or block when used with an UnsafeRWRegister or +/// UnsafeRORegister, but not if used with RWRegister, or RORegister. +/// +/// When run in an unsafe context, peripheral instances are directly accessible without requiring +/// having called `take()` beforehand: +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// let val = unsafe { read_reg!(stm32ral::gpio, GPIOA, MODER) }; +/// # } +/// ``` +/// This works because `GPIOA` is a `*const RegisterBlock` in the `stm32ral::gpio` module; +/// and the macro brings such constants into scope and then dereferences the provided reference. +#[macro_export] +#[doc(hidden)] +macro_rules! read_reg { + ( $periph:path, $instance:expr, $reg:ident, $( $field:ident ),+ ) => {{ + #[allow(unused_imports)] + use $periph::{*}; + let val = ((*$instance).$reg.read()); + ( $({ + #[allow(unused_imports)] + use $periph::{$reg::$field::{mask, offset, R::*, RW::*}}; + (val & mask) >> offset + }) , *) + }}; + ( $periph:path, $instance:expr, $reg:ident, $field:ident $($cmp:tt)* ) => {{ + #[allow(unused_imports)] + use $periph::{*}; + #[allow(unused_imports)] + use $periph::{$reg::$field::{mask, offset, R::*, RW::*}}; + (((*$instance).$reg.read() & mask) >> offset) $($cmp)* + }}; + ( $periph:path, $instance:expr, $reg:ident ) => {{ + #[allow(unused_imports)] + use $periph::{*}; + ((*$instance).$reg.read()) + }}; +} + +/// Reset a RWRegister, UnsafeRWRegister, WORegister, or UnsafeWORegister to its reset value. +/// +/// # Examples +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// // Safely acquire the peripheral instance (will panic if already acquired) +/// let gpioa = stm32ral::gpio::GPIOA::take().unwrap(); +/// +/// // Reset PA14 and PA15 to their reset state +/// reset_reg!(stm32ral::gpio, gpioa, GPIOA, MODER, MODER14, MODER15); +/// +/// // Reset the entire GPIOA.MODER to its reset state +/// reset_reg!(stm32ral::gpio, gpioa, GPIOA, MODER); +/// # } +/// ``` +/// +/// # Usage +/// Like `write_reg!`, this macro can be used in two ways, either resetting the entire register +/// or just resetting specific fields within in. The register or fields are written with their +/// reset values. +/// +/// In both cases, the first arguments are: +/// * the path to the peripheral module: `stm32ral::gpio`, +/// * a reference to the instance of that peripheral: 'gpioa' (anything which dereferences to +/// `RegisterBlock`, such as `Instance`, `&Instance`, `&RegisterBlock`, or +/// `*const RegisterBlock`), +/// * the module for the instance of that peripheral: `GPIOA`, +/// * the register you wish to access: `MODER` (a field on the `RegisterBlock`). +/// +/// In the whole-register usage, that's it: +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// # let gpioa = stm32ral::gpio::GPIOA::take().unwrap(); +/// // Reset the entire GPIOA.MODER +/// reset_reg!(stm32ral::gpio, gpioa, GPIOA, MODER); +/// # } +/// ``` +/// +/// Otherwise, the remaining arguments are each field names: +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// # let gpioa = stm32ral::gpio::GPIOA::take().unwrap(); +/// // Reset the JTAG pins +/// reset_reg!(stm32ral::gpio, gpioa, GPIOA, MODER, MODER13, MODER14, MODER15); +/// reset_reg!(stm32ral::gpio, gpioa, GPIOB, MODER, MODER3, MODER4); +/// # } +/// ``` +/// +/// The second form is only available to RWRegister and UnsafeRWRegister, since `.read()` is +/// not available for WORegister and UnsafeWORegister. +/// +/// This macro expands to calling `(*$instance).$register.write(value)`, where +/// `value` is either the register's reset value, or the current read value of the register +/// masked appropriately and combined with the reset value for each field. +/// +/// # Safety +/// This macro will require an unsafe function or block when used with an UnsafeRWRegister or +/// UnsafeRORegister, but not if used with RWRegister or RORegister. +/// +/// When run in an unsafe context, peripheral instances are directly accessible without requiring +/// having called `take()` beforehand: +/// ```rust,no_run +/// # use stm32ral::{read_reg, write_reg, modify_reg, reset_reg}; fn main() { +/// unsafe { reset_reg!(stm32ral::gpio, GPIOA, GPIOA, MODER) }; +/// # } +/// ``` +/// This works because `GPIOA` is a `*const RegisterBlock` in the `stm32ral::gpio` module; +/// and the macro brings such constants into scope and then dereferences the provided reference. +/// +/// Note that the second argument is a `*const` and the third is a path; despite both being written +/// `GPIOA` they are not the same thing. +#[macro_export] +#[doc(hidden)] +macro_rules! reset_reg { + ( $periph:path, $instance:expr, $instancemod:path, $reg:ident, $( $field:ident ),+ ) => {{ + #[allow(unused_imports)] + use $periph::{*}; + use $periph::{$instancemod::{reset}}; + #[allow(unused_imports)] + (*$instance).$reg.write({ + let resetmask: u32 = $({ use $periph::{$reg::$field::mask}; mask }) | *; + ((*$instance).$reg.read() & !resetmask) | (reset.$reg & resetmask) + }); + }}; + ( $periph:path, $instance:expr, $instancemod:path, $reg:ident ) => {{ + #[allow(unused_imports)] + use $periph::{*}; + use $periph::{$instancemod::{reset}}; + (*$instance).$reg.write(reset.$reg); + }}; +} diff --git a/firmware/vendor/synopsys-usb-otg/src/ral/stm32f429/mod.rs b/firmware/vendor/synopsys-usb-otg/src/ral/stm32f429/mod.rs new file mode 100644 index 0000000..774f205 --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/ral/stm32f429/mod.rs @@ -0,0 +1,7 @@ +pub use super::instances::otg_fs_device_f401_f405_f407_f411_f427_f429 as otg_fs_device; +pub use super::instances::otg_fs_global_f401_f405_f407_f411_f427_f429 as otg_fs_global; +pub use super::instances::otg_fs_host; +pub use super::instances::otg_hs_device; +pub use super::instances::otg_hs_global; +pub use super::instances::otg_hs_host; +pub use super::instances::otg_s_pwrclk; diff --git a/firmware/vendor/synopsys-usb-otg/src/target.rs b/firmware/vendor/synopsys-usb-otg/src/target.rs new file mode 100644 index 0000000..728c2b6 --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/target.rs @@ -0,0 +1,92 @@ +//! Target-specific definitions + +use vcell::VolatileCell; + +use crate::ral::register::RWRegister; +use crate::ral::{ + endpoint0_out, endpoint_in, endpoint_out, otg_device, otg_global, otg_global_dieptxfx, + otg_pwrclk, +}; +use crate::UsbPeripheral; + +pub fn fifo_write(usb: UsbRegisters, channel: impl Into, mut buf: &[u8]) { + let fifo = usb.fifo(channel.into()); + + while buf.len() >= 4 { + let mut u32_bytes = [0u8; 4]; + u32_bytes.copy_from_slice(&buf[..4]); + buf = &buf[4..]; + fifo.write(u32::from_ne_bytes(u32_bytes)); + } + if !buf.is_empty() { + let mut u32_bytes = [0u8; 4]; + u32_bytes[..buf.len()].copy_from_slice(buf); + fifo.write(u32::from_ne_bytes(u32_bytes)); + } +} + +pub fn fifo_read_into(usb: UsbRegisters, buf: &[VolatileCell]) { + let fifo = usb.fifo(0); + + for p in buf { + let word = fifo.read(); + p.set(word); + } +} + +/// Wrapper around device-specific peripheral that provides unified register interface +#[derive(Copy, Clone)] +pub struct UsbRegisters(usize); + +impl UsbRegisters { + #[inline(always)] + pub fn new() -> Self { + Self(USB::REGISTERS as usize) + } + + #[inline(always)] + pub fn global(&self) -> &'static otg_global::RegisterBlock { + unsafe { &*(self.0 as *const _) } + } + + #[inline(always)] + pub fn device(&self) -> &'static otg_device::RegisterBlock { + unsafe { &*((self.0 + 0x800) as *const _) } + } + + #[inline(always)] + pub fn pwrclk(&self) -> &'static otg_pwrclk::RegisterBlock { + unsafe { &*((self.0 + 0xe00) as *const _) } + } + + #[inline(always)] + pub fn fifo(&self, channel: usize) -> &'static RWRegister { + assert!(channel <= 15); + let address = self.0 + 0x1000 + channel * 0x1000; + unsafe { &*(address as *const RWRegister) } + } + + #[inline(always)] + pub fn dieptxfx(&self, index: usize) -> &'static otg_global_dieptxfx::RegisterBlock { + let address = self.0 + 0x100 + 4 * index; + unsafe { &*(address as *const _) } + } + + #[inline(always)] + pub fn endpoint_in(&self, index: usize) -> &'static endpoint_in::RegisterBlock { + let address = self.0 + 0x900 + 0x20 * index; + unsafe { &*(address as *const _) } + } + + #[inline(always)] + pub fn endpoint0_out(&self) -> &'static endpoint0_out::RegisterBlock { + let address = self.0 + 0xb00; + unsafe { &*(address as *const _) } + } + + #[inline(always)] + pub fn endpoint_out(&self, index: usize) -> &'static endpoint_out::RegisterBlock { + let address = self.0 + 0xb00 + 0x20 * index; + unsafe { &*(address as *const _) } + } +} diff --git a/firmware/vendor/synopsys-usb-otg/src/tests.rs b/firmware/vendor/synopsys-usb-otg/src/tests.rs new file mode 100644 index 0000000..469c029 --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/tests.rs @@ -0,0 +1,382 @@ +extern crate std; + +use core::cell::UnsafeCell; +use std::{ + boxed::Box, + sync::{Arc, Mutex}, + vec::Vec, +}; +use usb_device::{ + bus::{PollResult, UsbBus as _}, + device::{UsbDevice, UsbDeviceBuilder, UsbVidPid}, + endpoint::{EndpointIn, EndpointOut}, + UsbError, +}; + +use crate::{UsbBus, UsbPeripheral}; + +const AHB_IDLE: u32 = 1 << 31; +const CORE_RESET: u32 = 1; +const RX_FLUSH: u32 = 1 << 4; +const TX_FLUSH: u32 = 1 << 5; +const GAHBCFG: usize = 0x08; +const GUSBCFG: usize = 0x0c; +const GRSTCTL: usize = 0x10; +const GINTSTS: usize = 0x14; +const GRXSTSR: usize = 0x1c; +const DCFG: usize = 0x800; +const DCTL: usize = 0x804; +const IN_SIZE: usize = 0x910; + +struct Registers(UnsafeCell<[u32; 0x3000 / 4]>); +// Tests hold TEST_LOCK for the complete register/peripheral lifetime in use. +unsafe impl Sync for Registers {} +static REGISTERS: Registers = Registers(UnsafeCell::new([0; 0x3000 / 4])); +static TEST_LOCK: Mutex<()> = Mutex::new(()); + +fn read(offset: usize) -> u32 { + // Only aligned offsets in the allocated register storage are used. + unsafe { + REGISTERS + .0 + .get() + .cast::() + .add(offset / 4) + .read_volatile() + } +} +fn write(offset: usize, value: u32) { + // The emulated peripheral and driver access the same interior-mutable words. + unsafe { + REGISTERS + .0 + .get() + .cast::() + .add(offset / 4) + .write_volatile(value) + } +} +fn snapshot() -> Vec { + (0..0x3000).step_by(4).map(read).collect() +} + +#[derive(Default)] +struct Clock { + now: u64, + step: u64, + calls: u32, + blocked: u32, + second_idle: bool, + regress: bool, + aborted: u32, + hook: Option>, +} +struct Peripheral(Arc>); +// This fixture is serialized and supplies the same aligned backing for all views. +unsafe impl UsbPeripheral for Peripheral { + const REGISTERS: *const () = REGISTERS.0.get().cast(); + const HIGH_SPEED: bool = false; + const FIFO_DEPTH_WORDS: usize = 320; + const ENDPOINT_COUNT: usize = 6; + const WAIT_TIMEOUT_US: u32 = 5_000; + const WAIT_POLL_LIMIT: u32 = 32; + + fn enable() {} + fn ahb_frequency_hz(&self) -> u32 { + 216_000_000 + } + fn now_us(&self) -> u64 { + let (now, hook) = { + let mut clock = self.0.lock().unwrap(); + clock.calls += 1; + clock.now = if clock.regress { + clock.now.saturating_sub(1) + } else { + clock.now + clock.step + }; + let mut reset = read(GRSTCTL); + if clock.blocked & AHB_IDLE != 0 || clock.second_idle && read(GUSBCFG) != 0 { + reset &= !AHB_IDLE; + } + reset &= !(CORE_RESET | RX_FLUSH | TX_FLUSH) | clock.blocked; + write(GRSTCTL, reset); + (clock.now, clock.hook.take()) + }; + if let Some(hook) = hook { + hook(); + } + now + } + fn abort(&self) { + let hook = { + let mut clock = self.0.lock().unwrap(); + clock.aborted += 1; + clock.hook.take() + }; + if let Some(hook) = hook { + hook(); + } + } +} + +type Bus = UsbBus; +struct Fixture { + device: UsbDevice<'static, Bus>, + input: EndpointIn<'static, Bus>, + output: EndpointOut<'static, Bus>, + clock: Arc>, +} +impl Fixture { + fn new(blocked: u32, second_idle: bool) -> Self { + for offset in (0..0x3000).step_by(4) { + write(offset, 0); + } + write(GRSTCTL, AHB_IDLE); + write(0x40, 0x2000); // F722 core identifier + let clock = Arc::new(Mutex::new(Clock { + step: 1_000, + blocked, + second_idle, + ..Clock::default() + })); + let memory = Box::leak(Box::new([0; 32])); + let allocator = Box::leak(Box::new(UsbBus::new(Peripheral(clock.clone()), memory))); + let input = allocator.bulk(64); + let output = allocator.bulk(64); + let device = UsbDeviceBuilder::new(allocator, UsbVidPid(0x1209, 1)) + .max_packet_size_0(64) + .unwrap() + .build(); + write(GINTSTS, 0); // backing storage does not emulate W1C + Self { + device, + input, + output, + clock, + } + } + fn bus(&self) -> &Bus { + self.device.bus() + } + fn block(&self, bits: u32) { + self.clock.lock().unwrap().blocked = bits; + } + fn offline(&self) { + assert!(self.bus().is_offline()); + assert_eq!(self.clock.lock().unwrap().aborted, 1); + let before = snapshot(); + self.bus().reset(); + self.bus().set_device_address(7); + self.bus().set_stalled(self.input.address(), true); + self.bus().suspend(); + self.bus().resume(); + self.bus().disable(); + assert!(self.bus().is_stalled(self.input.address())); + assert!(matches!(self.bus().poll(), PollResult::None)); + assert_eq!(self.input.write(&[1]), Err(UsbError::InvalidState)); + assert_eq!(self.output.read(&mut [0; 64]), Err(UsbError::InvalidState)); + assert_eq!(snapshot(), before); + assert_eq!(self.clock.lock().unwrap().aborted, 1); + } +} + +#[test] +fn register_waits() { + let _test = TEST_LOCK.lock().unwrap(); + for (blocked, second_idle) in [(AHB_IDLE, false), (0, true), (CORE_RESET, false)] { + let fixture = Fixture::new(blocked, second_idle); + fixture.offline(); + assert_eq!(read(GAHBCFG) & 1, 0); + } + for blocked in [RX_FLUSH, TX_FLUSH, RX_FLUSH | TX_FLUSH] { + let fixture = Fixture::new(0, false); + fixture.block(blocked); + write(DCFG, 0x7f << 4); + fixture.bus().reset(); + fixture.offline(); + assert_eq!(read(DCFG), 0x7f << 4); // reset never continued after failure + assert_eq!(read(GAHBCFG) & 1, 0); + } + let fixture = Fixture::new(0, false); + fixture.block(RX_FLUSH); + write(GINTSTS, 1 << 12); // host reset + assert!(matches!(fixture.bus().poll(), PollResult::None)); + fixture.offline(); + + let fixture = Fixture::new(0, false); + fixture.block(TX_FLUSH); + write(GINTSTS, 1 << 4); // RX FIFO level + write(GRXSTSR, 0x06 << 17); // SETUP for EP0 + write(IN_SIZE, 1 << 19); // pending IN packet + assert!(matches!(fixture.bus().poll(), PollResult::None)); + fixture.offline(); +} + +#[test] +fn healthy_reset_and_packets() { + let _test = TEST_LOCK.lock().unwrap(); + let fixture = Fixture::new(0, false); + assert!(!fixture.bus().is_offline()); + assert_eq!(read(GAHBCFG) & 1, 1); + fixture.bus().set_device_address(7); + fixture.bus().reset(); + assert_eq!(read(DCFG) & (0x7f << 4), 0); + assert_eq!(read(GAHBCFG) & 1, 1); + assert_eq!(fixture.clock.lock().unwrap().aborted, 0); + + write(0x900 + fixture.input.address().index() * 0x20 + 0x18, 16); + assert_eq!(fixture.input.write(&[9, 8, 7, 6]), Ok(4)); + assert_eq!( + read(0x1000 + fixture.input.address().index() * 0x1000), + 0x0607_0809 + ); + assert_eq!(fixture.input.write(&[1]), Err(UsbError::WouldBlock)); + + write(GINTSTS, (1 << 12) | (1 << 13)); + write(0x808, 0b11 << 1); // Full-speed enumeration + assert!(matches!(fixture.bus().poll(), PollResult::Reset)); + fixture.bus().reset(); + assert!(!fixture.bus().is_offline()); + write(GINTSTS, 1 << 4); + write(GRXSTSR, (0x06 << 17) | (8 << 4)); + write(IN_SIZE, 1 << 19); + assert!(matches!( + fixture.bus().poll(), + PollResult::Data { ep_setup: 1, .. } + )); + assert!(!fixture.bus().is_offline()); + assert_eq!(fixture.bus().read(0u8.into(), &mut [0; 8]), Ok(8)); + + write(GINTSTS, 1 << 4); + write( + GRXSTSR, + (0x02 << 17) | (4 << 4) | fixture.output.address().index() as u32, + ); + write(0x1000, 0x0403_0201); + assert!(matches!(fixture.bus().poll(), PollResult::Data { .. })); + let mut packet = [0; 64]; + assert_eq!(fixture.output.read(&mut packet), Ok(4)); + assert_eq!(&packet[..4], &[1, 2, 3, 4]); + write(GINTSTS, 0); + write(GAHBCFG, 0); + assert!(matches!(fixture.bus().poll(), PollResult::None)); + assert_eq!(read(GAHBCFG), 0); // preserve an already masked USB source +} + +#[test] +fn broken_clock_and_contention() { + let _test = TEST_LOCK.lock().unwrap(); + for regress in [false, true] { + let fixture = Fixture::new(0, false); + fixture.block(RX_FLUSH); + let before = { + let mut clock = fixture.clock.lock().unwrap(); + clock.step = 0; + clock.regress = regress; + clock.calls + }; + fixture.bus().reset(); + fixture.offline(); + assert!( + fixture.clock.lock().unwrap().calls - before <= 2 * (Peripheral::WAIT_POLL_LIMIT + 1) + ); + } + + for mutation in [false, true] { + let fixture = Box::leak(Box::new(Fixture::new(0, false))); + let bus = fixture.bus(); + let input = fixture.input.address(); + let output = fixture.output.address(); + fixture.clock.lock().unwrap().hook = Some(Box::new(move || { + assert_eq!(read(GAHBCFG) & 1, 0); + assert_eq!(bus.write(input, &[1]), Err(UsbError::WouldBlock)); + assert_eq!(bus.read(output, &mut [0; 64]), Err(UsbError::WouldBlock)); + assert!(matches!(bus.poll(), PollResult::None)); + if mutation { + bus.set_device_address(42); + } else { + bus.disable(); + } + })); + fixture.bus().reset(); + fixture.offline(); + assert_eq!(read(GAHBCFG) & 1, 0); + } +} + +fn other_critical_section() { + let (send, recv) = std::sync::mpsc::channel(); + let worker = std::thread::spawn(move || critical_section::with(|_| send.send(()).unwrap())); + recv.recv_timeout(std::time::Duration::from_secs(1)) + .expect("callback holds global critical section"); + worker.join().unwrap(); +} + +#[test] +fn wait_and_abort_allow_other_critical_sections() { + let _test = TEST_LOCK.lock().unwrap(); + let fixture = Fixture::new(0, false); + fixture.clock.lock().unwrap().hook = Some(Box::new(other_critical_section)); + fixture.bus().reset(); + assert!(!fixture.bus().is_offline()); + assert_eq!(read(DCTL) & (1 << 1), 0); + fixture.clock.lock().unwrap().hook = Some(Box::new(other_critical_section)); + fixture.bus().disable(); + fixture.offline(); +} + +struct Delay(F); +impl embedded_hal::blocking::delay::DelayMs for Delay { + fn delay_ms(&mut self, _ms: u32) { + (self.0)(); + } +} + +#[test] +fn force_reset_failure() { + let _test = TEST_LOCK.lock().unwrap(); + for unwind in [false, true] { + let fixture = Fixture::new(0, false); + let mut delay = Delay(|| { + if unwind { + panic!("delay failed"); + } + fixture.bus().disable(); + }); + let result = std::panic::catch_unwind(std::panic::AssertUnwindSafe(|| { + fixture.bus().force_reset(&mut delay) + })); + if unwind { + assert!(result.is_err()); + } else { + assert_eq!(result.unwrap(), Err(UsbError::InvalidState)); + } + fixture.offline(); + assert_ne!(read(DCTL) & (1 << 1), 0); // Never reconnected after the failed delay. + assert_eq!(read(GAHBCFG) & 1, 0); + } +} + +#[test] +fn flush_requires_idle() { + let _test = TEST_LOCK.lock().unwrap(); + for path in 0..3 { + let fixture = Fixture::new(0, false); + fixture.block(AHB_IDLE); + match path { + 0 => fixture.bus().reset(), + 1 => { + write(GINTSTS, 1 << 12); + assert!(matches!(fixture.bus().poll(), PollResult::None)); + } + _ => { + write(GINTSTS, 1 << 4); + write(GRXSTSR, 0x06 << 17); + write(IN_SIZE, 1 << 19); + assert!(matches!(fixture.bus().poll(), PollResult::None)); + } + } + fixture.offline(); + assert_eq!(read(GRSTCTL) & (RX_FLUSH | TX_FLUSH), 0); + } +} diff --git a/firmware/vendor/synopsys-usb-otg/src/transition.rs b/firmware/vendor/synopsys-usb-otg/src/transition.rs new file mode 100644 index 0000000..3e10948 --- /dev/null +++ b/firmware/vendor/synopsys-usb-otg/src/transition.rs @@ -0,0 +1,41 @@ +use usb_device::endpoint::{EndpointAddress, EndpointType}; + +/// USB endpoint descriptor information. +pub struct EndpointDescriptor { + /// Endpoint address. + pub address: EndpointAddress, + + /// Endpoint transfer type. + pub ep_type: EndpointType, + + /// Maximum packet size. + pub max_packet_size: u16, + + /// Poll interval for interrupt endpoints. + pub interval: u8, +} + +/// Configuration for an endpoint allocation. +pub struct EndpointConfig { + /// The transfer type of the endpoint to be allocated. + pub ep_type: EndpointType, + + /// Maximum packet size for the endpoint to be allocated. + pub max_packet_size: u16, + + /// Poll interval for interrupt endpoints. + pub interval: u8, + + /// Requests a specific endpoint number. Allocation shall fail if the number is not available. + pub number: Option, + + /// Specifies that the endpoint is the "pair" of another endpoint. + /// + /// If `ep` is an endpoint in the opposite direction, this means that the endpoint to be + /// allocated uses the same transfer type as `ep` but in the opposite direction in all alternate + /// settings for the interface. + /// + /// If `ep` is an endpoint in the same direction, this means that the two endpoints belong to + /// different alternate settings for the interface and may never be enabled at the same time. + pub pair_of: Option, +}