public/kaptan
1
Fork 0
Code Issues 24 Pull Requests Actions Packages Projects Releases Wiki Activity

feat(firmware): validate configuration and migrate sensor-driven flight APIs

Browse Source 
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.
This commit is contained in:
Onur Özkan committed 2026-11-07 18:12:11 +03:00
1 parent fb1b8af2d1
commit f9a61a086f
211 files changed
+45519 -12602
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

docs/src/architecture.md
+153 -55
View File
@@ -12,22 +12,72 @@ F722 platform / RTIC application ──► data channel ──► MAVLink teleme
├── physical sensor drivers ├── physical sensor drivers
└── startup motor-pin inhibition └── startup motor-pin inhibition
portable libraries Portable flight libraries (not connected to physical actuators)
math ─► navigation ─► control ─► safety FlightEndpoint ──► Navigation / control / phases / safety
│
└── caller-owned synchronous output sink
``` ```
Each F722 board contract pairs a board-owned `UserConfig` schema and explicit Each F722 board combines `F722UserConfig` with an immutable hardware manifest.
`FACTORY_USER_CONFIG` with a board-owned `ImmutableConfig`. The immutable root The manifest selects the IMU route, driver settings, mount, serial inventory,
is one literal typestate manifest and the sole source of output inhibition, status LED, optional I²C sensor, motor pins and service frequency. User settings
physical routes, driver setup, sensor mount, bootstrap line rates, status LED, select UART roles, baud and MAVLink settings. Motor pins are inhibited at startup.
image role, and service frequency. The user root contains only settings
consumed by that compiled driver graph.
The data channel owns rate selection, mailbox consumption, message encoding, `F722ImmutableConfig<R>` retains the IMU route type through initialization and
and bounded TX classification. Board code supplies physical routes, target interrupt acknowledgement. The sealed `Mpu6000Spi1` and `Icm42688Spi3` routes
schema, and factory values; it does not receive generic output authority. select IMU wiring and backend independently of serial inventory and LED wiring.
Configuration and runtime observability are separate planes: Each board declares one `PortCatalog<N>::DESCRIPTORS` array. Both
`Ports<Catalog, N>` 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<N>`. 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 ```text
FILE_CONTROL / FILE_DATA FILE_CONTROL / FILE_DATA
@@ -42,50 +92,97 @@ kaptan-file-mavlink ──► virtual FileProvider
kaptan-log / kaptan-err-reportable 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 The Kaptan-native file dialect owns bounded transfers and retries across links.
FTP, `PARAM_*`, or a QGC adapter. Its two messages know only bounded paths, Its provider maps saved, active and factory-reset endpoints to streaming KCFG v3
bytes, positions, checksums, transfer ownership, and retries. The configuration and the generic configuration lifecycle.
provider maps `/config/user.kcfg` to a canonical self-describing artifact.
KCFG carries generated metadata and a complete committed value set; the target `ConfigManager` owns boot-active values, known committed values or unknown durable
streams it without retaining a whole-file buffer. Saving imports and validates state, and the factory candidate. F722 supplies its UART-disabled setup fallback
one complete replacement candidate. The core distinguishes only immutable through `recover_with`. Readable invalid settings remain inspectable for USB
boot-active values from the latest committed values. A save cannot mutate repair. Saving or factory reset never changes active consumers until reboot.
controller or estimator state in flight.
Target-owned stores use private persistence formats. F722 alternates compact Target-owned stores use private persistence formats. F722 alternates compact
`KCSP` snapshots between two erase sectors, retaining only the old and `KCSP` snapshots between two erase sectors, retaining only the old and replacement
replacement records across the commit boundary. That format is not exposed as the public KCFG file. records across the commit boundary. That format is not exposed as KCFG. Motor
Motor allocation/mixer topology is immutable compiled vehicle capability data, allocation/mixer topology remains 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.
The MAVLink dialect root directly includes one-message XML leaves under ## I/O ownership
`publish/` and `transfer/`. The `exchange/` contract remains empty until a
current consumer justifies a message. There are no category index XML files or Sensor handoff uses owned `LatestMailbox<T>` storage split into exactly one
separate enum catalogs. The build watches the definitions tree and generates non-cloneable producer and one non-cloneable consumer. Publishing replaces an
one `generated::kaptan` module; leaves are never separately generated Rust unread value, and taking removes it at most once. Each operation copies or takes
modules. `kaptan-log` itself has an `Option<T>` inside the platform critical section. No callback, allocation,
no MAVLink, platform, time, allocation, or filesystem dependency; its sole retry or wait occurs inside that section. Storage stays borrowed while endpoints
configuration coupling is compile-time enum metadata for `LevelFilter`. are in use, and moving, dropping or forgetting an endpoint holds no lock. The
`kaptan-err-reportable` independently owns explicitly assigned reportable-error backend must exclude every participating context, using Cortex-M single-core exclusion on
identity and a bounded nonblocking ingress; it has no logging, MAVLink, F722 and the `critical-section/std` backend for host threads.
platform, filesystem, or clock dependency. Event creation is independent from
sink filtering, allowing a future blackbox and MAVLink to select different UART TX and RX are interrupt-driven, without UART DMA. Each I/O owner retains
policies. The platform drains reportable errors before its bounded critical and the peripheral, TX queue and a 512-byte RX buffer with a typed `Timestamp` per
best-effort log queues and removes a pending record only after TX admission. byte captured at IRQ receipt. The worker takes at most 16 bytes per UART per
`LOG` carries one conventional level plus one bounded text field; `ERROR` turn under a short lock. Protocol parsing, scheduling and persistence run
carries an explicitly assigned 16-bit ID and the bounded `file:line` of the outside UART locks. Overflow or a hardware RX fault discards the damaged backlog
reporting point. Its message kind already identifies a reportable firmware and resets the decoder before later bytes. These bounds do not establish a
error. Kaptan-native heartbeat and sensor publications remain separate from measured latency guarantee.
diagnostics.
`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, See [Configuration](configuration.md) for KCFG and the file-transfer contract,
[Logs](logs.md) for the receiver contract, and [Errors](errors.md) [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. - A peripheral owner is local to the platform runtime.
- Portable crates use typed units and validated values rather than raw wire or - Portable crates use typed units and validated values rather than raw wire or
timer representations. timer representations.
- A safety permit is epoch-bound and must be revalidated at the only nonzero - `SafetySupervisor::evaluate_and_commit` checks current evidence at the
output boundary. synchronous enabled-output boundary. No transferable permit escapes it.
- A failed observation or numerical check does not partially commit estimator, - Rejected numerical updates do not partially commit computational candidates.
controller, allocator, or safety state. Fault latches, inhibition and ingress replay watermarks follow their explicit
failure contracts rather than rolling back authority or replay history.
docs/src/boards/geprc-f722-v2.md
+14 -23
View File
@@ -1,19 +1,13 @@
# GEPRC GEP-F722-HD V2 # GEPRC GEP-F722-HD V2
The GEPRC GEP-F722-HD V2 board declaration uses the ICM-42688-P on SPI3 and The GEPRC GEP-F722-HD V2 uses the ICM-42688-P on SPI3.
MAVLink telemetry on USB CDC-ACM and USART2. It declares no vertical sensor stream. The 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)
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`.
labels the external UART2 pads `T2` and `R2`.
Its board-local `UserConfig` factory selects endpoint `1/1`, IMU and The board declares its yaw-180° IMU mount, UART catalog, active-high PA13 LED
heartbeat publication rates, and an enabled MAVLink diagnostic sink with an and no I²C range sensor.
`Info` level filter. The web configurator may replace those Before fallible initialization, the platform holds the four documented motor
target-advertised values in the journal. Range pins PB0/PB1/PB4/PB5 low.
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.
## Build and package ## Build and package
@@ -53,9 +47,10 @@ make firmware/tools/configurator
Open `http://127.0.0.1:4861` and connect `/dev/serial/by-id/<kaptan-device>` or Open `http://127.0.0.1:4861` and connect `/dev/serial/by-id/<kaptan-device>` or
the discovered `/dev/ttyACM*` path from the Connection section. 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 | | 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/<adapter>` in the same configurator's Connection section. Enter `/dev/serial/by-id/<adapter>` in the same configurator's Connection section.
Open `http://127.0.0.1:4861`, Save the discovered fields, and manually Use UART2's boot-active baud. If its role was changed or recovery disabled it,
power-cycle the controller after Save completes. The flash journal is outside connect over normal USB. Follow [save/reset/reload](../configuration.md#web-configurator)
the application image, so the exact range-limited DFU command above preserves before expecting new settings to apply. Disconnect other transmitters from `R2`
it. A mass erase or full-chip erase removes committed settings and restores while configuring. Physical motor control is not yet implemented.
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.
docs/src/boards/index.md
+13 -3
View File
@@ -3,10 +3,20 @@
Board pages describe supported hardware topology, build products, and safe Board pages describe supported hardware topology, build products, and safe
verification. They do not declare a board flight-ready. 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 | | [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) | STM32F722 | ICM-42688-P | USB CDC-ACM, USART2 | | [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 All board images share the motor-pin inhibit boundary described in
[Safety boundary](../safety-boundary.md). [Safety boundary](../safety-boundary.md).
docs/src/boards/kakute-f7-mini-v3.md
+13 -22
View File
@@ -1,20 +1,14 @@
# Kakute F7 Mini V3 # Kakute F7 Mini V3
The Holybro Kakute F7 Mini V3 board declaration uses the MPU-6000 on SPI1, The Holybro Kakute F7 Mini V3 uses the MPU-6000 on SPI1.
MAVLink telemetry on USB CDC-ACM and USART1, and the MTF01 range/optical-flow
route on UART4.
The checked-in manufacturer datasheet is The checked-in manufacturer datasheet is
`firmware/boards/kakute-f7-mini-v3/datasheet.pdf`. `firmware/boards/kakute-f7-mini-v3/datasheet.pdf`.
Its board-local `UserConfig` factory selects endpoint `1/1`, IMU, range, The board maps MPU axes `(X, Y, Z)` to `(-Y, X, Z)`, matching the manufacturer's
optical-flow and heartbeat publication rates, and an enabled MAVLink diagnostic `KAKUTEF7MINIV3` target, and supplies the driver settings. It declares the UART
sink with an `Info` level filter. The web configurator may replace those catalog, active-low PD2 LED and no I²C range sensor.
target-advertised values in the journal. Its `ImmutableConfig` keeps the Before fallible initialization, the platform holds all six documented
MPU-6000 route, body-aligned motor pins PB0/PB1/PB4/PB5/PB6/PB7 low.
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.
## Build and package ## Build and package
@@ -59,9 +53,10 @@ make firmware/tools/configurator
Open `http://127.0.0.1:4861` and connect `/dev/serial/by-id/<kaptan-device>` or Open `http://127.0.0.1:4861` and connect `/dev/serial/by-id/<kaptan-device>` or
the discovered `/dev/ttyACM*` path from the Connection section. 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 | | 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/<adapter>` in the same configurator's Connection section. Enter `/dev/serial/by-id/<adapter>` in the same configurator's Connection section.
Open `http://127.0.0.1:4861`, edit the discovered fields, press Save, and Use the UART's boot-active baud. If its role was changed or recovery disabled
manually power-cycle the controller after Save completes. The flash journal is UARTs, connect over normal USB. Follow [save/reset/reload](../configuration.md#web-configurator)
outside the application image, so the exact range-limited DFU command above before expecting new settings to apply. Disconnect other transmitters from the
preserves it. A mass erase or full-chip erase removes committed settings and selected RX pad while configuring. Physical motor control is not yet implemented.
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.
docs/src/configuration.md
+221 -106
View File
@@ -1,16 +1,56 @@
# Configuration # Configuration
Kaptan has one generic configuration model in `kaptan-config`. Target schemas `kaptan-config` owns typed fields, validation and the committed-versus-active
declare typed fields and validation once; derive code generates the stable lifecycle. The F722 platform supplies the shared user schema and factory values.
field registry and schema fingerprint. Boards supply the sole erased-flash Boards declare their UART catalog and immutable hardware context. The
factory values. The browser contains no board, driver, sensor, schema, or configurator renders target metadata without a board or parameter catalog.
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 ## Web configurator
@@ -27,155 +67,230 @@ as `/dev/serial/by-id/<kaptan-device>` or `/dev/ttyACM0`. For UART, connect a
3.3 V USB-UART adapter to the board-specific RX/TX pads and enter its device 3.3 V USB-UART adapter to the board-specific RX/TX pads and enter its device
path instead. path instead.
The web tool defaults to MAVLink source system `255` and baud `115200`. The tool defaults to MAVLink source system `255` and baud `115200`.
`--source-system` and `--baud` remain optional command-line overrides. Every `--source-system` and `--baud` are command-line overrides. Match the adapter baud
simultaneously connected GCS must use a different nonzero system ID so transfer to the UART's boot-active value. For example, from `firmware/`, launch
ownership and retries cannot alias another client. `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 1. Connect and wait for discovery to read both saved and boot-active artifacts.
complete `/config/user.kcfg` artifact, validates it, and renders the metadata Each field shows **Boot-active**, **Saved** and **Draft** values.
and values contained in that file. Save produces one complete edited KCFG 2. Edit the draft and press **Save**. The target checks the downloaded baseline,
artifact and uploads it. The target validates the complete candidate before validates the complete replacement and persists it. After success, the tool
atomically persisting it. The boot-active runtime does not change; manually reloads both artifacts to establish the next save's baseline.
power-cycle the controller after a successful save when the UI reports that a 3. To replace saved settings with the target's factory values, press
restart is required. **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 A stale baseline returns `REQUEST_CONFLICT` without writing flash. The tool
one second without telemetry or sensor servicing while the target flash discards that draft. Disconnect and reconnect to reload before editing again.
controller is busy; the host gives the final close a separate five-second If saved state is unknown, ordinary saves are blocked. Reconnect to inspect it,
timeout. Keep the vehicle unpowered except for setup power and do not disconnect then use factory reset or reboot recovery as described below.
it during Save.
The artifact carries generated field IDs, native scalar kinds, display keys, A changed save or explicit factory reset synchronously erases a 128-KiB journal
units, bounds, enum choices, factory values, committed values, schema identity, sector. Flash activity can interrupt telemetry and sensor servicing. The host
and the derived restart-required state. A new board or field therefore needs no allows five seconds for the final close, which is not a measured erase-time
configurator release. `u8`, `u16`, and enum-backed values retain their native guarantee. Keep setup power connected until the operation completes. An unchanged
width rather than being widened at the configuration boundary. ordinary replacement skips flash writes.
The Incoming MAVLink table groups live, CRC-valid Kaptan packets by source and See [MAVLink](mavlink.md) and [Logs](logs.md) for the packet and runtime-log views.
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.
## Board ownership ## Board ownership
Each F722 board declares one `UserConfig` root for the settings consumed by its Each F722 board selects `F722UserConfig<Catalog, N>`. The shared factory values
compiled driver graph. The explicit `FACTORY_USER_CONFIG` literal is the sole leave every configurable UART disabled. USB always carries Kaptan MAVLink and
erased-flash value. There is no `Default` implementation and no platform-wide has no configurable role. Board catalogs determine which UART fields exist.
union of board schemas.
Both current boards expose MAVLink identity, IMU and heartbeat publication Both current boards expose MAVLink identity, publication rates, log filtering,
rates, and the MAVLink log level filter. Kakute also exposes range and and `ports` fields from `kaptan-ports`. Range and optical-flow rates are present
optical-flow rates because its immutable hardware manifest contains an MTF01. on both boards because either can assign an inventoried UART to MTF01. Their
Those fields are absent from GEPRC rather than present-but-disabled. streams are enabled only when the selected sensor service supplies them.
The adjacent `IMMUTABLE_CONFIG` literal remains the sole source of facts that The adjacent `IMMUTABLE_CONFIG` manifest holds:
cannot safely be edited as scalar configuration:
- output-inhibit and status-LED routes; - the IMU route, buses and interrupt wiring
- IMU model, bus, interrupt route, mount, and driver setup; - sensor mount and driver setup
- MAVLink UART and optional vertical-sensor routes and bootstrap baud rates; - the available serial inventory, status LED and optional I²C range route
- the fixed F722 USB CDC-ACM transport; - service-timer frequency.
- service-timer frequency; and
- compiled image role.
The F722 platform consumes stable semantic field IDs through the generic Motor pins are inhibited at startup. The board images have no physical actuator
`Config::field_value` boundary. It does not know a board section, Rust field, driver.
or aggregate shape. Whole-candidate validation checks both generated local
rules and relationships such as publication budget and immutable hardware UART roles and baud are writable settings applied only at boot. Each inventoried
capabilities. UART supports `Disabled = 0`, `KaptanMavlink = 1` (`Role::Mavlink`), and
`Mtf01 = 2`. Disabled ports do not initialize their peripheral or pins.
One board-owned `PortCatalog<N>::DESCRIPTORS` array supplies both
`Ports<Catalog, N>` 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 artifact
KCFG is the public, canonical import/export representation. It is independent KCFG is the public, canonical import/export representation. It is independent
of MAVLink and of the private storage format. A file contains: of MAVLink and of the private storage format. A file contains:
- `KCFG` magic and one format version; - `KCFG` magic, format version **3** and a restart flag
- generated 16-byte schema identity and exact field count; - generated 16-byte schema identity and exact field count
- one canonical self-describing record for every field; and - 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. - a big-endian CRC-32C trailer covering the preceding artifact bytes.
The trailer uses network byte order deliberately. The generic file dialect Exports put the selected view's value in both current and expected-base slots.
also calculates CRC-32C over every complete file byte; network-order storage Edits change only requested values. On ordinary save, the manager compares every
keeps that independent transfer checksum content-specific instead of producing expected-base scalar's native kind and bits with current committed values before
the fixed residue obtained by appending a reflected CRC in little-endian order. 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, An imported file must match the target-generated schema, descriptor metadata,
factory values, field order, native scalar kinds, and exact length. Unknown, factory values, field order, native scalar kinds, and exact length. Unknown,
duplicate, missing, malformed, noncanonical, out-of-range, and relationally duplicate, missing, malformed, noncanonical, out-of-range, and relationally
invalid values are rejected. The target stages only the typed candidate and a invalid requested values are rejected. The target stages typed requested and
small parser state; it never buffers the complete public file. Exports are also expected-base candidates with a small parser state, not a whole-file buffer.
generated by range directly from compiled metadata and current committed Exports are generated by range from metadata and the selected view, with one
values. One small export plan containing the exact length and checksums is length/checksum plan prepared at open.
prepared when a read opens and reused for every requested range.
Schema identity is required because a complete file can otherwise be validly The board-agnostic `decode_kcfg` API instead borrows a complete input artifact
encoded while belonging to another firmware schema. It is generated from the and requires caller-owned 8192-byte field-ID scratch. Hosted callers can allocate
schema and is not a hand-maintained board ID. 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. Schema identity detects schema mismatch. It is not a device identity,
It is not an instruction accepted from an upload: after persistence the target authentication token or proof of all hardware compatibility. Complete-candidate
derives restart state by comparing committed values with immutable boot-active validation against the immutable context remains authoritative.
values.
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 ## Generic file dialect
Kaptan uses two custom messages rather than standard MAVLink FTP or a family of Kaptan uses custom `FILE_CONTROL` and `FILE_DATA` messages rather than standard
configuration-specific requests: MAVLink FTP or configuration-specific messages.
```text
FILE_CONTROL
FILE_DATA
```
`FILE_CONTROL` opens, closes, aborts, and acknowledges a transfer. It `FILE_CONTROL` opens, closes, aborts, and acknowledges a transfer. It
carries a client-selected nonzero `u16` transfer ID, exact target, position, 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 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. result detail. A peer 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 A new ID from the same source and ingress may supersede an open only before the
has advanced; a progressed abandoned transfer remains pinned until lease expiry. first byte advances. A progressed abandoned transfer stays pinned until expiry.
`FILE_DATA` uses the full MAVLink payload budget for a `FILE_DATA` uses the full MAVLink payload budget for a
246-byte chunk. The logical final-chunk length follows from the opened file 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 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`. request carries status `UNSPECIFIED`; a read-data response carries status `OK`.
All failures remain typed `FILE_CONTROL` responses. All failures remain typed `FILE_CONTROL` responses.
The service owns one bounded stop-and-wait transfer. Chunks are sequential; One stop-and-wait transfer is shared across all links. Ownership includes the
immediate retries are replay-safe, conflicting retries are rejected, and stale claimed source system/component and the platform-supplied ingress identity.
transfer IDs cannot mutate a later transfer. A receive fault or bounded lease Chunks are sequential. Immediate retries compare exact retained bytes, not just
expiry aborts staged provider state. The generic layer knows only paths, bytes, checksums. Conflicting retries are rejected. Accepted owner activity renews the
positions, checksums, ownership, and retry state. It does not parse KCFG or ten-second monotonic lease, while rejected traffic does not. Expiry aborts staged
contain configuration policy. 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 For either writable KCFG endpoint, a successful close has generic status `OK`
close has generic status `OK` and content beginning with the authoritative and an authoritative seven-byte KCFG result prefix:
seven-byte KCFG result prefix: restart flag, primary field, related field, and `flags:u8 | primary_field:u16 LE |
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
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 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 KCFG `CONTENT_REJECTED` responses use the same prefix and may append bounded
UTF-8 detail. The configurator rejects a successful close with a missing, UTF-8 detail. The configurator rejects a successful close with a missing,
malformed, or failure-bearing prefix. The file layer passes these bytes through malformed, or failure-bearing prefix. The file layer passes these bytes through
without interpreting them. without interpreting them.
Local USB CDC-ACM and the directly wired UART are the F722 transports. CRC See [MAVLink](mavlink.md) for the local-link trust boundary.
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.
## Private persistence ## 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 flash journal
F722 stores a compact private `KCSP` scalar snapshot inside a two-slot journal. F722 stores a `KCSP` scalar snapshot inside a two-slot journal.
Each entry stores its stable field ID, scalar kind, and value at that kind's Each entry stores its stable field ID, scalar kind and value at native width. Each
native width; an `u8` is one byte rather than a widened integer slot. Each
journal record includes payload length, CRC-32C, commit marker, and a private journal record includes payload length, CRC-32C, commit marker, and a private
monotonic sequence. Saving erases the inactive slot, programs and verifies the monotonic sequence. Saving erases the inactive slot, programs and verifies the
complete replacement, and writes the commit marker last. The previous valid complete replacement, and writes the commit marker last. The previous valid
docs/src/errors.md
+13 -2
View File
@@ -78,14 +78,25 @@ different reportable error is already pending.
`PersistConfigurationError` ID that distinguishes journal geometry, model, `PersistConfigurationError` ID that distinguishes journal geometry, model,
sequence, flash-controller, bounds, unlock, and verification sequence, flash-controller, bounds, unlock, and verification
failures. Replaying the cached close result does not repeat the report or 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 - Reported `ReportableError` values reach the F722 MAVLink worker as a
`ERROR`, subject to bounded transport admission. The payload is `ERROR`, subject to bounded transport admission. The payload is
`{ error_id, raised_at[96] }`. `raised_at` is a zero-padded UTF-8 `{ error_id, raised_at[96] }`. `raised_at` is a zero-padded UTF-8
`file:line` prefix for the reporting point. `file:line` prefix for the reporting point.
Dynamic human-readable context belongs in `LOG`, while Dynamic human-readable context belongs in `LOG`, while
structured protocol failures remain in their owning response message. 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. their live owners are not yet attached to a MAVLink worker.
F722 uses `panic-halt`, without live panic reporting or retained-record replay. F722 uses `panic-halt`, without live panic reporting or retained-record replay.
docs/src/getting-started.md
+2 -2
View File
@@ -30,8 +30,8 @@ target-specific commands in [Development and verification](development.md).
| Path | Responsibility | | Path | Responsibility |
| --- | --- | | --- | --- |
| `firmware/boards/` | Board-owned user schemas, factory values, immutable hardware declarations, and image composition roots. | | `firmware/boards/` | Hardware declarations, UART catalogs and image roots. |
| `firmware/platforms/stm32f7/` | Reviewed F722 RTIC platform, peripheral ownership, and shared linker layout. | | `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/libs/` | Typed math, estimation, control, safety, protocol, and driver crates. |
| `firmware/tools/configurator/` | Board-agnostic loopback web UI and serial bridge. | | `firmware/tools/configurator/` | Board-agnostic loopback web UI and serial bridge. |
| `docs/` | This Markdown documentation site. | | `docs/` | This Markdown documentation site. |
docs/src/index.md
+3 -3
View File
@@ -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, single source of project documentation: board support, safety boundaries,
development workflow, and design references live here as Markdown. development workflow, and design references live here as Markdown.
> **Status:** the checked-in firmware is telemetry-only. It does not expose > **Status:** The board images do not yet run the flight runtime or implement
> physical motor authority, and this site does not make a physical-flight > motor drivers. Motor pins are inhibited at startup. Physical flight has not
> release claim. > been qualified.
## Index ## Index
docs/src/logs.md
+11 -9
View File
@@ -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 diagnostics. Protocol failures remain in their owning response; for example, a
rejected KCFG upload returns a `FILE_CONTROL` failure and readable rejected KCFG upload returns a `FILE_CONTROL` failure and readable
bounded detail. 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 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 not periodic streams. The worker offers each retained event to USB and every
lower transport admits it; temporary queue pressure only defers that same MAVLink UART, then removes it once at least one transport admits it. Admission
event. 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 ## 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 accepts Info, Warn, and Error log events. `Off` suppresses `LOG` on MAVLink
without suppressing reportable `ERROR` events or logs sent to another without suppressing reportable `ERROR` events or logs sent to another
sink. There is no separate enable flag and no MAVLink-specific set of extra 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 runtime log levels. F722 uses the validated boot-active filter. Empty storage
back to its board factory when the journal is empty or invalid. 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 Events retain the compact typed `Level` and sink configuration retains the
compact typed `LevelFilter`. Schema metadata supplies the filter choices to 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 Only the MAVLink adapter converts the five event levels to the typed Kaptan
wire enum. wire enum.
The log ingress is transport-neutral and unfiltered. F722 images accept the The log ingress is transport-neutral and unfiltered.
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.
docs/src/mavlink.md
+55 -36
View File
@@ -9,48 +9,67 @@ dialect. Inbound processing is narrowly scoped:
- Signed frames are discarded because no signature verifier or persistent - Signed frames are discarded because no signature verifier or persistent
replay state exists. replay state exists.
- CRC-valid unsigned MAVLink 2 frames are retained only when their message ID - 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 and extra CRC belong to the Kaptan dialect. The board firmware consumes
are the only inbound records with a current firmware consumer. exact-target Kaptan file requests.
Shared or remote inbound authority is deliberately unsupported. Enabling it Shared or remote inbound authority is deliberately unsupported. Enabling it
requires verified MAVLink 2 signing, configured source/target binding, requires verified MAVLink 2 signing, configured source/target binding,
persistent anti-replay state, and a provisioning/key-rotation policy as one persistent anti-replay state, and a provisioning/key-rotation policy as one
reviewed feature. reviewed feature.
Each F722 image exposes MAVLink on its board-selected UART and as a USB CDC-ACM Each F722 board image initializes USB CDC-ACM as a MAVLink link independently
serial device. Linux normally names the application USB link `/dev/ttyACM*`; of UART roles. Linux normally names it `/dev/ttyACM*`. It is separate from the
this is distinct from the STM32 ROM DFU device used while the Boot pin is STM32 ROM DFU device used for flashing. Every UART assigned `KaptanMavlink` at
asserted. UART and USB have independent receive framers. A host that opens the boot has its own link. USB needs an enumerated device and asserted DTR to carry
CDC port and asserts DTR becomes the publication and diagnostic link; UART is traffic. Opening or closing it does not redirect UART publications.
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 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 The current descriptor uses the pid.codes shared development identity
`1209:0001` plus the STM32 factory device ID as its serial number. Obtain and `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. 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 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 can issue file requests, so use a direct USB cable or USB-UART adapter and
disconnect other transmitters while configuring. The platform retains its disconnect other transmitters while configuring. The platform retains its
startup-configured low motor-output state throughout this workflow; no marker startup-configured low motor-output state throughout this workflow; no marker
type is treated as GPIO ownership or command authority. A networked GCS must type is treated as GPIO ownership or command authority.
add verified signing, source binding, and durable replay protection before it
can receive authority.
The custom Kaptan file service admits one exact source principal for a bounded One file service serializes transfers across USB and all MAVLink UARTs. Its
stop-and-wait transfer. A receive fault or lease expiry aborts its staged owner is the claimed source system/component plus ingress identity, so the same
provider state. Completed control replies have a bounded replay path so a lost wire identity on another port cannot take over a transfer. Completed replies
close response does not repeat a flash operation. This serialization does not have an exact bounded replay path so a lost close response does not repeat a
authenticate its peer. 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 Telemetry transmission uses bounded frame classes. A critical frame may replace
replace pending best-effort traffic, but never a UART-DMA-owned or active USB pending best-effort traffic, but never an active frame. UART TX is IRQ-owned:
frame. UART completion retries preserve ordering, while USB advances a retained each interrupt writes at most one byte and retires at most one completed frame.
frame across bounded CDC endpoint writes. DTR close, suspend, and USB advances a retained frame through bounded CDC endpoint writes. DTR close,
deconfiguration pause USB transmission and return publications to UART. Only a suspend and deconfiguration pause USB TX. Only a bus reset discards its TX state.
bus reset discards USB transmission state. Closing DTR also discards partial Closing DTR discards partial input. UART receive faults reset framing. File and
input. File-transfer and response-replay state expire under the protocol lease, replay state remain governed by the protocol lease rather than disconnects.
not on transport disconnect.
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 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 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 | | Directory | Contract |
| --- | --- | | --- | --- |
| `publish/` | Zero-reply records for any interested receiver. | | `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. | | `transfer/` | Exact-endpoint, bounded multi-message sessions. |
The root includes every message leaf directly; there are no redundant category The root includes every message leaf directly; there are no redundant category
index files or separate enum catalogs. Placement is a wire-contract index files or separate enum catalogs. Placement is a wire-contract
classification, while bindgen still generates one flat `generated::kaptan` classification, while bindgen still generates one flat `generated::kaptan`
module. Bare message names are scoped by that sole dialect and must not be 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 merged with another MAVLink dialect.
an exchange has a current consumer.
`LOG` is an outbound human-readable four-field record. `ERROR` `LOG` is an outbound human-readable four-field record. `ERROR`
is the separate bounded channel for reportable firmware errors, carrying a 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 stable explicit ID plus the `file:line` of its reporting point, and needs no
redundant level field. `FILE_CONTROL` and `FILE_DATA` implement redundant level field. `FILE_CONTROL` and `FILE_DATA` implement
Kaptan's generic virtual-file transport without standard MAVLink FTP Kaptan's generic virtual-file transport without standard MAVLink FTP
compatibility. The current configurator reads and replaces compatibility. The configurator reads saved and boot-active KCFG values and can
`/config/user.kcfg`. See save replacements or reset saved settings to factory values. See
[Configuration](configuration.md), [Logs](logs.md), and [Errors](errors.md) for [Configuration](configuration.md), [Logs](logs.md), and [Errors](errors.md) for
the exact contracts. 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: projections into a broader dialect:
| Publication | Current payload | | Publication | Current payload |
| --- | --- | | --- | --- |
| `HEARTBEAT` (51027) | The single current initialized/output-inhibited state. | | `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. | | `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. | | `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 The IMU payload is 28 bytes with MAVLink extra CRC **22**. Wire field ordering
timestamps, or clamped measurements. 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.
docs/src/safety-boundary.md
+74 -20
View File
@@ -1,34 +1,88 @@
# Safety boundary # Safety boundary
Every checked-in F722 image drives PB0, PB1, PB4, and PB5 low before normal F722 startup drives PB0, PB1, PB4 and PB5 low, plus PB6 and PB7 on Kakute.
board initialization. This startup output inhibition configures each pin as a It configures them as push-pull GPIO outputs before clock-tree setup, board
low, push-pull GPIO output; it runs before clock-tree setup, board routing, routing, UART setup or RTIC tasks.
UART setup, or RTIC tasks. There is no marker or token standing in for the
register writes.
This is a firmware startup property, not a hardware latch. Reset temporarily 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 returns GPIO configuration to the MCU reset state, and the code cannot prove
external routing, pull resistors, ESC polarity, or ESC power behavior. Board external routing, pull resistors, ESC polarity or ESC power behavior. Board
support must document those electrical assumptions; continuous inhibition support must document those electrical assumptions. Continuous inhibition
through reset requires board hardware that provides it. through reset requires board hardware that provides it.
There is no checked-in platform actuator route: 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 DShot or PWM peripheral implementation is linked; no actuator access.
- 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.
Run the structural guard after any platform or board change: Run the structural guard after any platform or board change:
```sh ```sh
make firmware -- test -p platform-stm32f7 \ cd firmware
cargo test -p platform-stm32f7 \
--test platform_safety_boundary --locked --offline --test platform_safety_boundary --locked --offline
``` ```
This boundary is deliberately stronger than an application-level disarm flag: ## Shared runtime commit contract
it removes the physical actuator implementation from the platform image.
`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.
firmware/Cargo.lock
Generated
+23 -37
View File
@@ -52,6 +52,7 @@ checksum = "8ec610d8f49840a5b376c69663b6369e71f4b34484b9b2eb29fb918d92516cb9"
dependencies = [ dependencies = [
"bare-metal", "bare-metal",
"bitfield", "bitfield",
"critical-section",
"embedded-hal", "embedded-hal",
"volatile-register", "volatile-register",
] ]
@@ -222,6 +223,7 @@ dependencies = [
name = "kaptan-data-channel" name = "kaptan-data-channel"
version = "0.1.0" version = "0.1.0"
dependencies = [ dependencies = [
"critical-section",
"kaptan-err-reportable", "kaptan-err-reportable",
"kaptan-mavlink", "kaptan-mavlink",
"kaptan-sensor", "kaptan-sensor",
@@ -239,10 +241,6 @@ dependencies = [
"kaptan-mavlink", "kaptan-mavlink",
] ]
[[package]]
name = "kaptan-dma"
version = "0.1.0"
[[package]] [[package]]
name = "kaptan-drivers" name = "kaptan-drivers"
version = "0.1.0" version = "0.1.0"
@@ -283,9 +281,9 @@ name = "kaptan-flight-config"
version = "0.1.0" version = "0.1.0"
dependencies = [ dependencies = [
"kaptan-config", "kaptan-config",
"kaptan-err-reportable", "kaptan-control",
"kaptan-log", "kaptan-math",
"minicbor", "kaptan-navigation",
] ]
[[package]] [[package]]
@@ -293,15 +291,18 @@ name = "kaptan-flight-runtime"
version = "0.1.0" version = "0.1.0"
dependencies = [ dependencies = [
"kaptan-config", "kaptan-config",
"kaptan-control",
"kaptan-flight-config", "kaptan-flight-config",
"kaptan-log", "kaptan-math",
"kaptan-navigation",
"kaptan-safety",
"kaptan-time",
] ]
[[package]] [[package]]
name = "kaptan-geprc-f722-v2" name = "kaptan-geprc-f722-v2"
version = "0.1.0" version = "0.1.0"
dependencies = [ dependencies = [
"kaptan-config",
"platform-stm32f7", "platform-stm32f7",
"rtic", "rtic",
] ]
@@ -309,12 +310,14 @@ dependencies = [
[[package]] [[package]]
name = "kaptan-i2c" name = "kaptan-i2c"
version = "0.1.0" version = "0.1.0"
dependencies = [
"kaptan-time",
]
[[package]] [[package]]
name = "kaptan-kakute-f7-mini-v3" name = "kaptan-kakute-f7-mini-v3"
version = "0.1.0" version = "0.1.0"
dependencies = [ dependencies = [
"kaptan-config",
"platform-stm32f7", "platform-stm32f7",
"rtic", "rtic",
] ]
@@ -338,11 +341,8 @@ name = "kaptan-mavlink"
version = "0.1.0" version = "0.1.0"
dependencies = [ dependencies = [
"bitflags", "bitflags",
"kaptan-dma",
"kaptan-err-reportable",
"kaptan-sensor", "kaptan-sensor",
"kaptan-time", "kaptan-time",
"kaptan-tx",
"mavlink-bindgen", "mavlink-bindgen",
"mavlink-core", "mavlink-core",
"num-derive", "num-derive",
@@ -353,12 +353,18 @@ dependencies = [
name = "kaptan-navigation" name = "kaptan-navigation"
version = "0.1.0" version = "0.1.0"
dependencies = [ dependencies = [
"kaptan-err-reportable",
"kaptan-math", "kaptan-math",
"kaptan-sensor", "kaptan-sensor",
"kaptan-time", "kaptan-time",
] ]
[[package]]
name = "kaptan-ports"
version = "0.1.0"
dependencies = [
"kaptan-config",
]
[[package]] [[package]]
name = "kaptan-safety" name = "kaptan-safety"
version = "0.1.0" version = "0.1.0"
@@ -424,26 +430,6 @@ version = "2.8.3"
source = "registry+https://github.com/rust-lang/crates.io-index" source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "cf8baf1c55e62ffcace7a9f06f4bd9cd3f0c4beb022d3b367256b91b87513d98" 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]] [[package]]
name = "nb" name = "nb"
version = "0.1.3" version = "0.1.3"
@@ -491,16 +477,17 @@ version = "0.1.0"
dependencies = [ dependencies = [
"cortex-m", "cortex-m",
"cortex-m-rt", "cortex-m-rt",
"critical-section",
"kaptan-config", "kaptan-config",
"kaptan-data-channel", "kaptan-data-channel",
"kaptan-diagnostics-mavlink", "kaptan-diagnostics-mavlink",
"kaptan-dma",
"kaptan-drivers", "kaptan-drivers",
"kaptan-err-reportable", "kaptan-err-reportable",
"kaptan-file-mavlink", "kaptan-file-mavlink",
"kaptan-i2c", "kaptan-i2c",
"kaptan-log", "kaptan-log",
"kaptan-mavlink", "kaptan-mavlink",
"kaptan-ports",
"kaptan-sensor", "kaptan-sensor",
"kaptan-spi", "kaptan-spi",
"kaptan-time", "kaptan-time",
@@ -724,10 +711,9 @@ dependencies = [
[[package]] [[package]]
name = "synopsys-usb-otg" name = "synopsys-usb-otg"
version = "0.4.0" version = "0.4.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "e948d523b316939545d8b21a48c27aef150ce25321b9f95ff7978647a806a6fe"
dependencies = [ dependencies = [
"cortex-m", "cortex-m",
"critical-section",
"embedded-hal", "embedded-hal",
"usb-device", "usb-device",
"vcell", "vcell",
firmware/Cargo.toml
+5 -1
View File
@@ -8,7 +8,6 @@ members = [
"libs/kaptan-config-derive", "libs/kaptan-config-derive",
"libs/kaptan-file-mavlink", "libs/kaptan-file-mavlink",
"libs/kaptan-data-channel", "libs/kaptan-data-channel",
"libs/kaptan-dma",
"libs/kaptan-drivers", "libs/kaptan-drivers",
"libs/kaptan-err-reportable", "libs/kaptan-err-reportable",
"libs/kaptan-err-reportable-macros", "libs/kaptan-err-reportable-macros",
@@ -20,6 +19,7 @@ members = [
"libs/kaptan-log", "libs/kaptan-log",
"libs/kaptan-diagnostics-mavlink", "libs/kaptan-diagnostics-mavlink",
"libs/kaptan-navigation", "libs/kaptan-navigation",
"libs/kaptan-ports",
"libs/kaptan-safety", "libs/kaptan-safety",
"libs/kaptan-sensor", "libs/kaptan-sensor",
"libs/kaptan-spi", "libs/kaptan-spi",
@@ -28,6 +28,7 @@ members = [
"platforms/stm32f7", "platforms/stm32f7",
"tools/configurator", "tools/configurator",
] ]
exclude = ["vendor/synopsys-usb-otg"]
resolver = "2" resolver = "2"
[workspace.package] [workspace.package]
@@ -49,3 +50,6 @@ codegen-units = 1
lto = "fat" lto = "fat"
panic = "abort" panic = "abort"
strip = "symbols" strip = "symbols"
[patch.crates-io]
synopsys-usb-otg = { path = "vendor/synopsys-usb-otg" }
firmware/README.md
+6 -6
View File
@@ -1,15 +1,13 @@
# Firmware # Firmware
This directory is the Kaptan flight-controller firmware workspace. It owns the Kaptan flight-controller firmware workspace.
Rust workspace manifest, board declarations, platform implementation, reusable
firmware crates and firmware scripts.
```text ```text
firmware/ firmware/
├── boards/ immutable board declarations and linker layouts ├── boards/ board hardware declarations and entry points
├── libs/ reusable no_std firmware crates ├── libs/ reusable no_std firmware crates
├── platforms/ hardware platform composition ├── platforms/ hardware composition, configuration and linker layouts
├── tools/ firmware development tooling ├── tools/ local configuration and telemetry tools
└── scripts/ packaging, analysis, and coverage helpers └── 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. `make firmware/boards`, or select one board by its directory path.
Run `make firmware clippy` for Rust lints and `make firmware docs` for API Run `make firmware clippy` for Rust lints and `make firmware docs` for API
documentation with warnings denied. 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.
firmware/boards/geprc-f722-v2/Cargo.toml
+1 -3
View File
@@ -10,10 +10,8 @@ build = "build.rs"
workspace = true workspace = true
[dependencies] [dependencies]
kaptan-config = { path = "../../libs/kaptan-config" }
platform-stm32f7 = { path = "../../platforms/stm32f7" } platform-stm32f7 = { path = "../../platforms/stm32f7" }
# RTIC's attribute expansion emits absolute `rtic` support paths in the final # RTIC's macro emits absolute `rtic` paths in the board crate.
# board crate; the task graph and all RTIC source remain platform-owned.
rtic = { version = "2.3", default-features = false, features = [ rtic = { version = "2.3", default-features = false, features = [
"thumbv7-backend", "thumbv7-backend",
] } ] }
firmware/boards/geprc-f722-v2/src/board.rs
+24 -68
View File
@@ -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::{ use platform_stm32f7::{
F7Board, F722Hardware, F722ImmutableConfig as ImmutableConfig, Icm42688, ImuMount, LevelFilter, F7Board, F722Hardware, F722ImmutableConfig, F722UserConfig, Icm42688Spi3, ImuMount, MotorPins,
Mavlink, Pa13ActiveHigh, Pb0Pb1Pb4Pb5Low, Pc3, VerticalSensor, 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; pub struct GeprcF722V2;
impl F7Board for GeprcF722V2 { impl PortCatalog<5> for GeprcF722V2 {
type UserConfig = UserConfig; const DESCRIPTORS: [PortDescriptor; 5] = [UART1, UART2, UART3, UART4, UART6];
type ImmutableConfig = ImmutableConfig; }
const FACTORY_USER_CONFIG: UserConfig = UserConfig { impl F7Board for GeprcF722V2 {
mavlink: MavlinkConfig { type UserConfig = F722UserConfig<Self, 5>;
system_id: 1, type ImmutableConfig = F722ImmutableConfig<Icm42688Spi3>;
component_id: 1,
log_level_filter: LevelFilter::Info, const FACTORY_USER_CONFIG: Self::UserConfig = Self::UserConfig::FACTORY;
}, const IMMUTABLE_CONFIG: Self::ImmutableConfig = F722ImmutableConfig {
telemetry: TelemetryConfig {
imu_hz: 20,
heartbeat_hz: 2,
},
};
const IMMUTABLE_CONFIG: ImmutableConfig = ImmutableConfig {
name: "GEPRC TAKER F722 V2 (ICM-42688-P)", name: "GEPRC TAKER F722 V2 (ICM-42688-P)",
service_hz: 100, service_hz: 100,
hardware: F722Hardware::new() hardware: F722Hardware::new(
.output_inhibit(Pb0Pb1Pb4Pb5Low) Icm42688Spi3 {
.imu(Icm42688::on_spi3().with_drdy(Pc3).mounted(ImuMount::Yaw180)) mount: ImuMount::Yaw180,
.mavlink(Mavlink::on_usart2().baud(115_200)) },
.vertical_sensor(VerticalSensor::none()) SerialInventory::new(&Self::DESCRIPTORS),
.status_led(Pa13ActiveHigh) StatusLed::ActiveHighPa13,
.telemetry_only(), VerticalSensor::None,
MotorPins::Four,
),
}; };
} }
firmware/boards/kakute-f7-mini-v3/Cargo.toml
+1 -3
View File
@@ -9,10 +9,8 @@ version.workspace = true
workspace = true workspace = true
[dependencies] [dependencies]
kaptan-config = { path = "../../libs/kaptan-config" }
platform-stm32f7 = { path = "../../platforms/stm32f7" } platform-stm32f7 = { path = "../../platforms/stm32f7" }
# RTIC's attribute expansion emits absolute `rtic` support paths in the final # RTIC's macro emits absolute `rtic` paths in the board crate.
# board crate; the task graph and all RTIC source remain platform-owned.
rtic = { version = "2.3", default-features = false, features = [ rtic = { version = "2.3", default-features = false, features = [
"thumbv7-backend", "thumbv7-backend",
] } ] }
firmware/boards/kakute-f7-mini-v3/src/board.rs
+26 -94
View File
@@ -1,110 +1,42 @@
//! Declarative Kakute F7 Mini V3 board description. //! 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::{ use platform_stm32f7::{
AccelerometerRange, DigitalLowPassFilter, F7Board, F722Hardware, AccelerometerRange, DigitalLowPassFilter, F7Board, F722Hardware, F722ImmutableConfig,
F722ImmutableConfig as ImmutableConfig, GyroscopeRange, ImuMount, LevelFilter, Mavlink, F722UserConfig, GyroscopeRange, ImuMount, MotorPins, Mpu6000Config, Mpu6000Spi1,
Mpu6000, Mpu6000Config, Pa4, Pb0Pb1Pb4Pb5Low, Pd2ActiveLow, VerticalSensor, 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; pub struct KakuteF7MiniV3;
impl F7Board for KakuteF7MiniV3 { impl PortCatalog<5> for KakuteF7MiniV3 {
type UserConfig = UserConfig; const DESCRIPTORS: [PortDescriptor; 5] = [UART1, UART2, UART3, UART4, UART6];
type ImmutableConfig = ImmutableConfig; }
const FACTORY_USER_CONFIG: UserConfig = UserConfig { impl F7Board for KakuteF7MiniV3 {
mavlink: MavlinkConfig { type UserConfig = F722UserConfig<Self, 5>;
system_id: 1, type ImmutableConfig = F722ImmutableConfig<Mpu6000Spi1>;
component_id: 1,
log_level_filter: LevelFilter::Info, const FACTORY_USER_CONFIG: Self::UserConfig = Self::UserConfig::FACTORY;
}, const IMMUTABLE_CONFIG: Self::ImmutableConfig = F722ImmutableConfig {
telemetry: TelemetryConfig {
imu_hz: 20,
heartbeat_hz: 2,
range_hz: 20,
optical_flow_hz: 20,
},
};
const IMMUTABLE_CONFIG: ImmutableConfig = ImmutableConfig {
name: "Kakute F7 Mini V3", name: "Kakute F7 Mini V3",
service_hz: 100, service_hz: 100,
hardware: F722Hardware::new() hardware: F722Hardware::new(
.output_inhibit(Pb0Pb1Pb4Pb5Low) Mpu6000Spi1 {
.imu( mount: ImuMount::Yaw270,
Mpu6000::on_spi1() config: Mpu6000Config {
.with_drdy(Pa4)
.mounted(ImuMount::BodyAligned)
.configured(Mpu6000Config {
accelerometer: AccelerometerRange::PlusMinus4G, accelerometer: AccelerometerRange::PlusMinus4G,
gyroscope: GyroscopeRange::PlusMinus500DegreesPerSecond, gyroscope: GyroscopeRange::PlusMinus500DegreesPerSecond,
digital_low_pass_filter: DigitalLowPassFilter::Bandwidth42Hz, digital_low_pass_filter: DigitalLowPassFilter::Bandwidth42Hz,
sample_rate_divider: 9, sample_rate_divider: 9,
}), },
) },
.mavlink(Mavlink::on_usart1().baud(115_200)) SerialInventory::new(&Self::DESCRIPTORS),
.vertical_sensor(VerticalSensor::mtf01_on_uart4().baud(115_200)) StatusLed::ActiveLowPd2,
.status_led(Pd2ActiveLow) VerticalSensor::None,
.telemetry_only(), MotorPins::Six,
),
}; };
} }
firmware/libs/kaptan-config-derive/src/expand.rs
+49 -5
View File
@@ -38,7 +38,7 @@ fn expand_scalars(
checks: field_checks, checks: field_checks,
} = expand_field(&crate_path, field)?; } = expand_field(&crate_path, field)?;
descriptors.push(descriptor); descriptors.push(descriptor);
checks.extend(field_checks); checks.extend(field_checks.iter().cloned());
let id = field.id.expect("scalar configuration fields have IDs"); let id = field.id.expect("scalar configuration fields have IDs");
let ty = &field.ty; let ty = &field.ty;
let field_ident = &field.ident; let field_ident = &field.ident;
@@ -90,7 +90,11 @@ fn expand_scalars(
}; };
let previous = self.#field_ident; let previous = self.#field_ident;
self.#field_ident = next; 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; self.#field_ident = previous;
return Err(#crate_path::FieldWriteError::Violation(violation)); 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), Check::Maximum(value) => Some(value),
_ => None, _ => 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 { 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 key = field.display_key.as_ref().expect("id fields have a key");
let unit = field.unit.as_ref().expect("id fields have a unit"); 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, maximum: #maximum,
enum_options: #enum_options, enum_options: #enum_options,
unit: #unit, unit: #unit,
finite: #finite,
nonzero: #nonzero,
} }
}, },
checks, checks,
@@ -344,7 +377,18 @@ fn config_value_constructor_tokens(crate_path: &syn::Path, ty: &syn::Type) -> Re
Ok(value) 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 { match check {
Check::Finite => quote! { Check::Finite => quote! {
if !self.#field.is_finite() { 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! { Check::Nonzero => quote! {
if self.#field == 0 { if self.#field == #zero {
return Err(#crate_path::ConfigViolation::field( return Err(#crate_path::ConfigViolation::field(
#crate_path::FieldId::new(#id), #crate_path::FieldId::new(#id),
#crate_path::ValidationRule::Zero, #crate_path::ValidationRule::Zero,
firmware/libs/kaptan-config-derive/src/parse.rs
+17 -34
View File
@@ -482,10 +482,12 @@ fn scalar_literal(expression: &Expr, ty: &str) -> Result<ScalarLiteral> {
})?; })?;
Ok(ScalarLiteral::Unsigned(value)) Ok(ScalarLiteral::Unsigned(value))
} }
"f32" => Ok(ScalarLiteral::Float(f64::from( // Parse at the emitted literal's native precision. Parsing through f64
float_literal(expression)? as f32 // first can double-round an f32 bound across an ordering boundary.
))), "f32" => Ok(ScalarLiteral::Float(f64::from(float_literal::<f32>(
"f64" => float_literal(expression).map(ScalarLiteral::Float), expression,
)?))),
"f64" => float_literal::<f64>(expression).map(ScalarLiteral::Float),
_ => Err(syn::Error::new( _ => Err(syn::Error::new(
expression.span(), expression.span(),
"configuration metadata supports bool, i32, u8, u16, u32, u64, f32, and f64", "configuration metadata supports bool, i32, u8, u16, u32, u64, f32, and f64",
@@ -542,7 +544,11 @@ fn unsigned_literal(expression: &Expr) -> Result<u128> {
} }
} }
fn float_literal(expression: &Expr) -> Result<f64> { fn float_literal<T>(expression: &Expr) -> Result<T>
where
T: std::str::FromStr + core::ops::Neg<Output = T>,
T::Err: core::fmt::Display,
{
let (negative, literal) = match expression { let (negative, literal) = match expression {
Expr::Lit(expression) => (false, &expression.lit), Expr::Lit(expression) => (false, &expression.lit),
Expr::Unary(expression) if matches!(expression.op, UnOp::Neg(_)) => { Expr::Unary(expression) if matches!(expression.op, UnOp::Neg(_)) => {
@@ -563,10 +569,10 @@ fn float_literal(expression: &Expr) -> Result<f64> {
}; };
let value = match literal { let value = match literal {
Lit::Int(value) => value Lit::Int(value) => value
.base10_parse::<f64>() .base10_parse::<T>()
.map_err(|error| syn::Error::new(value.span(), error))?, .map_err(|error| syn::Error::new(value.span(), error))?,
Lit::Float(value) => value Lit::Float(value) => value
.base10_parse::<f64>() .base10_parse::<T>()
.map_err(|error| syn::Error::new(value.span(), error))?, .map_err(|error| syn::Error::new(value.span(), error))?,
_ => { _ => {
return Err(syn::Error::new( 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<()> { fn require_numeric(ty: &Type, span: &impl Spanned) -> Result<()> {
if scalar_type_name(ty).is_some_and(|name| { if scalar_type_name(ty)
matches!( .is_some_and(|name| matches!(name, "f32" | "f64" | "i32" | "u8" | "u16" | "u32" | "u64"))
name, {
"f32"
| "f64"
| "i8"
| "i16"
| "i32"
| "i64"
| "i128"
| "isize"
| "u8"
| "u16"
| "u32"
| "u64"
| "u128"
| "usize"
)
}) {
Ok(()) Ok(())
} else { } else {
Err(syn::Error::new( Err(syn::Error::new(
span.span(), 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 == "bool" => "bool",
ident if ident == "f32" => "f32", ident if ident == "f32" => "f32",
ident if ident == "f64" => "f64", ident if ident == "f64" => "f64",
ident if ident == "i8" => "i8",
ident if ident == "i16" => "i16",
ident if ident == "i32" => "i32", 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 == "u8" => "u8",
ident if ident == "u16" => "u16", ident if ident == "u16" => "u16",
ident if ident == "u32" => "u32", ident if ident == "u32" => "u32",
ident if ident == "u64" => "u64", ident if ident == "u64" => "u64",
ident if ident == "u128" => "u128",
ident if ident == "usize" => "usize",
_ => return None, _ => return None,
}) })
} }
firmware/libs/kaptan-config-derive/tests/section.rs
+88
View File
@@ -290,3 +290,91 @@ fn recursive_tree_rejects_duplicate_display_keys() {
Err(issue) if issue.rule() == ValidationRule::Inconsistent 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
}])
);
}
firmware/libs/kaptan-config-derive/tests/ui/f32_double_rounded_bounds.rs
+9
View File
@@ -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() {}
firmware/libs/kaptan-config-derive/tests/ui/f32_double_rounded_bounds.stderr
+5
View File
@@ -0,0 +1,5 @@
error: configuration range minimum exceeds maximum
--> tests/ui/f32_double_rounded_bounds.rs:6:5
|
6 | value: f32,
| ^^^^^
firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_bool.rs
+24
View File
@@ -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() {}
firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_bool.stderr
+17
View File
@@ -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)]
| ^^^^^^
firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_enum.rs
+31
View File
@@ -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() {}
firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_enum.stderr
+17
View File
@@ -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)]
| ^^^^^^
firmware/libs/kaptan-config-derive/tests/ui/unsupported_scalar.rs
+15
View File
@@ -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() {}
firmware/libs/kaptan-config-derive/tests/ui/unsupported_scalar.stderr
+11
View File
@@ -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)]
| ^^^^^^^
firmware/libs/kaptan-config/src/artifact.rs
+270 -96
View File
@@ -2,7 +2,9 @@
//! //!
//! The artifact is independent of any transport and of the private flash //! The artifact is independent of any transport and of the private flash
//! journal. It contains the generated field metadata, one complete committed //! 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; use core::str;
@@ -24,10 +26,16 @@ const CHECKSUM_LENGTH: usize = 4;
const FIELD_PREFIX_LENGTH: usize = 7; const FIELD_PREFIX_LENGTH: usize = 7;
const FIELD_HAS_MINIMUM: u8 = 1 << 0; const FIELD_HAS_MINIMUM: u8 = 1 << 0;
const FIELD_HAS_MAXIMUM: u8 = 1 << 1; 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. /// Current canonical KCFG artifact format.
const KCFG_VERSION: u8 = 1; const KCFG_VERSION: u8 = 3;
/// KCFG header bytes preceding the first field record. /// KCFG header bytes preceding the first field record.
const KCFG_HEADER_LENGTH: usize = FIELDS_OFFSET; const KCFG_HEADER_LENGTH: usize = FIELDS_OFFSET;
/// KCFG trailer bytes containing a big-endian CRC-32C. /// KCFG trailer bytes containing a big-endian CRC-32C.
@@ -208,7 +216,7 @@ pub enum KcfgEncodeError {
} }
/// Failure while decoding untrusted KCFG bytes. /// Failure while decoding untrusted KCFG bytes.
#[derive(Debug)] #[derive(Clone, Copy, Debug, PartialEq)]
pub enum KcfgDecodeError { pub enum KcfgDecodeError {
/// Input ends before a required fixed or variable-length component. /// Input ends before a required fixed or variable-length component.
Truncated, Truncated,
@@ -309,12 +317,15 @@ pub struct KcfgField<'a> {
kind: ValueKind, kind: ValueKind,
current: ConfigValue, current: ConfigValue,
factory: ConfigValue, factory: ConfigValue,
expected_base: ConfigValue,
minimum: Option<ConfigValue>, minimum: Option<ConfigValue>,
maximum: Option<ConfigValue>, maximum: Option<ConfigValue>,
key: &'a str, key: &'a str,
unit: &'a str, unit: &'a str,
options: KcfgEnumOptions<'a>, options: KcfgEnumOptions<'a>,
current_offset: usize, current_offset: usize,
finite: bool,
nonzero: bool,
} }
impl<'a> KcfgField<'a> { impl<'a> KcfgField<'a> {
@@ -338,6 +349,12 @@ impl<'a> KcfgField<'a> {
self.factory 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. /// Inclusive generated lower bound.
pub const fn minimum(self) -> Option<ConfigValue> { pub const fn minimum(self) -> Option<ConfigValue> {
self.minimum self.minimum
@@ -362,6 +379,16 @@ impl<'a> KcfgField<'a> {
pub const fn enum_options(self) -> KcfgEnumOptions<'a> { pub const fn enum_options(self) -> KcfgEnumOptions<'a> {
self.options 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. /// 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. /// Validates and borrows one board-agnostic KCFG artifact.
pub fn decode_kcfg(input: &[u8]) -> Result<KcfgArtifact<'_>, 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<KcfgArtifact<'a>, KcfgDecodeError> {
if input.len() < KCFG_HEADER_LENGTH + KCFG_CHECKSUM_LENGTH { if input.len() < KCFG_HEADER_LENGTH + KCFG_CHECKSUM_LENGTH {
return Err(KcfgDecodeError::Truncated); return Err(KcfgDecodeError::Truncated);
} }
@@ -449,9 +484,13 @@ pub fn decode_kcfg(input: &[u8]) -> Result<KcfgArtifact<'_>, KcfgDecodeError> {
} }
let field_count = read_u16(&input[FIELD_COUNT_OFFSET..FIELDS_OFFSET]); let field_count = read_u16(&input[FIELD_COUNT_OFFSET..FIELDS_OFFSET]);
field_ids.fill(0);
let mut cursor = FIELDS_OFFSET; let mut cursor = FIELDS_OFFSET;
for _ in 0..field_count { 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; cursor = next;
} }
if cursor != checksum_offset { if cursor != checksum_offset {
@@ -466,7 +505,6 @@ pub fn decode_kcfg(input: &[u8]) -> Result<KcfgArtifact<'_>, KcfgDecodeError> {
field_count, field_count,
restart_required: artifact_flags & KCFG_RESTART_REQUIRED != 0, restart_required: artifact_flags & KCFG_RESTART_REQUIRED != 0,
}; };
reject_duplicate_fields(&artifact)?;
Ok(artifact) Ok(artifact)
} }
@@ -482,7 +520,7 @@ pub fn kcfg_length<C: ConfigModel>() -> Result<usize, KcfgEncodeError> {
))?; ))?;
validate_descriptor(descriptor)?; validate_descriptor(descriptor)?;
let width = descriptor.kind.width(); let width = descriptor.kind.width();
let values = 2 let values = 3
+ usize::from(descriptor.minimum.is_some()) + usize::from(descriptor.minimum.is_some())
+ usize::from(descriptor.maximum.is_some()); + usize::from(descriptor.maximum.is_some());
let mut field_length = FIELD_PREFIX_LENGTH let mut field_length = FIELD_PREFIX_LENGTH
@@ -507,11 +545,7 @@ pub fn kcfg_length<C: ConfigModel>() -> Result<usize, KcfgEncodeError> {
Ok(length) Ok(length)
} }
/// Prepared immutable metadata for one canonical generated artifact. /// Cached length and checksums for an immutable export.
///
/// 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.
#[derive(Clone, Copy)] #[derive(Clone, Copy)]
pub struct KcfgExportPlan { pub struct KcfgExportPlan {
length: usize, length: usize,
@@ -520,12 +554,8 @@ pub struct KcfgExportPlan {
restart_required: bool, restart_required: bool,
} }
/// Allocation-free view of one canonical generated artifact. /// Exports arbitrary byte ranges directly from typed configuration.
/// /// Construction computes both the KCFG checksum and the complete-file checksum.
/// 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.
pub struct KcfgExporter<'a, C: ConfigModel> { pub struct KcfgExporter<'a, C: ConfigModel> {
current: &'a C, current: &'a C,
factory: &'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<C> {
/// 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. /// Bounded sequential decoder for a typed KCFG replacement.
/// ///
/// Only the candidate plus eight scalar bytes are retained. Generated /// Only two typed candidates plus fixed scalar/parser state are retained.
/// metadata and factory bytes are checked as they arrive, and a field setter /// 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. /// is called only after its complete canonical record has been received.
/// Discarding the importer discards every partially imported candidate. /// Discarding the importer discards every partially imported candidate.
pub struct KcfgImporter<C: ConfigModel> { pub struct KcfgImporter<C: ConfigModel> {
candidate: C, candidate: C,
expected_base: C,
error: Option<KcfgDecodeError>,
phase: ImportPhase, phase: ImportPhase,
header_offset: usize, header_offset: usize,
field_index: u16, field_index: u16,
field_offset: usize, field_offset: usize,
current_bytes: [u8; 8], current_bytes: [u8; 8],
expected_base_bytes: [u8; 8],
checksum_bytes: [u8; CHECKSUM_LENGTH], checksum_bytes: [u8; CHECKSUM_LENGTH],
checksum_offset: usize, checksum_offset: usize,
crc: Crc32c, crc: Crc32c,
@@ -646,31 +688,48 @@ impl<C: ConfigModel> KcfgImporter<C> {
emit_body(factory, factory, false, |_| true)?; emit_body(factory, factory, false, |_| true)?;
Ok(Self { Ok(Self {
candidate: factory.clone(), candidate: factory.clone(),
expected_base: factory.clone(),
error: None,
phase: ImportPhase::Header, phase: ImportPhase::Header,
header_offset: 0, header_offset: 0,
field_index: 0, field_index: 0,
field_offset: 0, field_offset: 0,
current_bytes: [0; 8], current_bytes: [0; 8],
expected_base_bytes: [0; 8],
checksum_bytes: [0; CHECKSUM_LENGTH], checksum_bytes: [0; CHECKSUM_LENGTH],
checksum_offset: 0, checksum_offset: 0,
crc: Crc32c::new(), 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> { pub fn push(&mut self, factory: &C, input: &[u8]) -> Result<(), KcfgDecodeError> {
if let Some(error) = self.error {
return Err(error);
}
for byte in input { 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(()) Ok(())
} }
/// Returns the complete candidate after checksum and schema validation. /// Returns requested and expected-base candidates after checksum and schema
pub fn finish(self) -> Result<C, KcfgDecodeError> { /// validation. Relational validation belongs to the manager at commit time.
pub fn finish(self) -> Result<KcfgUpdate<C>, KcfgDecodeError> {
if let Some(error) = self.error {
return Err(error);
}
if self.phase != ImportPhase::Done { if self.phase != ImportPhase::Done {
return Err(KcfgDecodeError::Truncated); 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> { fn push_byte(&mut self, factory: &C, byte: u8) -> Result<(), KcfgDecodeError> {
@@ -743,6 +802,7 @@ impl<C: ConfigModel> KcfgImporter<C> {
} }
match expected_field_byte(descriptor, factory, self.field_offset) { match expected_field_byte(descriptor, factory, self.field_offset) {
Some(ExpectedFieldByte::Current(index)) => self.current_bytes[index] = byte, 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 => { Some(ExpectedFieldByte::Factory(expected)) if byte != expected => {
return Err(KcfgDecodeError::FactoryMismatch(descriptor.id)); return Err(KcfgDecodeError::FactoryMismatch(descriptor.id));
} }
@@ -762,7 +822,16 @@ impl<C: ConfigModel> KcfgImporter<C> {
self.candidate self.candidate
.set_field(descriptor.id, value) .set_field(descriptor.id, value)
.map_err(KcfgDecodeError::InvalidWrite)?; .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.current_bytes = [0; 8];
self.expected_base_bytes = [0; 8];
self.field_offset = 0; self.field_offset = 0;
self.field_index += 1; self.field_index += 1;
if self.field_index == C::field_count() { 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 /// 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. /// borrowed metadata to its UI, and provide edited values by stable field ID.
/// The generated metadata, schema, factory values, and enum choices are copied /// The expected-base scalars, metadata, schema, factory values, and enum choices
/// unchanged and the checksum is regenerated here. /// are copied unchanged and the checksum is regenerated here.
pub fn encode_edited_kcfg<'a, F>( pub fn encode_edited_kcfg<'a, F>(
source: &KcfgArtifact<'_>, source: &KcfgArtifact<'_>,
restart_required: bool, restart_required: bool,
@@ -848,31 +917,9 @@ where
#[cfg(test)] #[cfg(test)]
fn decode_kcfg_for<C: ConfigModel>(input: &[u8], factory: &C) -> Result<C, KcfgDecodeError> { fn decode_kcfg_for<C: ConfigModel>(input: &[u8], factory: &C) -> Result<C, KcfgDecodeError> {
validate_model::<C>().map_err(KcfgDecodeError::InvalidModel)?; let mut importer = KcfgImporter::new(factory).expect("valid test model");
let artifact = decode_kcfg(input)?; importer.push(factory, input)?;
if artifact.schema_id != C::SCHEMA_ID { Ok(importer.finish()?.candidate)
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)
} }
fn emit_body<C, F>( fn emit_body<C, F>(
@@ -940,6 +987,10 @@ where
if !emit_value(factory, &mut emit) { if !emit_value(factory, &mut emit) {
return Ok(()); 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 if let Some(minimum) = descriptor.minimum
&& !emit_value(minimum, &mut emit) && !emit_value(minimum, &mut emit)
{ {
@@ -973,16 +1024,17 @@ fn validate_descriptor(descriptor: &crate::FieldDescriptor) -> Result<(), KcfgEn
|| descriptor.unit.contains('\0') || descriptor.unit.contains('\0')
|| descriptor.enum_options.len() > u8::MAX as usize || descriptor.enum_options.len() > u8::MAX as usize
|| (!descriptor.enum_options.is_empty() && descriptor.kind != ValueKind::U8) || (!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)); 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() if label.is_empty()
|| label.len() > u8::MAX as usize || label.len() > u8::MAX as usize
|| label.contains('\0') || label.contains('\0')
|| descriptor.enum_options[..index] || mark_seen(&mut seen, usize::from(*value))
.iter()
.any(|previous| previous.0 == *value)
{ {
return Err(KcfgEncodeError::InvalidMetadata(descriptor.id)); return Err(KcfgEncodeError::InvalidMetadata(descriptor.id));
} }
@@ -995,6 +1047,8 @@ fn parse_field(
start: usize, start: usize,
limit: usize, limit: usize,
) -> Result<(KcfgField<'_>, usize), KcfgDecodeError> { ) -> Result<(KcfgField<'_>, usize), KcfgDecodeError> {
#[cfg(test)]
tests::FIELD_PARSES.with(|count| count.set(count.get() + 1));
let prefix_end = start let prefix_end = start
.checked_add(FIELD_PREFIX_LENGTH) .checked_add(FIELD_PREFIX_LENGTH)
.ok_or(KcfgDecodeError::Truncated)?; .ok_or(KcfgDecodeError::Truncated)?;
@@ -1007,7 +1061,10 @@ fn parse_field(
let kind = let kind =
ValueKind::try_from_code(raw_kind).ok_or(KcfgDecodeError::InvalidValueKind(raw_kind))?; ValueKind::try_from_code(raw_kind).ok_or(KcfgDecodeError::InvalidValueKind(raw_kind))?;
let flags = bytes[start + 3]; 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)); return Err(KcfgDecodeError::InvalidFieldFlags(id));
} }
let key_length = usize::from(bytes[start + 4]); let key_length = usize::from(bytes[start + 4]);
@@ -1021,6 +1078,7 @@ fn parse_field(
let current_offset = cursor; let current_offset = cursor;
let current = parse_value(bytes, &mut cursor, limit, id, kind)?; let current = parse_value(bytes, &mut cursor, limit, id, kind)?;
let factory = 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 { let minimum = if flags & FIELD_HAS_MINIMUM != 0 {
Some(parse_value(bytes, &mut cursor, limit, id, kind)?) Some(parse_value(bytes, &mut cursor, limit, id, kind)?)
} else { } else {
@@ -1047,7 +1105,8 @@ fn parse_field(
canonical_text(&bytes[key_end..unit_end], true).ok_or(KcfgDecodeError::InvalidText(id))?; canonical_text(&bytes[key_end..unit_end], true).ok_or(KcfgDecodeError::InvalidText(id))?;
cursor = unit_end; cursor = unit_end;
let options_start = cursor; 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) { if cursor.checked_add(2).is_none_or(|end| end > limit) {
return Err(KcfgDecodeError::Truncated); return Err(KcfgDecodeError::Truncated);
} }
@@ -1062,11 +1121,7 @@ fn parse_field(
if canonical_text(&bytes[cursor + 2..label_end], false).is_none() { if canonical_text(&bytes[cursor + 2..label_end], false).is_none() {
return Err(KcfgDecodeError::InvalidText(id)); return Err(KcfgDecodeError::InvalidText(id));
} }
let mut previous = KcfgEnumOptions { if mark_seen(&mut seen, usize::from(value)) {
bytes: &bytes[options_start..cursor],
remaining: option_index,
};
if previous.any(|option| option.value == value) {
return Err(KcfgDecodeError::DuplicateEnumValue(id)); return Err(KcfgDecodeError::DuplicateEnumValue(id));
} }
cursor = label_end; cursor = label_end;
@@ -1078,6 +1133,7 @@ fn parse_field(
kind, kind,
current, current,
factory, factory,
expected_base,
minimum, minimum,
maximum, maximum,
key, key,
@@ -1087,22 +1143,21 @@ fn parse_field(
remaining: enum_count, remaining: enum_count,
}, },
current_offset, current_offset,
finite: flags & FIELD_FINITE != 0,
nonzero: flags & FIELD_NONZERO != 0,
}, },
cursor, cursor,
)) ))
} }
fn reject_duplicate_fields(artifact: &KcfgArtifact<'_>) -> Result<(), KcfgDecodeError> { fn mark_seen(bitmap: &mut [u8], value: usize) -> bool {
for (index, field) in artifact.fields().enumerate() { #[cfg(test)]
if artifact tests::BITMAP_PROBES.with(|count| count.set(count.get() + 1));
.fields() let byte = &mut bitmap[value / 8];
.take(index) let bit = 1 << (value % 8);
.any(|previous| previous.id == field.id) let duplicate = *byte & bit != 0;
{ *byte |= bit;
return Err(KcfgDecodeError::DuplicateField(field.id)); duplicate
}
}
Ok(())
} }
fn parse_value( fn parse_value(
@@ -1132,21 +1187,6 @@ fn value_from_bytes(
ConfigValue::from_le_bytes(kind, bytes).ok_or(KcfgDecodeError::InvalidValue(id)) 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> { fn canonical_text(bytes: &[u8], allow_empty: bool) -> Option<&str> {
let text = str::from_utf8(bytes).ok()?; let text = str::from_utf8(bytes).ok()?;
if (!allow_empty && text.is_empty()) || text.contains('\0') { if (!allow_empty && text.is_empty()) || text.contains('\0') {
@@ -1166,6 +1206,7 @@ fn read_u32_be(bytes: &[u8]) -> u32 {
enum ExpectedFieldByte { enum ExpectedFieldByte {
Current(usize), Current(usize),
ExpectedBase(usize),
Factory(u8), Factory(u8),
Descriptor(u8), Descriptor(u8),
} }
@@ -1202,6 +1243,10 @@ fn expected_field_byte(
))); )));
} }
cursor += width; cursor += width;
if offset < cursor + width {
return Some(ExpectedFieldByte::ExpectedBase(offset - cursor));
}
cursor += width;
if let Some(minimum) = descriptor.minimum { if let Some(minimum) = descriptor.minimum {
if offset < cursor + width { if offset < cursor + width {
return Some(ExpectedFieldByte::Descriptor(value_byte( return Some(ExpectedFieldByte::Descriptor(value_byte(
@@ -1254,7 +1299,7 @@ fn expected_field_byte(
fn field_record_length(descriptor: &crate::FieldDescriptor) -> usize { fn field_record_length(descriptor: &crate::FieldDescriptor) -> usize {
let width = descriptor.kind.width(); let width = descriptor.kind.width();
let value_count = 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 FIELD_PREFIX_LENGTH
+ width * value_count + width * value_count
+ descriptor.display_key.len() + descriptor.display_key.len()
@@ -1269,6 +1314,8 @@ fn field_record_length(descriptor: &crate::FieldDescriptor) -> usize {
fn field_flags(descriptor: &crate::FieldDescriptor) -> u8 { fn field_flags(descriptor: &crate::FieldDescriptor) -> u8 {
(u8::from(descriptor.minimum.is_some()) * FIELD_HAS_MINIMUM) (u8::from(descriptor.minimum.is_some()) * FIELD_HAS_MINIMUM)
| (u8::from(descriptor.maximum.is_some()) * FIELD_HAS_MAXIMUM) | (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 { fn value_byte(value: ConfigValue, offset: usize) -> u8 {
@@ -1321,8 +1368,78 @@ impl<'a> RangeSink<'a> {
#[cfg(test)] #[cfg(test)]
mod tests { mod tests {
extern crate std;
use super::*; use super::*;
use crate::{ConfigViolation, FieldDescriptor, ValidationRule}; use crate::{ConfigViolation, FieldDescriptor, ValidationRule};
use std::{vec, vec::Vec};
std::thread_local! {
pub(super) static FIELD_PARSES: core::cell::Cell<usize> = const { core::cell::Cell::new(0) };
pub(super) static BITMAP_PROBES: core::cell::Cell<usize> = const { core::cell::Cell::new(0) };
}
fn raw_artifact(count: u16, last_id: u16, options: &[u8]) -> Vec<u8> {
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<u8> = (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 RATE: FieldId = FieldId::new(1);
const PORT: FieldId = FieldId::new(2); const PORT: FieldId = FieldId::new(2);
@@ -1337,6 +1454,8 @@ mod tests {
maximum: Some(ConfigValue::U8(100)), maximum: Some(ConfigValue::U8(100)),
enum_options: &[], enum_options: &[],
unit: "Hz", unit: "Hz",
finite: false,
nonzero: false,
}, },
FieldDescriptor { FieldDescriptor {
id: PORT, id: PORT,
@@ -1346,6 +1465,8 @@ mod tests {
maximum: Some(ConfigValue::U16(u16::MAX)), maximum: Some(ConfigValue::U16(u16::MAX)),
enum_options: &[], enum_options: &[],
unit: "id", unit: "id",
finite: false,
nonzero: false,
}, },
FieldDescriptor { FieldDescriptor {
id: MODE, id: MODE,
@@ -1355,6 +1476,8 @@ mod tests {
maximum: None, maximum: None,
enum_options: MODE_OPTIONS, enum_options: MODE_OPTIONS,
unit: "level", unit: "level",
finite: false,
nonzero: false,
}, },
]; ];
@@ -1472,7 +1595,8 @@ mod tests {
}; };
let mut bytes = [0; 256]; let mut bytes = [0; 256];
let encoded = encode_kcfg(&current, &factory, true, &mut bytes).unwrap(); let encoded = encode_kcfg(&current, &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_eq!(artifact.schema_id(), Candidate::SCHEMA_ID);
assert!(artifact.restart_required()); assert!(artifact.restart_required());
let mut fields = artifact.fields(); let mut fields = artifact.fields();
@@ -1489,6 +1613,20 @@ mod tests {
} }
}) })
.unwrap(); .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!( assert_eq!(
decode_kcfg_for(edited, &factory).unwrap(), decode_kcfg_for(edited, &factory).unwrap(),
Candidate { Candidate {
@@ -1553,7 +1691,9 @@ mod tests {
for chunk in contiguous[..exporter.length()].chunks(chunk_length) { for chunk in contiguous[..exporter.length()].chunks(chunk_length) {
importer.push(&factory, chunk).unwrap(); 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() { for truncated_length in 0..exporter.length() {
@@ -1597,11 +1737,11 @@ mod tests {
.unwrap() .unwrap()
.len(); .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_offset = length - CHECKSUM_LENGTH;
let checksum = crc32c(&storage[..checksum_offset]); let checksum = crc32c(&storage[..checksum_offset]);
storage[checksum_offset..length].copy_from_slice(&checksum.to_be_bytes()); 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!( assert!(matches!(
decode_kcfg_for(&storage[..length], &factory), decode_kcfg_for(&storage[..length], &factory),
Err(KcfgDecodeError::DescriptorMismatch(RATE)) Err(KcfgDecodeError::DescriptorMismatch(RATE))
@@ -1609,8 +1749,42 @@ mod tests {
storage[checksum_offset] ^= 1; storage[checksum_offset] ^= 1;
assert!(matches!( assert!(matches!(
decode_kcfg(&storage[..length]), decode_kcfg(&storage[..length], &mut [0; KCFG_FIELD_ID_SCRATCH_BYTES]),
Err(KcfgDecodeError::ChecksumMismatch) 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));
}
} }
firmware/libs/kaptan-config/src/error.rs
+3
View File
@@ -22,6 +22,8 @@ pub enum ValidationRule {
UnsupportedFormat = 7, UnsupportedFormat = 7,
/// A scalar does not name a declared variant of its typed enum field. /// A scalar does not name a declared variant of its typed enum field.
InvalidEnumValue = 8, InvalidEnumValue = 8,
/// An ordered range cannot contain NaN.
Unordered = 9,
} }
impl ValidationRule { impl ValidationRule {
@@ -41,6 +43,7 @@ impl ValidationRule {
6 => Self::UnsupportedCapability, 6 => Self::UnsupportedCapability,
7 => Self::UnsupportedFormat, 7 => Self::UnsupportedFormat,
8 => Self::InvalidEnumValue, 8 => Self::InvalidEnumValue,
9 => Self::Unordered,
_ => return None, _ => return None,
}; };
Some(rule) Some(rule)
firmware/libs/kaptan-config/src/field.rs
+19 -13
View File
@@ -3,11 +3,7 @@
use crate::{ConfigViolation, ValidationRule}; use crate::{ConfigViolation, ValidationRule};
use kaptan_err_reportable::KaptanError; use kaptan_err_reportable::KaptanError;
/// Maximum choices representable by one enum-backed field. /// Maximum choice count representable by indexed `u8` discovery.
///
/// Enum values and indexed discovery use their native `u8` domain. Options
/// are immutable flash metadata, so no smaller runtime resource limit is
/// necessary.
pub(crate) const MAX_ENUM_OPTIONS_PER_FIELD: usize = u8::MAX as usize; pub(crate) const MAX_ENUM_OPTIONS_PER_FIELD: usize = u8::MAX as usize;
/// Maximum UTF-8 bytes advertised for one enum variant name. /// Maximum UTF-8 bytes advertised for one enum variant name.
pub(crate) const MAX_ENUM_OPTION_LABEL_BYTES: usize = 32; pub(crate) const MAX_ENUM_OPTION_LABEL_BYTES: usize = 32;
@@ -47,9 +43,9 @@ impl FieldId {
/// The scalar representation of a configuration value. /// The scalar representation of a configuration value.
/// ///
/// The variants deliberately preserve native widths: durations, for example, /// Preserves native widths. Equality compares kind and raw bits, including
/// remain `u64` microseconds instead of being lossy display-unit conversions. /// signed zero and NaN payloads.
#[derive(Clone, Copy, Debug, PartialEq)] #[derive(Clone, Copy, Debug)]
pub enum ConfigValue { pub enum ConfigValue {
/// Boolean value. /// Boolean value.
Bool(bool), Bool(bool),
@@ -69,6 +65,16 @@ pub enum ConfigValue {
F64(f64), 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. /// Failure to project a generic configuration scalar into its native type.
#[doc(hidden)] #[doc(hidden)]
#[derive(Clone, Copy, Debug, Eq, PartialEq)] #[derive(Clone, Copy, Debug, Eq, PartialEq)]
@@ -79,11 +85,7 @@ pub enum ScalarValueError {
OutOfDomain, OutOfDomain,
} }
/// Native representation used by generated configuration fields. /// Conversion between native field types and configuration scalars.
///
/// 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.
#[doc(hidden)] #[doc(hidden)]
pub trait ConfigScalar: Copy { pub trait ConfigScalar: Copy {
/// Generic scalar kind used at schema and transport boundaries. /// Generic scalar kind used at schema and transport boundaries.
@@ -246,6 +248,10 @@ pub struct FieldDescriptor {
pub enum_options: &'static [(u8, &'static str)], pub enum_options: &'static [(u8, &'static str)],
/// Human-readable unit for schema metadata. /// Human-readable unit for schema metadata.
pub unit: &'static str, 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. /// Failure while changing one candidate field.
firmware/libs/kaptan-config/src/journal.rs
+127 -82
View File
@@ -4,9 +4,12 @@
//! independently erasable slots; the generic scalar snapshot codec supplies //! independently erasable slots; the generic scalar snapshot codec supplies
//! the payload. The previous committed slot is never erased until the complete //! the payload. The previous committed slot is never erased until the complete
//! replacement is programmed, verified, and marked committed. //! 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::{ 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"); const MAGIC: u32 = u32::from_le_bytes(*b"KJNL");
@@ -213,23 +216,25 @@ where
Ok(selected.map(|record| record.candidate)) Ok(selected.map(|record| record.candidate))
} }
fn load_matching<P>(&mut self, factory: &C, mut compatible: P) -> Result<Option<C>, Self::Error> fn load_recovery(&mut self, factory: &C) -> Result<StoreRecovery<C>, Self::Error> {
where if let Some(candidate) = self.load(factory)? {
P: FnMut(&C) -> bool, return Ok(StoreRecovery::Committed(candidate));
{ }
self.geometry()?; // Only the no-readable-record case needs an erased-byte scan. Do not
let left = self.record(Slot::Left, factory)?; // infer factory state from erased headers with occupied payload bytes.
let right = self.record(Slot::Right, factory)?; let mut bytes = [0; VERIFY_CHUNK_LENGTH];
let left = left.filter(|record| compatible(&record.candidate)); for slot in 0..2 {
let right = right.filter(|record| compatible(&record.candidate)); for offset in (0..self.flash.slot_size()).step_by(bytes.len()) {
let selected = match (left, right) { let length = bytes.len().min(self.flash.slot_size() - offset);
(Some(left), Some(right)) if right.sequence > left.sequence => Some(right), self.flash
(Some(left), Some(_)) | (Some(left), None) => Some(left), .read(slot, offset, &mut bytes[..length])
(None, Some(right)) => Some(right), .map_err(JournalError::Storage)?;
(None, None) => None, if bytes[..length].iter().any(|&byte| byte != 0xff) {
}; return Ok(StoreRecovery::Unreadable);
self.protected_slot = selected.as_ref().map(|record| record.slot); }
Ok(selected.map(|record| record.candidate)) }
}
Ok(StoreRecovery::Empty)
} }
fn persist(&mut self, candidate: &C) -> Result<(), Self::Error> { fn persist(&mut self, candidate: &C) -> Result<(), Self::Error> {
@@ -294,6 +299,8 @@ where
self.flash self.flash
.program_word(target_slot.index(), COMMIT_OFFSET, COMMIT_MARKER) .program_word(target_slot.index(), COMMIT_OFFSET, COMMIT_MARKER)
.map_err(JournalError::Storage)?; .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 { let Some(record) = self.record(target_slot, candidate)? else {
return Err(JournalError::Verification); return Err(JournalError::Verification);
}; };
@@ -330,7 +337,8 @@ fn read_u64(bytes: &[u8]) -> u64 {
mod tests { mod tests {
use super::*; use super::*;
use crate::{ use crate::{
ConfigValue, ConfigViolation, FieldDescriptor, FieldId, FieldWriteError, ValueKind, ConfigManager, ConfigValue, ConfigViolation, FieldDescriptor, FieldId, FieldWriteError,
ReplaceError, ValueKind,
}; };
const VALUE: FieldId = FieldId::new(401); const VALUE: FieldId = FieldId::new(401);
@@ -342,6 +350,8 @@ mod tests {
maximum: Some(ConfigValue::U32(100)), maximum: Some(ConfigValue::U32(100)),
enum_options: &[], enum_options: &[],
unit: "ticks", unit: "ticks",
finite: false,
nonzero: false,
}; };
#[derive(Clone, Copy, Debug, Eq, PartialEq)] #[derive(Clone, Copy, Debug, Eq, PartialEq)]
@@ -398,15 +408,26 @@ mod tests {
#[derive(Clone, Copy, Debug, Eq, PartialEq)] #[derive(Clone, Copy, Debug, Eq, PartialEq)]
enum FakeError { enum FakeError {
PowerLoss, PowerLoss,
Read,
Program,
Bounds, Bounds,
ZeroToOne, ZeroToOne,
} }
#[derive(Clone, Copy, Debug)]
enum CommitFailure {
Program,
Read(usize),
CorruptRead(usize),
}
#[derive(Clone)] #[derive(Clone)]
struct FakeNor { struct FakeNor {
slots: [[u8; 256]; 2], slots: [[u8; 256]; 2],
operations: usize, operations: usize,
fail_at: Option<usize>, fail_at: Option<usize>,
commit_failure: Option<CommitFailure>,
commit_written: bool,
} }
impl FakeNor { impl FakeNor {
@@ -415,6 +436,8 @@ mod tests {
slots: [[0xff; 256]; 2], slots: [[0xff; 256]; 2],
operations: 0, operations: 0,
fail_at: None, fail_at: None,
commit_failure: None,
commit_written: false,
} }
} }
@@ -446,6 +469,13 @@ mod tests {
.and_then(|slot| slot.get(offset..offset + output.len())) .and_then(|slot| slot.get(offset..offset + output.len()))
.ok_or(FakeError::Bounds)?; .ok_or(FakeError::Bounds)?;
output.copy_from_slice(source); 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(()) Ok(())
} }
@@ -479,6 +509,12 @@ mod tests {
return Err(FakeError::ZeroToOne); return Err(FakeError::ZeroToOne);
} }
destination.copy_from_slice(&bytes); 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(()) Ok(())
} }
} }
@@ -486,6 +522,78 @@ mod tests {
type Journal = DualSlotJournal<FakeNor, 64>; type Journal = DualSlotJournal<FakeNor, 64>;
const FACTORY: Candidate = Candidate(20); 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<Candidate> for &mut Journal {
type Error = JournalError<FakeError>;
fn load(&mut self, factory: &Candidate) -> Result<Option<Candidate>, 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] #[test]
fn alternates_slots_and_recovers_the_newest_record() { fn alternates_slots_and_recovers_the_newest_record() {
let mut journal = Journal::new(FakeNor::erased()); let mut journal = Journal::new(FakeNor::erased());
@@ -538,67 +646,4 @@ mod tests {
let mut recovered = Journal::new(flash); let mut recovered = Journal::new(flash);
assert_eq!(recovered.load(&FACTORY).unwrap(), Some(Candidate(30))); 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"
);
}
}
} }
firmware/libs/kaptan-config/src/lib.rs
+13 -11
View File
@@ -1,11 +1,11 @@
//! Runtime-configuration primitives shared by vehicle-specific schemas. //! Runtime-configuration primitives shared by vehicle-specific schemas.
//! //!
//! This crate has no knowledge of MAVLink, filesystems, board routing, motors, //! Provides configuration lifecycle, field metadata, KCFG artifacts and storage
//! or a particular airframe. It owns the committed-versus-active lifecycle and //! snapshots. Vehicle crates supply the schema and validation.
//! parameter metadata; vehicle crates provide the typed schema and complete //!
//! validation. It also owns the canonical public KCFG artifact and the compact //! A persistence error makes the manager's committed view unknown and blocks
//! private scalar snapshot. Transport and storage framing remain separate //! ordinary saves until reboot recovery or explicit factory reset. The active
//! adapters. //! and factory views remain available; an errored save may become active at boot.
#![no_std] #![no_std]
@@ -22,9 +22,9 @@ pub mod storage;
pub mod validation; pub mod validation;
pub use artifact::{ pub use artifact::{
KcfgArtifact, KcfgDecodeError, KcfgEncodeError, KcfgEnumOption, KcfgEnumOptions, KCFG_FIELD_ID_SCRATCH_BYTES, KcfgArtifact, KcfgDecodeError, KcfgEncodeError, KcfgEnumOption,
KcfgExportPlan, KcfgExporter, KcfgField, KcfgFields, KcfgImporter, KcfgResultDetail, KcfgEnumOptions, KcfgExportPlan, KcfgExporter, KcfgField, KcfgFields, KcfgImporter,
decode_kcfg, encode_edited_kcfg, kcfg_length, KcfgResultDetail, KcfgUpdate, decode_kcfg, encode_edited_kcfg, kcfg_length,
}; };
pub use error::{ConfigViolation, ValidationRule}; pub use error::{ConfigViolation, ValidationRule};
pub use field::{ pub use field::{
@@ -33,10 +33,12 @@ pub use field::{
}; };
pub use journal::{DualSlotJournal, DualSlotNor, JournalError}; pub use journal::{DualSlotJournal, DualSlotNor, JournalError};
pub use kaptan_config_derive::{Config, ConfigEnum}; 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::{ pub use snapshot::{
ScalarSnapshotDecodeError, ScalarSnapshotEncodeError, SnapshotModelError, ScalarSnapshotDecodeError, ScalarSnapshotEncodeError, SnapshotModelError,
decode_scalar_snapshot, encode_scalar_snapshot, scalar_snapshot_length, decode_scalar_snapshot, encode_scalar_snapshot, scalar_snapshot_length,
}; };
pub use storage::ConfigStore; pub use storage::{ConfigStore, StoreRecovery};
pub use validation::Config; pub use validation::Config;
firmware/libs/kaptan-config/src/lifecycle.rs
+298 -62
View File
@@ -2,7 +2,7 @@
use kaptan_err_reportable::KaptanError; 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. /// Failure while recovering a boot configuration from a store.
#[derive(Debug, KaptanError)] #[derive(Debug, KaptanError)]
@@ -16,6 +16,23 @@ pub enum StartupError<E> {
/// The board's compiled factory candidate violates its immutable context. /// The board's compiled factory candidate violates its immutable context.
#[kaptan_error(id = 1103)] #[kaptan_error(id = 1103)]
InvalidFactory(ConfigViolation), 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. /// Result of replacing the complete committed configuration.
@@ -31,9 +48,15 @@ pub enum ReplaceError<E> {
/// The complete candidate violates a relational or capability invariant. /// The complete candidate violates a relational or capability invariant.
#[kaptan_error(id = 1402)] #[kaptan_error(id = 1402)]
InvalidCandidate(ConfigViolation), 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)] #[kaptan_error(id = 1403)]
Store(E), 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. /// Owns the immutable boot-active configuration and latest durable candidate.
@@ -41,12 +64,15 @@ pub enum ReplaceError<E> {
/// There is deliberately no in-place activation operation: [`Self::active`] /// There is deliberately no in-place activation operation: [`Self::active`]
/// never changes after boot. A file endpoint owns its in-progress candidate; /// never changes after boot. A file endpoint owns its in-progress candidate;
/// this manager receives only complete candidates for validation and commit. /// 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<C: ConfigModel, S> { pub struct ConfigManager<C: ConfigModel, S> {
active: C::Active, active: C::Active,
factory: C, factory: C,
committed: C, committed: Option<C>,
context: C::Context, context: C::Context,
store: S, store: S,
diagnostic: RecoveryDisposition,
} }
impl<C, S> ConfigManager<C, S> impl<C, S> ConfigManager<C, S>
@@ -55,58 +81,69 @@ where
S: ConfigStore<C>, S: ConfigStore<C>,
{ {
/// Recovers the newest valid committed snapshot or uses the supplied /// 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( pub fn recover(
factory: C, factory: C,
context: C::Context, context: C::Context,
mut store: S, store: S,
) -> Result<Self, StartupError<S::Error>> { ) -> Result<Self, StartupError<S::Error>> {
let factory_active = factory Self::recover_with(factory, context, store, |_, _, _| None)
.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,
})
} }
/// Recovers the newest candidate compatible with the current context, /// Explicit setup recovery policy. On invalid or unreadable storage the
/// falling back to the supplied factory when no compatible record exists. /// callback may supply a fallback candidate, which this manager validates
/// /// before activation. Returning `None` preserves strict startup failure.
/// This is an explicit recovery policy for output-inhibited setup images. /// Readable invalid committed values remain available for repair. Neither
/// Safety-critical compositions should use [`Self::recover`] if an /// fallback activation nor inspection writes anything to storage.
/// incompatible durable record must prevent initialization. /// The fallback supplies only the active view. It never replaces the
pub fn recover_compatible_or_factory( /// committed candidate or turns unknown durable state into known state.
pub fn recover_with(
factory: C, factory: C,
context: C::Context, context: C::Context,
mut store: S, mut store: S,
recovery: impl FnOnce(&C, &C::Context, RecoveryDisposition) -> Option<C>,
) -> Result<Self, StartupError<S::Error>> { ) -> Result<Self, StartupError<S::Error>> {
let factory_active = factory let factory_active = factory
.validate(&context) .validate(&context)
.map_err(StartupError::InvalidFactory)?; .map_err(StartupError::InvalidFactory)?;
let committed = store let (committed, active, diagnostic) = match store.load_recovery(&factory) {
.load_matching(&factory, |candidate| candidate.validate(&context).is_ok()) Ok(StoreRecovery::Committed(committed)) => {
.map_err(StartupError::Store)?; let active = committed.validate(&context);
let (active, committed) = match committed { let diagnostic = match active {
Some(committed) => { Ok(_) => RecoveryDisposition::Healthy,
let active = committed 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) .validate(&context)
.map_err(StartupError::InvalidPersisted)?; .map_err(StartupError::InvalidRecovery)?
(active, committed)
} }
None => (factory_active, factory.clone()),
}; };
Ok(Self { Ok(Self {
active, active,
@@ -114,6 +151,7 @@ where
committed, committed,
context, context,
store, store,
diagnostic,
}) })
} }
@@ -122,9 +160,12 @@ where
&self.active &self.active
} }
/// Returns the complete last durably committed candidate. /// Returns the readable committed candidate (possibly invalid), or the
pub const fn committed(&self) -> &C { /// empty-store factory selected at boot. Returns `None` when boot could not
&self.committed /// 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. /// Returns the complete board/image factory candidate.
@@ -132,28 +173,76 @@ where
&self.factory &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 { pub fn restart_required(&self) -> bool {
!active_matches_candidate::<C>(&self.active, &self.committed) self.committed
.as_ref()
.is_none_or(|committed| !active_matches_candidate::<C>(&self.active, committed))
} }
/// Validates and atomically persists one complete replacement candidate. /// Validates and atomically persists one complete replacement candidate.
/// ///
/// An unchanged candidate does not consume a flash erase/write cycle. The /// An unchanged candidate does not consume a flash erase/write cycle. The
/// boot-active view remains immutable until a physical restart. /// 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<ReplaceOutcome, ReplaceError<S::Error>> { pub fn replace(&mut self, candidate: C) -> Result<ReplaceOutcome, ReplaceError<S::Error>> {
let committed = self
.committed
.as_ref()
.ok_or(ReplaceError::DurableStateUnknown)?;
candidate candidate
.validate(&self.context) .validate(&self.context)
.map_err(ReplaceError::InvalidCandidate)?; .map_err(ReplaceError::InvalidCandidate)?;
if same_values(&candidate, &self.committed) { if same_values(&candidate, committed) {
return Ok(ReplaceOutcome { return Ok(ReplaceOutcome {
restart_required: self.restart_required(), restart_required: self.restart_required(),
}); });
} }
self.store self.persist(candidate)
.persist(&candidate) }
.map_err(ReplaceError::Store)?;
self.committed = 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<ReplaceOutcome, ReplaceError<S::Error>> {
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<ReplaceOutcome, ReplaceError<S::Error>> {
self.persist(self.factory.clone())
}
fn persist(&mut self, candidate: C) -> Result<ReplaceOutcome, ReplaceError<S::Error>> {
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(); let restart_required = self.restart_required();
if restart_required { if restart_required {
kaptan_log::info!("configuration saved; restart required"); kaptan_log::info!("configuration saved; restart required");
@@ -175,7 +264,7 @@ fn same_values<C: ConfigModel>(left: &C, right: &C) -> bool {
) else { ) else {
return false; return false;
}; };
if left.kind() != right.kind() || left.raw_bits() != right.raw_bits() { if left != right {
return false; return false;
} }
} }
@@ -193,7 +282,7 @@ fn active_matches_candidate<C: ConfigModel>(active: &C::Active, candidate: &C) -
) else { ) else {
return false; return false;
}; };
if active.kind() != candidate.kind() || active.raw_bits() != candidate.raw_bits() { if active != candidate {
return false; return false;
} }
} }
@@ -220,6 +309,8 @@ mod tests {
maximum: Some(ConfigValue::U8(10)), maximum: Some(ConfigValue::U8(10)),
enum_options: &[], enum_options: &[],
unit: "count", unit: "count",
finite: false,
nonzero: false,
}, },
FieldDescriptor { FieldDescriptor {
id: OTHER, id: OTHER,
@@ -229,6 +320,8 @@ mod tests {
maximum: Some(ConfigValue::U8(10)), maximum: Some(ConfigValue::U8(10)),
enum_options: &[], enum_options: &[],
unit: "count", unit: "count",
finite: false,
nonzero: false,
}, },
]; ];
@@ -319,9 +412,24 @@ mod tests {
} }
} }
struct IncompatibleStore; struct FailingStore<'a>(&'a Cell<usize>);
impl ConfigStore<Candidate> for IncompatibleStore { impl ConfigStore<Candidate> for FailingStore<'_> {
type Error = ();
fn load(&mut self, _: &Candidate) -> Result<Option<Candidate>, 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<usize>);
impl ConfigStore<Candidate> for IncompatibleStore<'_> {
type Error = (); type Error = ();
fn load(&mut self, _: &Candidate) -> Result<Option<Candidate>, Self::Error> { fn load(&mut self, _: &Candidate) -> Result<Option<Candidate>, Self::Error> {
@@ -329,6 +437,7 @@ mod tests {
} }
fn persist(&mut self, _: &Candidate) -> Result<(), Self::Error> { fn persist(&mut self, _: &Candidate) -> Result<(), Self::Error> {
self.0.set(self.0.get() + 1);
Ok(()) Ok(())
} }
} }
@@ -362,7 +471,7 @@ mod tests {
ValidationRule::Inconsistent, ValidationRule::Inconsistent,
))) )))
); );
assert_eq!(manager.committed(), &factory); assert_eq!(manager.committed(), Some(&factory));
assert_eq!(writes.get(), 0); assert_eq!(writes.get(), 0);
assert_eq!( assert_eq!(
@@ -396,22 +505,147 @@ mod tests {
restart_required: false, restart_required: false,
}) })
); );
assert_eq!(manager.committed(), &factory); assert_eq!(manager.committed(), Some(&factory));
assert_eq!(manager.factory(), &factory); assert_eq!(manager.factory(), &factory);
assert_eq!(writes.get(), 2); assert_eq!(writes.get(), 2);
} }
#[test] #[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 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!( assert!(matches!(
ConfigManager::recover(factory.clone(), (), IncompatibleStore), ConfigManager::recover(factory.clone(), (), IncompatibleStore(&writes)),
Err(StartupError::InvalidPersisted(_)) Err(StartupError::InvalidPersisted(_))
)); ));
let mut manager = ConfigManager::recover_with(
let manager = factory.clone(),
ConfigManager::recover_compatible_or_factory(factory, (), IncompatibleStore).unwrap(); (),
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!(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<usize>,
fail: &'a Cell<bool>,
}
impl ConfigStore<Candidate> for Store<'_> {
type Error = ();
fn load(&mut self, _: &Candidate) -> Result<Option<Candidate>, ()> {
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] #[test]
@@ -422,7 +656,9 @@ mod tests {
Err(StartupError::InvalidFactory(_)) Err(StartupError::InvalidFactory(_))
)); ));
assert!(matches!( 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(_)) Err(StartupError::InvalidFactory(_))
)); ));
} }
firmware/libs/kaptan-config/src/snapshot.rs
+25 -15
View File
@@ -12,9 +12,7 @@
//! field_id:u16 | value_kind:u8 | value:[value_kind.width] //! field_id:u16 | value_kind:u8 | value:[value_kind.width]
//! ``` //! ```
//! //!
//! Field entries are emitted and decoded in descriptor order. The schema //! Field entries follow descriptor order, as fixed by the schema fingerprint.
//! fingerprint already fixes that order, so accepting permutations would add
//! work without adding a compatible persistence state.
use kaptan_err_reportable::KaptanError; 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. /// 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 /// `value_bytes` must sum the native widths of all fields, each 1 through 8
/// supported by this crate, including 32-bit microcontrollers. /// 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 { pub const fn scalar_snapshot_length(field_count: u16, value_bytes: usize) -> usize {
SCALAR_SNAPSHOT_HEADER_LENGTH let count = field_count as usize;
+ field_count as usize * SCALAR_SNAPSHOT_ENTRY_PREFIX_LENGTH assert!(
+ value_bytes 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. /// 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`. /// Decodes a complete scalar snapshot into a clone of `factory`.
/// ///
/// Structural validation completes before any field setter runs. Each write /// Checks structure before applying writes through [`ConfigModel::set_field`].
/// then passes through [`ConfigModel::set_field`], preserving the model's local /// The caller must validate whole-candidate relations and capabilities.
/// validation. Whole-candidate capability validation remains the composition
/// owner's responsibility because its context is deliberately not part of a
/// storage codec.
pub fn decode_scalar_snapshot<C: ConfigModel>( pub fn decode_scalar_snapshot<C: ConfigModel>(
input: &[u8], input: &[u8],
factory: &C, factory: &C,
@@ -378,6 +376,8 @@ fn read_u16(bytes: &[u8]) -> u16 {
#[cfg(test)] #[cfg(test)]
mod tests { mod tests {
extern crate std;
use super::*; use super::*;
use crate::{ConfigViolation, FieldDescriptor, ValidationRule}; use crate::{ConfigViolation, FieldDescriptor, ValidationRule};
@@ -403,6 +403,15 @@ mod tests {
descriptor(U16, ValueKind::U16), 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 { const fn descriptor(id: FieldId, kind: ValueKind) -> FieldDescriptor {
FieldDescriptor { FieldDescriptor {
id, id,
@@ -412,6 +421,8 @@ mod tests {
maximum: None, maximum: None,
enum_options: &[], enum_options: &[],
unit: "", unit: "",
finite: false,
nonzero: false,
} }
} }
@@ -682,8 +693,7 @@ mod tests {
}) })
); );
// Every numeric kind now occupies its native width, so unlike the old // Native-width encodings cannot carry out-of-width high bits.
// widened u64 representation it cannot carry out-of-width high bits.
} }
#[test] #[test]
firmware/libs/kaptan-config/src/storage.rs
+23 -12
View File
@@ -1,5 +1,15 @@
//! Persistence interface for complete validated candidate snapshots. //! Persistence interface for complete validated candidate snapshots.
/// Storage recovery without conflating erased storage with unreadable records.
pub enum StoreRecovery<C> {
/// 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. /// Durable storage of complete configuration snapshots.
/// ///
/// Flight implementations use a two-slot journal: a record contains a header, /// Flight implementations use a two-slot journal: a record contains a header,
@@ -14,20 +24,21 @@ pub trait ConfigStore<C> {
/// Loads the newest valid committed candidate, if one exists. /// Loads the newest valid committed candidate, if one exists.
fn load(&mut self, factory: &C) -> Result<Option<C>, Self::Error>; fn load(&mut self, factory: &C) -> Result<Option<C>, Self::Error>;
/// Loads the newest structurally valid candidate accepted by `compatible`. /// Recovers a candidate while distinguishing empty from undecodable storage.
/// /// Stores whose `load(None)` can mean occupied storage must override this.
/// Single-record stores inherit this implementation. Journals should fn load_recovery(&mut self, factory: &C) -> Result<StoreRecovery<C>, Self::Error> {
/// override it so an incompatible newest record can fall back to an older Ok(match self.load(factory)? {
/// compatible slot without weakening structural integrity checks. Some(candidate) => StoreRecovery::Committed(candidate),
fn load_matching<F>(&mut self, factory: &C, mut compatible: F) -> Result<Option<C>, Self::Error> None => StoreRecovery::Empty,
where })
F: FnMut(&C) -> bool,
{
Ok(self
.load(factory)?
.filter(|candidate| compatible(candidate)))
} }
/// Persists one complete already-validated candidate atomically. /// 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>; fn persist(&mut self, candidate: &C) -> Result<(), Self::Error>;
} }
firmware/libs/kaptan-config/src/validation.rs
+10 -6
View File
@@ -14,7 +14,8 @@ pub trait Config {
/// Exact value bytes in the flattened private snapshot. /// Exact value bytes in the flattened private snapshot.
const SNAPSHOT_VALUE_BYTES: usize; 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]; const SCHEMA_ID: [u8; 16];
/// Returns a descriptor from the flattened declaration order. /// 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 { if descriptor.enum_options.len() > crate::field::MAX_ENUM_OPTIONS_PER_FIELD {
return false; 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() if option.1.is_empty()
|| option.1.len() > crate::field::MAX_ENUM_OPTION_LABEL_BYTES || option.1.len() > crate::field::MAX_ENUM_OPTION_LABEL_BYTES
|| descriptor.kind != crate::ValueKind::U8 || descriptor.kind != crate::ValueKind::U8
{ {
return false; return false;
} }
if descriptor.enum_options[..index] let byte = usize::from(option.0 / 8);
.iter() let bit = 1 << (option.0 % 8);
.any(|previous| previous.0 == option.0) if seen[byte] & bit != 0 {
{
return false; return false;
} }
seen[byte] |= bit;
} }
} }
true true
@@ -173,6 +175,8 @@ pub const fn __schema_id(descriptors: &[FieldDescriptor]) -> [u8; 16] {
option += 1; option += 1;
} }
state = hash_str(state, descriptor.unit); state = hash_str(state, descriptor.unit);
state = hash_byte(state, descriptor.finite as u8);
state = hash_byte(state, descriptor.nonzero as u8);
index += 1; index += 1;
} }
finish_fingerprint(state) finish_fingerprint(state)
firmware/libs/kaptan-config/tests/transactions.rs
+318
View File
@@ -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] = <Self as kaptan_config::Config>::SCHEMA_ID;
fn descriptor(index: u16) -> Option<&'static FieldDescriptor> {
<Self as kaptan_config::Config>::descriptor(usize::from(index))
}
fn field_count() -> u16 {
<Self as kaptan_config::Config>::FIELD_COUNT as u16
}
fn field_value(&self, id: FieldId) -> Option<ConfigValue> {
<Self as kaptan_config::Config>::field_value(self, id)
}
fn active_field_value(active: &Self::Active, id: FieldId) -> Option<ConfigValue> {
active.field_value(id)
}
fn set_field(&mut self, id: FieldId, value: ConfigValue) -> Result<(), FieldWriteError> {
<Self as kaptan_config::Config>::set_field(self, id, value)
}
fn validate(&self, _: &()) -> Result<Self::Active, ConfigViolation> {
<Self as kaptan_config::Config>::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<usize>);
impl ConfigStore<Scalars> for Store<'_> {
type Error = ();
fn load(&mut self, _: &Scalars) -> Result<Option<Scalars>, ()> {
Ok(None)
}
fn persist(&mut self, _: &Scalars) -> Result<(), ()> {
self.0.set(self.0.get() + 1);
Ok(())
}
}
fn export(current: &Scalars, factory: &Scalars) -> Vec<u8> {
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<u8> {
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<Scalars> {
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(&current, &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(&current, &current)),
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();
}
}
firmware/libs/kaptan-control/src/kani_proofs.rs
+95
View File
@@ -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]);
}
firmware/libs/kaptan-control/src/lib.rs
+931 -189
View File
File diff suppressed because it is too large. Load diff
firmware/libs/kaptan-data-channel/Cargo.toml
+4
View File
@@ -9,6 +9,7 @@ version.workspace = true
workspace = true workspace = true
[dependencies] [dependencies]
critical-section = "1.2"
kaptan-err-reportable = { path = "../kaptan-err-reportable" } kaptan-err-reportable = { path = "../kaptan-err-reportable" }
kaptan-mavlink = { path = "../kaptan-mavlink", default-features = false, features = [ kaptan-mavlink = { path = "../kaptan-mavlink", default-features = false, features = [
"sensor-telemetry", "sensor-telemetry",
@@ -17,3 +18,6 @@ kaptan-sensor = { path = "../kaptan-sensor" }
kaptan-time = { path = "../kaptan-time" } kaptan-time = { path = "../kaptan-time" }
kaptan-tx = { path = "../kaptan-tx" } kaptan-tx = { path = "../kaptan-tx" }
mavlink-core = { version = "0.18", default-features = false } mavlink-core = { version = "0.18", default-features = false }
[target.'cfg(not(target_os = "none"))'.dev-dependencies]
critical-section = { version = "1.2", features = ["std"] }
firmware/libs/kaptan-data-channel/src/lib.rs
+87 -136
View File
@@ -8,19 +8,19 @@
use kaptan_err_reportable::KaptanError; use kaptan_err_reportable::KaptanError;
use kaptan_mavlink::{MavlinkFrame, encoder::MavlinkEncoder, generated::kaptan::MavMessage}; 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 kaptan_tx::{FrameClass, PendingFrame};
use mavlink_core::MessageData; use mavlink_core::MessageData;
mod mailbox; 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. /// Publication rates in hertz.
///
/// 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.
#[derive(Clone, Copy, Debug, Eq, PartialEq)] #[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct DataChannelRates { pub struct DataChannelRates {
/// IMU publication rate. /// IMU publication rate.
@@ -33,15 +33,55 @@ pub struct DataChannelRates {
pub heartbeat_hz: usize, pub heartbeat_hz: usize,
} }
/// Mailboxes read by one data-channel worker. /// Mailboxes read by the single telemetry collector.
#[derive(Clone, Copy)]
pub struct DataSources<'a> { pub struct DataSources<'a> {
/// Latest IMU observation from the acquisition worker. /// Latest IMU observation from the acquisition worker.
pub imu: &'a LatestImuMailbox, pub imu: Consumer<'a, ImuSnapshot>,
/// Latest range observation from either supported vertical route. /// Latest range observation from either supported vertical route.
pub range: &'a LatestRangeMailbox, pub range: Consumer<'a, RangeSnapshot>,
/// Latest optical-flow observation from MTF-01. /// 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<ImuSnapshot>,
/// Latest range observation.
pub range: Option<RangeSnapshot>,
/// Latest optical-flow observation.
pub optical_flow: Option<OpticalFlowSnapshot>,
}
/// 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. /// Data-channel construction failure.
@@ -59,80 +99,70 @@ pub enum DataChannelError {
} }
/// Scheduled outbound MAVLink frame producer. /// Scheduled outbound MAVLink frame producer.
pub struct DataChannel<'a, const SERVICE_HZ: usize> { pub struct DataChannel<const SERVICE_HZ: usize> {
encoder: MavlinkEncoder, encoder: MavlinkEncoder,
schedule: Schedule<SERVICE_HZ>, schedule: Schedule<SERVICE_HZ>,
sources: DataSources<'a>, maximum_vertical_age_microseconds: u64,
maximum_vertical_age_ticks: u8, last_imu_timestamp: Option<Timestamp>,
observed_imu: u32,
observed_range: u32,
last_range: Option<RangeSnapshot>,
range_age_ticks: u8,
observed_optical_flow: u32,
last_optical_flow: Option<OpticalFlowSnapshot>,
optical_flow_age_ticks: u8,
} }
impl<'a, const SERVICE_HZ: usize> DataChannel<'a, SERVICE_HZ> { impl<const SERVICE_HZ: usize> DataChannel<SERVICE_HZ> {
/// Creates an outbound data flow from already-validated runtime inputs. /// Creates an outbound data flow from already-validated runtime inputs.
pub fn new( pub fn new(
system_id: u8, system_id: u8,
component_id: u8, component_id: u8,
rates: DataChannelRates, rates: DataChannelRates,
maximum_vertical_age_ticks: u8, maximum_vertical_age_microseconds: u64,
sources: DataSources<'a>,
) -> Result<Self, DataChannelError> { ) -> Result<Self, DataChannelError> {
let schedule = Schedule::from_rates(rates)?; let schedule = Schedule::from_rates(rates)?;
Ok(Self { Ok(Self {
encoder: MavlinkEncoder::new(system_id, component_id), encoder: MavlinkEncoder::new(system_id, component_id),
schedule, schedule,
sources, maximum_vertical_age_microseconds,
maximum_vertical_age_ticks, last_imu_timestamp: None,
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,
}) })
} }
/// Runs all outbound work due for one platform service tick. /// Runs all outbound work due for one platform service tick.
/// ///
/// At most one classified frame is returned for transport admission. /// At most one classified frame is returned for transport admission.
pub fn on_service_tick(&mut self) -> Option<PendingFrame<MavlinkFrame>> { /// IMU timestamps must increase; recreate the channel for a new clock epoch.
self.refresh_vertical_data(); pub fn on_service_tick(
&mut self,
samples: &TelemetrySamples,
now: Timestamp,
) -> Option<PendingFrame<MavlinkFrame>> {
match self.schedule.next_due() { match self.schedule.next_due() {
Some(DueData::Imu) => { Some(DueData::Imu) => {
self.sources let sample = samples.imu.filter(|sample| {
.imu self.last_imu_timestamp
.take_newer(&mut self.observed_imu) .is_none_or(|last| sample.timestamp > last)
.map(|sample| PendingFrame { })?;
self.last_imu_timestamp = Some(sample.timestamp);
Some(PendingFrame {
class: FrameClass::BestEffort, class: FrameClass::BestEffort,
frame: self.encoder.imu_snapshot(&sample), frame: self.encoder.imu_snapshot(&sample),
}) })
} }
Some(DueData::Distance) if self.range_age_ticks <= self.maximum_vertical_age_ticks => { Some(DueData::Distance) => samples
self.last_range.map(|range| PendingFrame { .range
.filter(|range| self.vertical_is_fresh(range.timestamp, now))
.map(|range| PendingFrame {
class: FrameClass::BestEffort, class: FrameClass::BestEffort,
frame: self.encoder.range_snapshot(&range), frame: self.encoder.range_snapshot(&range),
}) }),
} Some(DueData::OpticalFlow) => samples
Some(DueData::OpticalFlow) .optical_flow
if self.optical_flow_age_ticks <= self.maximum_vertical_age_ticks => .filter(|flow| self.vertical_is_fresh(flow.timestamp, now))
{ .map(|flow| PendingFrame {
self.last_optical_flow.map(|flow| PendingFrame {
class: FrameClass::BestEffort, class: FrameClass::BestEffort,
frame: self.encoder.optical_flow_snapshot(&flow), frame: self.encoder.optical_flow_snapshot(&flow),
}) }),
}
Some(DueData::Heartbeat) => Some(PendingFrame { Some(DueData::Heartbeat) => Some(PendingFrame {
class: FrameClass::Vital, class: FrameClass::Vital,
frame: self.encoder.heartbeat(), 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) { fn vertical_is_fresh(&self, timestamp: Timestamp, now: Timestamp) -> bool {
if let Some(range) = self.sources.range.take_newer(&mut self.observed_range) { now.as_microseconds()
self.last_range = Some(range); .checked_sub(timestamp.as_microseconds())
self.range_age_ticks = 0; .is_some_and(|age| age <= self.maximum_vertical_age_microseconds)
} 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);
}
} }
} }
@@ -245,70 +262,4 @@ impl<const SERVICE_HZ: usize> Schedule<SERVICE_HZ> {
} }
#[cfg(test)] #[cfg(test)]
mod tests { 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::<MavMessage>());
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);
}
}
firmware/libs/kaptan-data-channel/src/mailbox.rs
+185 -192
View File
@@ -1,227 +1,220 @@
//! Lock-free single-producer, single-consumer latest-value handoff. //! Single-producer, single-consumer latest-value handoff.
use core::{
marker::PhantomData,
sync::atomic::{AtomicU32, Ordering},
};
use core::cell::Cell;
use critical_section::Mutex;
use kaptan_sensor::{imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot}; 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 /// Fixed-size latest-value mailbox with one producer and one consumer.
/// [`LatestMailbox`].
/// ///
/// This is a data-channel handoff contract, not a sensor-domain contract: /// A publication replaces any unread value. Each operation acquires the platform
/// sensor crates own their fields and units, while this crate owns their /// critical section only to copy or take an `Option<T>`. There are no callbacks,
/// lock-free representation between an acquisition producer and consumer. /// allocation, retries, or waits inside that section. The platform must supply a
pub trait MailboxValue<const WORDS: usize>: Copy { /// critical-section implementation that excludes every participating context.
/// Converts this observation into its handoff representation. /// On a host this requires the `critical-section` crate's `std` backend.
fn into_words(self) -> [u32; WORDS]; ///
/// Storage is borrowed by the endpoints and cannot move while they are in use.
/// Reconstructs an observation from its handoff representation. /// Dropping or forgetting an endpoint does not leave a lock held. After both
fn from_words(words: [u32; WORDS]) -> Self; /// 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<T: Copy> {
slot: Mutex<Cell<Option<T>>>,
} }
/// Lock-free latest-value SPSC mailbox. impl<T: Copy> LatestMailbox<T> {
/// /// Creates an empty mailbox without acquiring a critical section.
/// 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<Value, const WORDS: usize> {
sequence: AtomicU32,
words: [AtomicU32; WORDS],
value: PhantomData<Value>,
}
impl<Value: MailboxValue<WORDS>, const WORDS: usize> LatestMailbox<Value, WORDS> {
/// Creates an empty mailbox.
pub const fn new() -> Self { pub const fn new() -> Self {
Self { Self {
sequence: AtomicU32::new(0), slot: Mutex::new(Cell::new(None)),
words: [const { AtomicU32::new(0) }; WORDS],
value: PhantomData,
} }
} }
/// Publishes a snapshot, replacing any unread older snapshot. /// Borrows the mailbox as exactly one producer and one consumer.
pub fn publish(&self, value: Value) { ///
let sequence = self.sequence.fetch_add(1, Ordering::SeqCst); /// A second split cannot overlap either endpoint's use:
debug_assert_eq!(sequence & 1, 0, "SPSC producer must not reenter"); ///
/// ```compile_fail,E0499
for (destination, word) in self.words.iter().zip(value.into_words()) { /// use kaptan_data_channel::LatestMailbox;
destination.store(word, Ordering::Relaxed); /// let mut mailbox = LatestMailbox::<u32>::new();
} /// let (mut producer, _consumer) = mailbox.split();
/// let (mut duplicate, _) = mailbox.split();
self.sequence /// producer.publish(1);
.store(sequence.wrapping_add(2), Ordering::SeqCst); /// duplicate.publish(2);
} /// ```
pub fn split(&mut self) -> (Producer<'_, T>, Consumer<'_, T>) {
/// Returns the newest stable snapshot different from `observed_sequence`. (Producer { mailbox: self }, Consumer { mailbox: self })
pub fn take_newer(&self, observed_sequence: &mut u32) -> Option<Value> {
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))
} }
} }
impl<Value: MailboxValue<WORDS>, const WORDS: usize> Default for LatestMailbox<Value, WORDS> { impl<T: Copy> Default for LatestMailbox<T> {
fn default() -> Self { fn default() -> Self {
Self::new() Self::new()
} }
} }
/// Exclusive publication endpoint. Moving it transfers publication ownership.
///
/// ```compile_fail,E0599
/// use kaptan_data_channel::LatestMailbox;
/// let mut mailbox = LatestMailbox::<u32>::new();
/// let (producer, _) = mailbox.split();
/// let duplicate = producer.clone();
/// ```
pub struct Producer<'a, T: Copy> {
mailbox: &'a LatestMailbox<T>,
}
impl<T: Copy> Producer<'_, T> {
/// Publishes a complete value, replacing any unread older value.
///
/// ```compile_fail,E0596
/// use kaptan_data_channel::LatestMailbox;
/// let mut mailbox = LatestMailbox::<u32>::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::<u32>::new();
/// let (_, consumer) = mailbox.split();
/// let duplicate = consumer.clone();
/// ```
pub struct Consumer<'a, T: Copy> {
mailbox: &'a LatestMailbox<T>,
}
impl<T: Copy> Consumer<'_, T> {
/// Takes the latest unread value, or `None` if no publication is pending.
pub fn take(&mut self) -> Option<T> {
critical_section::with(|cs| self.mailbox.slot.borrow(cs).take())
}
}
/// Latest-value mailbox for IMU data-channel snapshots. /// Latest-value mailbox for IMU data-channel snapshots.
pub type LatestImuMailbox = LatestMailbox<ImuSnapshot, 9>; pub type LatestImuMailbox = LatestMailbox<ImuSnapshot>;
/// Latest-value mailbox for range data-channel snapshots. /// Latest-value mailbox for range data-channel snapshots.
pub type LatestRangeMailbox = LatestMailbox<RangeSnapshot, 3>; pub type LatestRangeMailbox = LatestMailbox<RangeSnapshot>;
/// Latest-value mailbox for optical-flow data-channel snapshots. /// Latest-value mailbox for optical-flow data-channel snapshots.
pub type LatestOpticalFlowMailbox = LatestMailbox<OpticalFlowSnapshot, 6>; pub type LatestOpticalFlowMailbox = LatestMailbox<OpticalFlowSnapshot>;
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))
}
#[cfg(test)] #[cfg(test)]
mod tests { mod tests {
use super::{LatestImuMailbox, LatestOpticalFlowMailbox, LatestRangeMailbox}; extern crate std;
use kaptan_sensor::{
imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot, use super::*;
};
use kaptan_time::Timestamp; use kaptan_time::Timestamp;
use std::thread;
#[test] #[test]
fn publishes_only_the_newest_unread_imu_sample() { fn latest_publication_overwrites_and_is_taken_once() {
let mailbox = LatestImuMailbox::new(); let mut mailbox = LatestImuMailbox::new();
let mut observed = 0; let (mut producer, mut consumer) = mailbox.split();
mailbox.publish(ImuSnapshot::new( assert_eq!(consumer.take(), None);
Timestamp::from_microseconds(1), let first = ImuSnapshot::new(Timestamp::from_microseconds(1), [1; 3], [2; 3], 3);
[1; 3], let latest = ImuSnapshot::new(
[2; 3], Timestamp::from_microseconds(u64::MAX),
3, [-32_768, 0, 32_767],
)); [-34_906, 0, 34_906],
mailbox.publish(ImuSnapshot::new( -2_501,
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))
); );
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] #[test]
fn preserves_optical_flow_fields() { fn vertical_snapshots_preserve_all_fields() {
let mailbox = LatestOpticalFlowMailbox::new(); let mut range = LatestRangeMailbox::new();
let mut observed = 0; let mut flow = LatestOpticalFlowMailbox::new();
let sample = OpticalFlowSnapshot::new(Timestamp::from_microseconds(1), [-6, 7], 8, 9); let (mut range_producer, mut range_consumer) = range.split();
mailbox.publish(sample); 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);
});
} }
} }
firmware/libs/kaptan-data-channel/src/tests.rs
+442
View File
@@ -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<MavlinkFrame>) -> MavMessage {
assert!(pending.frame.has_valid_crc::<MavMessage>());
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<MavlinkFrame>, 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)
);
}
}
firmware/libs/kaptan-diagnostics-mavlink/src/lib.rs
+3 -5
View File
@@ -1,8 +1,6 @@
//! MAVLink projection for Kaptan logs and reportable firmware errors. //! MAVLink projection for Kaptan logs and reportable firmware errors.
//! //!
//! This adapter creates generated payload data only. A platform-owned link is //! Produces payloads. The caller handles framing and transmission.
//! solely responsible for MAVLink framing, bounded admission, DMA, and retry
//! policy.
#![no_std] #![no_std]
@@ -15,7 +13,7 @@ use kaptan_mavlink::generated::kaptan::{ERROR_DATA, LOG_DATA, LogFlags, LogLevel
const ERROR_RAISED_AT_BYTES: usize = 96; 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 { pub fn encode_log(record: &LogRecord) -> LOG_DATA {
LOG_DATA { LOG_DATA {
level: wire_level(record.level()), 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 { pub fn encode_error(record: &ErrorRecord) -> ERROR_DATA {
let source = record.raised_at().as_bytes(); let source = record.raised_at().as_bytes();
let mut length = source.len().min(ERROR_RAISED_AT_BYTES); let mut length = source.len().min(ERROR_RAISED_AT_BYTES);
firmware/libs/kaptan-dma/src/lib.rs
-73
View File
@@ -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<DmaReadableBuffer<'a>, 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>;
}
firmware/libs/kaptan-drivers/src/icm42688/error.rs
+12
View File
@@ -18,4 +18,16 @@ pub enum Error<BusError> {
/// The selected SPI route reported a terminal transfer fault. /// The selected SPI route reported a terminal transfer fault.
#[kaptan_error(id = 5104)] #[kaptan_error(id = 5104)]
TransferFault, 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,
} }
firmware/libs/kaptan-drivers/src/icm42688/mod.rs
+438 -137
View File
@@ -5,7 +5,7 @@ mod sample;
pub mod service; pub mod service;
use kaptan_sensor::imu::ImuSnapshot; use kaptan_sensor::imu::ImuSnapshot;
use kaptan_spi::{SpiCompletion, SpiDevice}; use kaptan_spi::{CompletionError, SpiCompletion, SpiDevice};
use kaptan_time::Timestamp; use kaptan_time::Timestamp;
pub use error::Error; pub use error::Error;
@@ -27,6 +27,19 @@ const INTF_CONFIG1: u8 = 0x4d;
const TEMP_DATA1: u8 = 0x1d; const TEMP_DATA1: u8 = 0x1d;
const SAMPLE_BYTES: usize = 14; const SAMPLE_BYTES: usize = 14;
const TRANSFER_CAPACITY: usize = SAMPLE_BYTES + 1; 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 // 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. // 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)] #[derive(Clone, Copy, Debug, Eq, PartialEq)]
enum State { enum State {
Dormant, Dormant,
Resetting, Pending {
WaitingForReset, operation: Operation,
ReadingIdentity, since: Timestamp,
WritingConfiguration(usize), },
WaitingForSensorStart, Active {
Ready, operation: Operation,
ReadingSample { timestamp: Timestamp }, 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)] #[derive(Debug, Eq, PartialEq)]
pub enum Icm42688Event<BusError> { pub enum Icm42688Event<BusError> {
/// The selected SPI interrupt had no terminal transfer event. /// No sample, initialization completion or terminal fault was produced.
NotPending, NotPending,
/// One initialization transfer completed; initialization continues. /// One initialization transfer completed; initialization continues.
Initializing, Initializing,
@@ -73,9 +107,8 @@ pub enum Icm42688Event<BusError> {
/// Configured ICM-42688-P driver. /// Configured ICM-42688-P driver.
/// ///
/// The driver owns fixed transfer buffers and progresses exclusively from /// The driver owns ordinary movable staging buffers and progresses from timer,
/// timer, data-ready and SPI-DMA completion events. It never waits for a /// data-ready and completion events. Hardware never addresses these buffers.
/// peripheral status flag.
pub struct Icm42688<Bus> { pub struct Icm42688<Bus> {
bus: Bus, bus: Bus,
state: State, state: State,
@@ -96,125 +129,139 @@ impl<Bus> Icm42688<Bus> {
} }
impl<Bus: SpiDevice> Icm42688<Bus> { impl<Bus: SpiDevice> Icm42688<Bus> {
/// Starts the sensor reset. A subsequent periodic tick provides the /// Accepts initialization once. Busy admission is retried by periodic
/// datasheet-required reset-settling interval before identification. /// service. All supplied instants must use one non-regressing boot clock.
pub fn begin_initialization(&mut self) -> Result<(), Error<Bus::StartError>> { /// Power must be stable by this call. Register access waits another 1 ms.
pub fn begin_initialization(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
if self.state != State::Dormant { if self.state != State::Dormant {
return Err(Error::NotReady); return Err(Error::NotReady);
} }
self.prepare_write(DEVICE_CONFIG, SOFT_RESET); self.wait(Operation::Reset, now, RESET_SETTLE_US);
self.submit(2)?;
self.state = State::Resetting;
Ok(()) Ok(())
} }
/// Advances reset/wake delays from the board periodic event. /// Services elapsed settling, admission and acquisition deadlines. Call
pub fn advance_initialization(&mut self) -> Result<(), Error<Bus::StartError>> { /// periodically even with an active transfer. No unsolicited reads occur.
match self.state { pub fn on_periodic_tick(&mut self, now: Timestamp) -> Icm42688Event<Bus::StartError> {
State::WaitingForReset => { if let Some(error) = self.check_deadline(now) {
self.prepare_read(WHO_AM_I, 2); return Icm42688Event::Fault(error);
self.submit(2)?;
self.state = State::ReadingIdentity;
} }
State::WaitingForSensorStart => { match self.state {
self.start_configuration(7)?; State::Waiting {
next,
since,
delay_us,
} if elapsed(now, since, delay_us) => {
self.queue(next, now);
}
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. /// Accepts a burst-read request. Busy admission remains pending for periodic
pub fn request_sample(&mut self, timestamp: Timestamp) -> Result<(), Error<Bus::StartError>> { /// retry. The eventual accepted submission time becomes the snapshot
if self.state != State::Ready { /// timestamp, not the ISR time or a physical sampling epoch.
pub fn request_sample(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
if let Some(error) = self.check_deadline(now) {
return Err(error);
}
if !self.is_ready() {
return Err(Error::NotReady); return Err(Error::NotReady);
} }
self.prepare_read(TEMP_DATA1, TRANSFER_CAPACITY); self.queue(Operation::Sample, now);
self.submit(TRANSFER_CAPACITY)?; self.submit_pending(now)
self.state = State::ReadingSample { timestamp };
Ok(())
} }
/// Advances the state machine from an already-acknowledged SPI completion. /// Advances the state machine from an already-acknowledged SPI completion.
pub fn on_transfer_complete( pub fn on_transfer_complete(
&mut self, &mut self,
completion: SpiCompletion, completion: SpiCompletion,
now: Timestamp,
) -> Icm42688Event<Bus::StartError> { ) -> Icm42688Event<Bus::StartError> {
self.bus.complete_transfer(completion); if completion == SpiCompletion::NotPending {
match completion { return Icm42688Event::NotPending;
SpiCompletion::NotPending => Icm42688Event::NotPending, }
SpiCompletion::TransferFault => { if let Some(error) = self.check_deadline(now) {
self.state = State::Dormant; 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) 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. /// Returns whether an IMU data-ready event can start a sensor transfer.
pub fn is_ready(&self) -> bool { pub fn is_ready(&self) -> bool {
self.state == State::Ready matches!(self.state, State::Ready { .. })
} }
fn advance_after_completion(&mut self) -> Icm42688Event<Bus::StartError> { fn advance_after_completion(
match self.state { &mut self,
State::Dormant => Icm42688Event::NotPending, operation: Operation,
State::Resetting => { requested: Timestamp,
self.state = State::WaitingForReset; now: Timestamp,
Icm42688Event::Initializing ) -> Icm42688Event<Bus::StartError> {
} match operation {
State::WaitingForReset | State::WaitingForSensorStart => Icm42688Event::NotPending, Operation::Reset => self.wait(Operation::Identity, now, RESET_SETTLE_US),
State::ReadingIdentity => { Operation::Identity => {
let identity = self.receive[1]; let identity = self.receive[1];
if identity != EXPECTED_WHO_AM_I { if identity != EXPECTED_WHO_AM_I {
self.state = State::Dormant; self.state = State::Faulted;
return Icm42688Event::Fault(Error::UnexpectedIdentity(identity)); 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; let next = index + 1;
if next == 7 { if next == 7 {
// PWR_MGMT0 starts both sensor paths. The following // Includes the 200 us no-write interval after PWR_MGMT0.
// timer tick gives them more than the required 200 µs self.wait(Operation::Configure(next), now, SENSOR_SETTLE_US);
// before their output-rate configuration is written.
self.state = State::WaitingForSensorStart;
Icm42688Event::Initializing
} else if next == CONFIGURATION.len() { } else if next == CONFIGURATION.len() {
self.state = State::Ready; self.state = State::Settling { since: now };
Icm42688Event::Initialized
} else { } else {
self.start_configuration_event(next) self.queue(Operation::Configure(next), now);
} }
} }
State::Ready => Icm42688Event::NotPending, Operation::Sample => {
State::ReadingSample { timestamp } => {
let mut bytes = [0; SAMPLE_BYTES]; let mut bytes = [0; SAMPLE_BYTES];
bytes.copy_from_slice(&self.receive[1..]); bytes.copy_from_slice(&self.receive[1..]);
self.state = State::Ready; self.state = State::Ready { since: now };
Icm42688Event::Sample(sample::decode(&bytes, timestamp)) return Icm42688Event::Sample(sample::decode(&bytes, requested));
} }
} }
} match self.submit_pending(now) {
fn start_configuration_event(&mut self, index: usize) -> Icm42688Event<Bus::StartError> {
match self.start_configuration(index) {
Ok(()) => Icm42688Event::Initializing, Ok(()) => Icm42688Event::Initializing,
Err(error) => Icm42688Event::Fault(error), Err(error) => Icm42688Event::Fault(error),
} }
} }
fn start_configuration(&mut self, index: usize) -> Result<(), Error<Bus::StartError>> {
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) { fn prepare_read(&mut self, register: u8, length: usize) {
self.transmit = [0; TRANSFER_CAPACITY]; self.transmit = [0; TRANSFER_CAPACITY];
self.receive = [0; TRANSFER_CAPACITY]; self.receive = [0; TRANSFER_CAPACITY];
@@ -228,83 +275,337 @@ impl<Bus: SpiDevice> Icm42688<Bus> {
self.transmit[..2].copy_from_slice(&[register, value]); self.transmit[..2].copy_from_slice(&[register, value]);
} }
fn submit(&mut self, length: usize) -> Result<(), Error<Bus::StartError>> { fn queue(&mut self, operation: Operation, now: Timestamp) {
let (bus, transmit, receive) = (&mut self.bus, &self.transmit, &mut self.receive); self.state = State::Pending {
bus.start_transfer(&transmit[..length], &mut receive[..length]) operation,
.map_err(Error::Start) 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<Bus::StartError>> {
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<Error<Bus::StartError>> {
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)] #[cfg(test)]
mod tests { mod tests {
use super::{Error, Icm42688, Icm42688Event, TRANSFER_CAPACITY}; use super::*;
use kaptan_spi::{SpiCompletion, SpiDevice}; use crate::mpu6000::test_bus::{FakeBus, FakeError};
use kaptan_spi::{CompletionError, SpiCompletion};
use kaptan_time::Timestamp; use kaptan_time::Timestamp;
struct Bus { fn t(us: u64) -> Timestamp {
response: [u8; TRANSFER_CAPACITY], Timestamp::from_microseconds(us + 1_000)
} }
impl SpiDevice for Bus { fn start(driver: &mut Icm42688<FakeBus>) {
type StartError = (); driver
.begin_initialization(Timestamp::from_microseconds(0))
fn start_transfer(&mut self, _: &[u8], receive: &mut [u8]) -> Result<(), Self::StartError> { .unwrap();
receive.copy_from_slice(&self.response[..receive.len()]); driver.on_periodic_tick(Timestamp::from_microseconds(999));
Ok(()) assert_eq!(driver.bus.starts, 0);
driver.on_periodic_tick(t(0));
} }
fn complete_transfer(&mut self, _: SpiCompletion) {} fn initialized() -> Icm42688<FakeBus> {
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, t(100)),
Icm42688Event::Initializing
));
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, t(1_200)),
Icm42688Event::Initializing
));
for index in 0..7 {
assert!(matches!(
imu.on_transfer_complete(SpiCompletion::Complete, t(1_300 + index * 100)),
Icm42688Event::Initializing
));
}
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, t(47_000)),
Icm42688Event::Initializing
));
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
} }
#[test] #[test]
fn accepts_a_sample_only_after_identity_and_wake_delays() { fn accepts_a_sample_only_after_elapsed_reset_wake_and_data_settling() {
let mut response = [0; TRANSFER_CAPACITY]; let mut imu = initialized();
response[1] = 0x47; imu.request_sample(t(48_101)).unwrap();
let mut imu = Icm42688::new(Bus { response }); assert_eq!(imu.receive, [0; TRANSFER_CAPACITY]);
imu.begin_initialization().unwrap(); 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!( assert!(matches!(
imu.on_transfer_complete(SpiCompletion::Complete), imu.on_transfer_complete(SpiCompletion::Complete, t(48_300)),
Icm42688Event::Initializing Icm42688Event::Sample(sample) if sample.timestamp == t(48_101)
));
assert!(matches!(imu.advance_initialization(), Ok(())));
assert!(matches!(
imu.on_transfer_complete(SpiCompletion::Complete),
Icm42688Event::Initializing
));
for _ in 0..7 {
assert!(matches!(
imu.on_transfer_complete(SpiCompletion::Complete),
Icm42688Event::Initializing
));
}
assert!(matches!(imu.advance_initialization(), Ok(())));
assert!(matches!(
imu.on_transfer_complete(SpiCompletion::Complete),
Icm42688Event::Initializing
));
assert!(matches!(
imu.on_transfer_complete(SpiCompletion::Complete),
Icm42688Event::Initialized
));
assert!(imu.is_ready());
imu.request_sample(Timestamp::from_microseconds(1)).unwrap();
assert!(matches!(
imu.on_transfer_complete(SpiCompletion::Complete),
Icm42688Event::Sample(_)
)); ));
assert_eq!(imu.receive[1], 0x47);
assert_eq!(
imu.on_transfer_complete(SpiCompletion::Complete, t(48_301)),
Icm42688Event::NotPending
);
} }
#[test] #[test]
fn rejects_a_wrong_identity() { fn rejects_a_wrong_identity() {
let mut imu = Icm42688::new(Bus { let mut imu = Icm42688::new(FakeBus::ready());
response: [0; TRANSFER_CAPACITY], start(&mut imu);
}); let _ = imu.on_transfer_complete(SpiCompletion::Complete, t(0));
imu.begin_initialization().unwrap(); imu.on_periodic_tick(t(1_000));
let _ = imu.on_transfer_complete(SpiCompletion::Complete);
imu.advance_initialization().unwrap();
assert!(matches!( assert!(matches!(
imu.on_transfer_complete(SpiCompletion::Complete), imu.on_transfer_complete(SpiCompletion::Complete, t(1_100)),
Icm42688Event::Fault(Error::UnexpectedIdentity(0)) 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));
}
} }
firmware/libs/kaptan-drivers/src/icm42688/sample.rs
+66 -4
View File
@@ -1,4 +1,9 @@
//! ICM-42688-P burst-sample decoding. //! 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_sensor::imu::ImuSnapshot;
use kaptan_time::Timestamp; 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). // The configured ±500°/s range has 65.5 counts/(°/s).
const fn scale_angular_rate(raw: i16) -> i16 { const fn scale_angular_rate(raw: i16) -> i32 {
((raw as i32 * 17_453) / 65_500) as i16 (raw as i32 * 17_453) / 65_500
} }
// ICM-42688-P temperature is 132.48 counts/°C with a 25°C offset. // ICM-42688-P temperature is 132.48 counts/°C with a 25°C offset.
const fn scale_temperature(raw: i16) -> i16 { 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)] #[cfg(test)]
mod tests { mod tests {
use super::decode; use super::{decode, scale_temperature};
use kaptan_time::Timestamp; use kaptan_time::Timestamp;
#[test] #[test]
@@ -54,4 +59,61 @@ mod tests {
assert_eq!(sample.angular_rate_milliradians_per_second[0], 8_725); assert_eq!(sample.angular_rate_milliradians_per_second[0], 8_725);
assert_eq!(sample.temperature_centidegrees_celsius, 2_500); 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;
}
}
} }
firmware/libs/kaptan-drivers/src/icm42688/service.rs
+106 -16
View File
@@ -36,27 +36,18 @@ where
Clock: MonotonicClock, Clock: MonotonicClock,
Bus: SpiDevice, Bus: SpiDevice,
{ {
/// Submits the asynchronous reset transaction. /// Accepts initialization and starts the power-on register-access delay.
pub fn begin(&mut self) -> Result<(), Error<Bus::StartError>> { pub fn begin(&mut self) -> Result<(), Error<Bus::StartError>> {
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<Bus::StartError> { pub fn on_periodic_tick(&mut self) -> Icm42688ServiceEvent<Bus::StartError> {
if self.imu.is_ready() { service_event(self.imu.on_periodic_tick(self.clock.now()))
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),
}
} }
/// 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<Bus::StartError> { pub fn on_data_ready(&mut self) -> Icm42688ServiceEvent<Bus::StartError> {
if !self.imu.is_ready() { if !self.imu.is_ready() {
return Icm42688ServiceEvent::Idle; return Icm42688ServiceEvent::Idle;
@@ -72,11 +63,110 @@ where
&mut self, &mut self,
completion: SpiCompletion, completion: SpiCompletion,
) -> Icm42688ServiceEvent<Bus::StartError> { ) -> Icm42688ServiceEvent<Bus::StartError> {
match self.imu.on_transfer_complete(completion) { service_event(self.imu.on_transfer_complete(completion, self.clock.now()))
}
}
fn service_event<E>(event: Icm42688Event<E>) -> Icm42688ServiceEvent<E> {
match event {
Icm42688Event::NotPending | Icm42688Event::Initializing => Icm42688ServiceEvent::Idle, Icm42688Event::NotPending | Icm42688Event::Initializing => Icm42688ServiceEvent::Idle,
Icm42688Event::Initialized => Icm42688ServiceEvent::Initialized, Icm42688Event::Initialized => Icm42688ServiceEvent::Initialized,
Icm42688Event::Sample(sample) => Icm42688ServiceEvent::Sample(sample), Icm42688Event::Sample(sample) => Icm42688ServiceEvent::Sample(sample),
Icm42688Event::Fault(error) => Icm42688ServiceEvent::Fault(error), 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<Clock, FakeBus> {
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);
} }
} }
firmware/libs/kaptan-drivers/src/lib.rs
-4
View File
@@ -1,8 +1,4 @@
//! Hardware-independent chip protocol drivers. //! 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] #![no_std]
firmware/libs/kaptan-drivers/src/lidar_lite_v3/error.rs
+3
View File
@@ -29,6 +29,9 @@ pub enum Error<BusError> {
/// The device reported a documented no-signal sentinel distance of zero or one centimetre. /// The device reported a documented no-signal sentinel distance of zero or one centimetre.
#[kaptan_error(id = 5208)] #[kaptan_error(id = 5208)]
NoSignal, 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. /// LIDAR-Lite v3 startup lifecycle failure.
firmware/libs/kaptan-drivers/src/lidar_lite_v3/mod.rs
+286 -49
View File
@@ -4,7 +4,7 @@ mod error;
mod registers; mod registers;
pub mod service; pub mod service;
use kaptan_i2c::{I2cBus, I2cCompletion}; use kaptan_i2c::{CompletionError, I2cBus, I2cCompletion};
use kaptan_sensor::range::RangeSnapshot; use kaptan_sensor::range::RangeSnapshot;
use kaptan_time::Timestamp; use kaptan_time::Timestamp;
@@ -24,6 +24,8 @@ const DISTANCE_LENGTH: usize = 2;
/// ///
/// The extra margin preserves liveness if the optical engine remains busy /// The extra margin preserves liveness if the optical engine remains busy
/// while still accepting a documented slow acquisition. /// 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; const MEASUREMENT_TIMEOUT_MICROSECONDS: u64 = 200_000;
/// Receiver-bias correction selection for one acquisition. /// Receiver-bias correction selection for one acquisition.
@@ -50,6 +52,7 @@ impl AcquisitionMode {
#[derive(Clone, Copy, Debug, Eq, PartialEq)] #[derive(Clone, Copy, Debug, Eq, PartialEq)]
enum State { enum State {
Settling, Settling,
Faulted,
Idle, Idle,
Starting { timestamp: Timestamp }, Starting { timestamp: Timestamp },
Waiting { timestamp: Timestamp }, Waiting { timestamp: Timestamp },
@@ -174,6 +177,7 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
timestamp: Timestamp, timestamp: Timestamp,
mode: AcquisitionMode, mode: AcquisitionMode,
) -> Result<(), Error<Bus::StartError>> { ) -> Result<(), Error<Bus::StartError>> {
self.service_bus(timestamp)?;
if self.state != State::Idle { if self.state != State::Idle {
return Err(Error::NotReady); return Err(Error::NotReady);
} }
@@ -192,15 +196,16 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
/// therefore only records phase completion; this periodic path begins the /// therefore only records phase completion; this periodic path begins the
/// following transaction after the bus has been released. /// following transaction after the bus has been released.
pub fn poll_measurement(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> { pub fn poll_measurement(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
self.service_bus(now)?;
let timestamp = match self.state { let timestamp = match self.state {
State::Waiting { timestamp } State::Waiting { timestamp }
| State::StatusReadPending { timestamp } | State::StatusReadPending { timestamp }
| State::DistanceRegisterPending { timestamp } | State::DistanceRegisterPending { timestamp }
| State::DistanceReadPending { timestamp } => 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) { if self.measurement_timed_out(now) {
self.bus.cancel_transaction(); self.bus.cancel_transaction(now);
self.state = State::Idle; self.state = State::Idle;
return Err(Error::MeasurementTimeout); return Err(Error::MeasurementTimeout);
} }
@@ -208,7 +213,7 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
} }
}; };
if self.measurement_timed_out(now) { if self.measurement_timed_out(now) {
self.bus.cancel_transaction(); self.bus.cancel_transaction(now);
self.state = State::Idle; self.state = State::Idle;
return Err(Error::MeasurementTimeout); return Err(Error::MeasurementTimeout);
} }
@@ -222,17 +227,36 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
} }
/// Advances the state machine from an already-acknowledged I²C terminal event. /// 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( pub fn on_transaction_complete(
&mut self, &mut self,
completion: I2cCompletion, completion: I2cCompletion,
now: Timestamp,
) -> LidarLiteV3Event<Bus::StartError> { ) -> LidarLiteV3Event<Bus::StartError> {
if completion == I2cCompletion::NotPending || !self.has_active_transaction() { if completion == I2cCompletion::NotPending || !self.has_active_transaction() {
return LidarLiteV3Event::NotPending; 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 { match completion {
I2cCompletion::TransferFault => { 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; self.state = State::Idle;
LidarLiteV3Event::Fault(Error::TransferFault) LidarLiteV3Event::Fault(Error::TransferFault)
} }
@@ -242,7 +266,13 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
State::ReadingDistance { .. } => Some(&mut self.receive[..DISTANCE_LENGTH]), State::ReadingDistance { .. } => Some(&mut self.receive[..DISTANCE_LENGTH]),
_ => None, _ => 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() self.advance_after_completion()
} }
I2cCompletion::NotPending => unreachable!(), I2cCompletion::NotPending => unreachable!(),
@@ -254,6 +284,14 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
self.state == State::Idle self.state == State::Idle
} }
fn service_bus(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
if let Err(error) = self.bus.service(now) {
self.state = State::Faulted;
return Err(Error::Completion(error));
}
Ok(())
}
fn has_active_transaction(&self) -> bool { fn has_active_transaction(&self) -> bool {
matches!( matches!(
self.state, self.state,
@@ -276,16 +314,16 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
| State::WritingDistanceRegister { timestamp } | State::WritingDistanceRegister { timestamp }
| State::DistanceReadPending { timestamp } | State::DistanceReadPending { timestamp }
| State::ReadingDistance { timestamp } => timestamp, | State::ReadingDistance { timestamp } => timestamp,
State::Settling | State::Idle => return false, State::Settling | State::Idle | State::Faulted => return false,
}; };
now.as_microseconds() now.as_microseconds()
.saturating_sub(timestamp.as_microseconds()) .saturating_sub(timestamp.as_microseconds())
>= MEASUREMENT_TIMEOUT_MICROSECONDS > MEASUREMENT_TIMEOUT_MICROSECONDS
} }
fn advance_after_completion(&mut self) -> LidarLiteV3Event<Bus::StartError> { fn advance_after_completion(&mut self) -> LidarLiteV3Event<Bus::StartError> {
match self.state { match self.state {
State::Settling => LidarLiteV3Event::NotPending, State::Settling | State::Faulted => LidarLiteV3Event::NotPending,
State::Idle => LidarLiteV3Event::NotPending, State::Idle => LidarLiteV3Event::NotPending,
State::Starting { timestamp } => { State::Starting { timestamp } => {
self.state = State::Waiting { timestamp }; self.state = State::Waiting { timestamp };
@@ -380,7 +418,7 @@ mod tests {
AcquisitionMode, Error, LidarLiteV3, LidarLiteV3Address, LidarLiteV3Event, AcquisitionMode, Error, LidarLiteV3, LidarLiteV3Address, LidarLiteV3Event,
MEASUREMENT_TIMEOUT_MICROSECONDS, StartupError, MEASUREMENT_TIMEOUT_MICROSECONDS, StartupError,
}; };
use kaptan_i2c::{I2cBus, I2cCompletion}; use kaptan_i2c::{CompletionError, I2cBus, I2cCompletion};
use kaptan_sensor::range::RangeSnapshot; use kaptan_sensor::range::RangeSnapshot;
use kaptan_time::Timestamp; use kaptan_time::Timestamp;
@@ -408,7 +446,11 @@ mod tests {
response: [u8; 2], response: [u8; 2],
fail_start: bool, fail_start: bool,
completions: usize, completions: usize,
read_copies: usize,
cancellations: usize, cancellations: usize,
completion_error: Option<CompletionError>,
service_error: Option<CompletionError>,
cancelled_at: Option<Timestamp>,
} }
impl FakeBus { impl FakeBus {
@@ -418,7 +460,11 @@ mod tests {
response: [0; 2], response: [0; 2],
fail_start: false, fail_start: false,
completions: 0, completions: 0,
read_copies: 0,
cancellations: 0, cancellations: 0,
completion_error: None,
service_error: None,
cancelled_at: None,
} }
} }
} }
@@ -454,17 +500,36 @@ mod tests {
Ok(()) 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 if completion == I2cCompletion::Complete
&& let Some(read) = read && let Some(read) = read
{ {
read.copy_from_slice(&self.response[..read.len()]); read.copy_from_slice(&self.response[..read.len()]);
self.read_copies += 1;
} }
self.completions += 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.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()); let mut driver = LidarLiteV3::new(FakeBus::ready());
assert!(!driver.is_ready()); assert!(!driver.is_ready());
assert_eq!( assert_eq!(
driver.on_transaction_complete(I2cCompletion::NotPending), driver.on_transaction_complete(
I2cCompletion::NotPending,
Timestamp::from_microseconds(0)
),
LidarLiteV3Event::NotPending LidarLiteV3Event::NotPending
); );
assert_eq!(driver.bus.completions, 0); assert_eq!(driver.bus.completions, 0);
assert_eq!( assert_eq!(
driver.on_transaction_complete(I2cCompletion::TransferFault), driver.on_transaction_complete(
I2cCompletion::TransferFault,
Timestamp::from_microseconds(0)
),
LidarLiteV3Event::NotPending LidarLiteV3Event::NotPending
); );
assert_eq!(driver.bus.completions, 0); assert_eq!(driver.bus.completions, 0);
@@ -514,7 +585,7 @@ mod tests {
} }
); );
assert_eq!( assert_eq!(
driver.on_transaction_complete(I2cCompletion::Complete), driver.on_transaction_complete(I2cCompletion::Complete, timestamp),
LidarLiteV3Event::MeasurementStarted LidarLiteV3Event::MeasurementStarted
); );
driver.poll_measurement(timestamp).unwrap(); driver.poll_measurement(timestamp).unwrap();
@@ -528,7 +599,7 @@ mod tests {
); );
driver.bus.response = [STATUS_HEALTHY, 0]; driver.bus.response = [STATUS_HEALTHY, 0];
assert_eq!( assert_eq!(
driver.on_transaction_complete(I2cCompletion::Complete), driver.on_transaction_complete(I2cCompletion::Complete, timestamp),
LidarLiteV3Event::ReadingStatus LidarLiteV3Event::ReadingStatus
); );
driver.poll_measurement(timestamp).unwrap(); driver.poll_measurement(timestamp).unwrap();
@@ -540,7 +611,7 @@ mod tests {
} }
); );
assert_eq!( assert_eq!(
driver.on_transaction_complete(I2cCompletion::Complete), driver.on_transaction_complete(I2cCompletion::Complete, timestamp),
LidarLiteV3Event::SelectingDistanceRegister LidarLiteV3Event::SelectingDistanceRegister
); );
driver.poll_measurement(timestamp).unwrap(); driver.poll_measurement(timestamp).unwrap();
@@ -553,7 +624,7 @@ mod tests {
} }
); );
assert_eq!( assert_eq!(
driver.on_transaction_complete(I2cCompletion::Complete), driver.on_transaction_complete(I2cCompletion::Complete, timestamp),
LidarLiteV3Event::ReadingMeasurement LidarLiteV3Event::ReadingMeasurement
); );
driver.bus.response = 291_u16.to_be_bytes(); driver.bus.response = 291_u16.to_be_bytes();
@@ -566,7 +637,7 @@ mod tests {
} }
); );
assert_eq!( assert_eq!(
driver.on_transaction_complete(I2cCompletion::Complete), driver.on_transaction_complete(I2cCompletion::Complete, timestamp),
LidarLiteV3Event::Measurement(RangeSnapshot::new(timestamp, 2_910)) LidarLiteV3Event::Measurement(RangeSnapshot::new(timestamp, 2_910))
); );
assert!(driver.is_ready()); assert!(driver.is_ready());
@@ -581,17 +652,20 @@ mod tests {
AcquisitionMode::WithoutReceiverBiasCorrection, AcquisitionMode::WithoutReceiverBiasCorrection,
) )
.unwrap(); .unwrap();
let _ = driver.on_transaction_complete(I2cCompletion::Complete); let _ = driver
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
driver driver
.poll_measurement(Timestamp::from_microseconds(0)) .poll_measurement(Timestamp::from_microseconds(0))
.unwrap(); .unwrap();
driver.bus.response = [STATUS_HEALTHY | 1, 0]; 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 driver
.poll_measurement(Timestamp::from_microseconds(0)) .poll_measurement(Timestamp::from_microseconds(0))
.unwrap(); .unwrap();
assert_eq!( assert_eq!(
driver.on_transaction_complete(I2cCompletion::Complete), driver
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
LidarLiteV3Event::MeasurementBusy LidarLiteV3Event::MeasurementBusy
); );
assert!(!driver.is_ready()); assert!(!driver.is_ready());
@@ -611,11 +685,12 @@ mod tests {
AcquisitionMode::WithReceiverBiasCorrection, AcquisitionMode::WithReceiverBiasCorrection,
) )
.unwrap(); .unwrap();
let _ = driver.on_transaction_complete(I2cCompletion::Complete); let _ = driver
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
assert_eq!( assert_eq!(
driver.poll_measurement(Timestamp::from_microseconds( driver.poll_measurement(Timestamp::from_microseconds(
MEASUREMENT_TIMEOUT_MICROSECONDS, MEASUREMENT_TIMEOUT_MICROSECONDS + 1,
)), )),
Err(Error::MeasurementTimeout) Err(Error::MeasurementTimeout)
); );
@@ -632,17 +707,20 @@ mod tests {
AcquisitionMode::WithReceiverBiasCorrection, AcquisitionMode::WithReceiverBiasCorrection,
) )
.unwrap(); .unwrap();
let _ = driver.on_transaction_complete(I2cCompletion::Complete); let _ = driver
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
driver driver
.poll_measurement(Timestamp::from_microseconds(0)) .poll_measurement(Timestamp::from_microseconds(0))
.unwrap(); .unwrap();
driver.bus.response = [STATUS_HEALTHY | STATUS_PROCESS_ERROR, 0]; 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 driver
.poll_measurement(Timestamp::from_microseconds(0)) .poll_measurement(Timestamp::from_microseconds(0))
.unwrap(); .unwrap();
assert_eq!( assert_eq!(
driver.on_transaction_complete(I2cCompletion::Complete), driver
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
LidarLiteV3Event::Fault(Error::ProcessError) LidarLiteV3Event::Fault(Error::ProcessError)
); );
@@ -652,17 +730,20 @@ mod tests {
AcquisitionMode::WithReceiverBiasCorrection, AcquisitionMode::WithReceiverBiasCorrection,
) )
.unwrap(); .unwrap();
let _ = driver.on_transaction_complete(I2cCompletion::Complete); let _ = driver
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
driver driver
.poll_measurement(Timestamp::from_microseconds(0)) .poll_measurement(Timestamp::from_microseconds(0))
.unwrap(); .unwrap();
driver.bus.response = [0, 0]; driver.bus.response = [0, 0];
let _ = driver.on_transaction_complete(I2cCompletion::Complete); let _ = driver
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
driver driver
.poll_measurement(Timestamp::from_microseconds(0)) .poll_measurement(Timestamp::from_microseconds(0))
.unwrap(); .unwrap();
assert_eq!( assert_eq!(
driver.on_transaction_complete(I2cCompletion::Complete), driver
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
LidarLiteV3Event::Fault(Error::Unhealthy) LidarLiteV3Event::Fault(Error::Unhealthy)
); );
@@ -672,17 +753,20 @@ mod tests {
AcquisitionMode::WithReceiverBiasCorrection, AcquisitionMode::WithReceiverBiasCorrection,
) )
.unwrap(); .unwrap();
let _ = driver.on_transaction_complete(I2cCompletion::Complete); let _ = driver
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
driver driver
.poll_measurement(Timestamp::from_microseconds(0)) .poll_measurement(Timestamp::from_microseconds(0))
.unwrap(); .unwrap();
driver.bus.response = [STATUS_HEALTHY | STATUS_INVALID_SIGNAL, 0]; 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 driver
.poll_measurement(Timestamp::from_microseconds(0)) .poll_measurement(Timestamp::from_microseconds(0))
.unwrap(); .unwrap();
assert_eq!( assert_eq!(
driver.on_transaction_complete(I2cCompletion::Complete), driver
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
LidarLiteV3Event::Fault(Error::InvalidSignal) LidarLiteV3Event::Fault(Error::InvalidSignal)
); );
} }
@@ -696,20 +780,25 @@ mod tests {
AcquisitionMode::WithReceiverBiasCorrection, AcquisitionMode::WithReceiverBiasCorrection,
) )
.unwrap(); .unwrap();
let _ = driver.on_transaction_complete(I2cCompletion::Complete); let _ = driver
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
driver driver
.poll_measurement(Timestamp::from_microseconds(0)) .poll_measurement(Timestamp::from_microseconds(0))
.unwrap(); .unwrap();
driver.bus.response = [STATUS_HEALTHY, 0]; 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.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.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(); driver.bus.response = 1_u16.to_be_bytes();
let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
assert_eq!( assert_eq!(
driver.on_transaction_complete(I2cCompletion::Complete), driver
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
LidarLiteV3Event::Fault(Error::NoSignal) LidarLiteV3Event::Fault(Error::NoSignal)
); );
@@ -719,20 +808,25 @@ mod tests {
AcquisitionMode::WithReceiverBiasCorrection, AcquisitionMode::WithReceiverBiasCorrection,
) )
.unwrap(); .unwrap();
let _ = driver.on_transaction_complete(I2cCompletion::Complete); let _ = driver
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
driver driver
.poll_measurement(Timestamp::from_microseconds(0)) .poll_measurement(Timestamp::from_microseconds(0))
.unwrap(); .unwrap();
driver.bus.response = [STATUS_HEALTHY, 0]; 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.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.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(); driver.bus.response = 0_u16.to_be_bytes();
let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
assert_eq!( assert_eq!(
driver.on_transaction_complete(I2cCompletion::Complete), driver
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
LidarLiteV3Event::Fault(Error::NoSignal) LidarLiteV3Event::Fault(Error::NoSignal)
); );
@@ -742,20 +836,25 @@ mod tests {
AcquisitionMode::WithReceiverBiasCorrection, AcquisitionMode::WithReceiverBiasCorrection,
) )
.unwrap(); .unwrap();
let _ = driver.on_transaction_complete(I2cCompletion::Complete); let _ = driver
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
driver driver
.poll_measurement(Timestamp::from_microseconds(0)) .poll_measurement(Timestamp::from_microseconds(0))
.unwrap(); .unwrap();
driver.bus.response = [STATUS_HEALTHY, 0]; 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.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.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(); driver.bus.response = u16::MAX.to_be_bytes();
let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
assert_eq!( assert_eq!(
driver.on_transaction_complete(I2cCompletion::Complete), driver
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
LidarLiteV3Event::Measurement(RangeSnapshot::new( LidarLiteV3Event::Measurement(RangeSnapshot::new(
Timestamp::from_microseconds(0), Timestamp::from_microseconds(0),
655_350, 655_350,
@@ -773,9 +872,147 @@ mod tests {
) )
.unwrap(); .unwrap();
assert_eq!( assert_eq!(
driver.on_transaction_complete(I2cCompletion::TransferFault), driver.on_transaction_complete(
I2cCompletion::TransferFault,
Timestamp::from_microseconds(0)
),
LidarLiteV3Event::Fault(Error::TransferFault) LidarLiteV3Event::Fault(Error::TransferFault)
); );
assert!(driver.is_ready()); 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());
}
}
}
} }
firmware/libs/kaptan-drivers/src/lidar_lite_v3/service.rs
+137 -22
View File
@@ -3,16 +3,16 @@
use super::{AcquisitionMode, Error, LidarLiteV3, LidarLiteV3Event, StartupError}; use super::{AcquisitionMode, Error, LidarLiteV3, LidarLiteV3Event, StartupError};
use kaptan_i2c::{I2cBus, I2cCompletion}; use kaptan_i2c::{I2cBus, I2cCompletion};
use kaptan_sensor::range::RangeSnapshot; use kaptan_sensor::range::RangeSnapshot;
use kaptan_time::MonotonicClock; use kaptan_time::{MonotonicClock, Timestamp};
/// One observable result from a LIDAR-Lite v3 acquisition service. /// One observable result from a LIDAR-Lite v3 acquisition service.
#[derive(Debug)] #[derive(Debug)]
pub enum LidarLiteV3ServiceEvent<BusError> { pub enum LidarLiteV3ServiceEvent<BusError> {
/// No observation or terminal fault was produced. /// No observation or reportable error was produced.
Idle, Idle,
/// One coherent range observation is ready for publication. /// One coherent range observation is ready for publication.
Measurement(RangeSnapshot), Measurement(RangeSnapshot),
/// The LIDAR-Lite v3 path entered a terminal fault state. /// Acquisition or controller failed.
Fault(Error<BusError>), Fault(Error<BusError>),
} }
@@ -55,22 +55,21 @@ impl<Clock: MonotonicClock, Bus: I2cBus> LidarLiteV3Service<Clock, Bus> {
&mut self, &mut self,
completion: I2cCompletion, completion: I2cCompletion,
) -> LidarLiteV3ServiceEvent<Bus::StartError> { ) -> LidarLiteV3ServiceEvent<Bus::StartError> {
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. /// Acknowledges a 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.
pub fn on_controller_interrupt<F>( pub fn on_controller_interrupt<F>(
&mut self, &mut self,
acknowledge: F, acknowledge: F,
) -> LidarLiteV3ServiceEvent<Bus::StartError> ) -> LidarLiteV3ServiceEvent<Bus::StartError>
where where
F: FnOnce(&mut Bus) -> I2cCompletion, F: FnOnce(&mut Bus, Timestamp) -> I2cCompletion,
{ {
let completion = self.lidar.acknowledge_with(acknowledge); let now = self.clock.now();
self.on_transaction_complete(completion) 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<BusError>(event: LidarLiteV3Event<BusError>) -> LidarLiteV3Serv
mod tests { mod tests {
use super::{LidarLiteV3Service, LidarLiteV3ServiceEvent, service_event}; use super::{LidarLiteV3Service, LidarLiteV3ServiceEvent, service_event};
use crate::lidar_lite_v3::{AcquisitionMode, Error, LidarLiteV3, LidarLiteV3Event}; 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}; use kaptan_time::{MonotonicClock, Timestamp};
struct Clock; struct Clock(u64);
impl MonotonicClock for Clock { impl MonotonicClock for Clock {
fn now(&mut self) -> Timestamp { fn now(&mut self) -> Timestamp {
Timestamp::from_microseconds(123) Timestamp::from_microseconds(self.0)
} }
} }
#[derive(Default)]
struct Bus { struct Bus {
response: [u8; 2], response: [u8; 2],
cancelled_at: Option<Timestamp>,
read_copies: usize,
} }
impl I2cBus for Bus { impl I2cBus for Bus {
@@ -115,22 +117,37 @@ mod tests {
Ok(()) 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 if completion == I2cCompletion::Complete
&& let Some(read) = read && let Some(read) = read
{ {
read.copy_from_slice(&self.response[..read.len()]); 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] #[test]
fn produces_a_measurement_after_stop_separated_i2c_transactions() { fn produces_a_measurement_after_stop_separated_i2c_transactions() {
let mut service = LidarLiteV3Service::new( let mut service = LidarLiteV3Service::new(
Clock, Clock(123),
LidarLiteV3::new(Bus { response: [0; 2] }), LidarLiteV3::new(Bus::default()),
AcquisitionMode::WithReceiverBiasCorrection, AcquisitionMode::WithReceiverBiasCorrection,
); );
service.finish_startup().unwrap(); service.finish_startup().unwrap();
@@ -148,7 +165,7 @@ mod tests {
LidarLiteV3ServiceEvent::Idle LidarLiteV3ServiceEvent::Idle
)); ));
assert!(matches!( assert!(matches!(
service.on_controller_interrupt(|bus| { service.on_controller_interrupt(|bus, _| {
bus.response = [1 << 5, 0]; bus.response = [1 << 5, 0];
I2cCompletion::Complete I2cCompletion::Complete
}), }),
@@ -159,7 +176,7 @@ mod tests {
LidarLiteV3ServiceEvent::Idle LidarLiteV3ServiceEvent::Idle
)); ));
assert!(matches!( assert!(matches!(
service.on_controller_interrupt(|_| I2cCompletion::Complete), service.on_controller_interrupt(|_, _| I2cCompletion::Complete),
LidarLiteV3ServiceEvent::Idle LidarLiteV3ServiceEvent::Idle
)); ));
assert!(matches!( assert!(matches!(
@@ -167,7 +184,7 @@ mod tests {
LidarLiteV3ServiceEvent::Idle LidarLiteV3ServiceEvent::Idle
)); ));
assert!(matches!( assert!(matches!(
service.on_controller_interrupt(|bus| { service.on_controller_interrupt(|bus, _| {
bus.response = [0x01, 0x23]; bus.response = [0x01, 0x23];
I2cCompletion::Complete I2cCompletion::Complete
}), }),
@@ -178,7 +195,7 @@ mod tests {
LidarLiteV3ServiceEvent::Idle LidarLiteV3ServiceEvent::Idle
)); ));
assert!(matches!( assert!(matches!(
service.on_controller_interrupt(|_| I2cCompletion::Complete), service.on_controller_interrupt(|_, _| I2cCompletion::Complete),
LidarLiteV3ServiceEvent::Measurement(sample) LidarLiteV3ServiceEvent::Measurement(sample)
if sample.timestamp == Timestamp::from_microseconds(123) if sample.timestamp == Timestamp::from_microseconds(123)
&& sample.distance_millimetres == 2_910 && sample.distance_millimetres == 2_910
@@ -200,4 +217,102 @@ mod tests {
LidarLiteV3ServiceEvent::Fault(Error::Unhealthy) LidarLiteV3ServiceEvent::Fault(Error::Unhealthy)
)); ));
} }
fn distance_pending() -> LidarLiteV3Service<Clock, Bus> {
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<Clock, Bus>| {
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);
}
}
} }
firmware/libs/kaptan-drivers/src/mpu6000/error.rs
+12
View File
@@ -18,4 +18,16 @@ pub enum Error<BusError> {
/// The selected SPI route reported a terminal transfer fault. /// The selected SPI route reported a terminal transfer fault.
#[kaptan_error(id = 5004)] #[kaptan_error(id = 5004)]
TransferFault, 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,
} }
firmware/libs/kaptan-drivers/src/mpu6000/mod.rs
+489 -163
View File
@@ -6,8 +6,11 @@ mod registers;
mod sample; mod sample;
pub mod service; pub mod service;
#[cfg(test)]
pub(crate) mod test_bus;
use kaptan_sensor::imu::ImuSnapshot; use kaptan_sensor::imu::ImuSnapshot;
use kaptan_spi::{SpiCompletion, SpiDevice}; use kaptan_spi::{CompletionError, SpiCompletion, SpiDevice};
use kaptan_time::Timestamp; use kaptan_time::Timestamp;
pub use config::{AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000Config}; pub use config::{AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000Config};
@@ -22,6 +25,21 @@ use registers::{
}; };
const TRANSFER_CAPACITY: usize = SAMPLE_BYTES + 1; 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] = [ const CONFIGURATION: [(u8, u8); 9] = [
(PWR_MGMT_1, CLOCK_SOURCE_PLL_X_GYRO), (PWR_MGMT_1, CLOCK_SOURCE_PLL_X_GYRO),
(USER_CTRL, I2C_INTERFACE_DISABLE), (USER_CTRL, I2C_INTERFACE_DISABLE),
@@ -37,18 +55,41 @@ const CONFIGURATION: [(u8, u8); 9] = [
#[derive(Clone, Copy, Debug, Eq, PartialEq)] #[derive(Clone, Copy, Debug, Eq, PartialEq)]
enum State { enum State {
Dormant, Dormant,
Resetting, Pending {
WaitingForReset, operation: Operation,
ReadingIdentity, since: Timestamp,
WritingConfiguration(usize), },
Ready, Active {
ReadingSample { timestamp: Timestamp }, 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)] #[derive(Debug, Eq, PartialEq)]
pub enum Mpu6000Event<BusError> { pub enum Mpu6000Event<BusError> {
/// The selected SPI interrupt had no terminal transfer event. /// No sample, initialization completion or terminal fault was produced.
NotPending, NotPending,
/// One initialization transfer completed; initialization continues. /// One initialization transfer completed; initialization continues.
Initializing, Initializing,
@@ -62,8 +103,8 @@ pub enum Mpu6000Event<BusError> {
/// Configured MPU6000 driver. /// Configured MPU6000 driver.
/// ///
/// The driver owns fixed transfer buffers and advances only from external /// The driver owns ordinary movable staging buffers and advances only from
/// interrupt events. It never waits for SPI status flags. /// external events. The bus copies these bytes; hardware never addresses them.
pub struct Mpu6000<Bus> { pub struct Mpu6000<Bus> {
bus: Bus, bus: Bus,
config: Mpu6000Config, config: Mpu6000Config,
@@ -86,109 +127,144 @@ impl<Bus> Mpu6000<Bus> {
} }
impl<Bus: SpiDevice> Mpu6000<Bus> { impl<Bus: SpiDevice> Mpu6000<Bus> {
/// Starts a device reset without waiting for SPI. /// Accepts initialization once. Busy admission is retried by periodic
/// /// service. All supplied instants must use one non-regressing boot clock.
/// The caller must subsequently call [`Self::advance_initialization`] only /// Power must be stable by this call. Register access waits another 100 ms.
/// after the platform-selected reset-settling interval has elapsed. pub fn begin_initialization(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
pub fn begin_initialization(&mut self) -> Result<(), Error<Bus::StartError>> {
if self.state != State::Dormant { if self.state != State::Dormant {
return Err(Error::NotReady); return Err(Error::NotReady);
} }
self.wait(Operation::Reset, now, RESET_SETTLE_US);
self.prepare_write(PWR_MGMT_1, DEVICE_RESET);
self.submit(2)?;
self.state = State::Resetting;
Ok(()) Ok(())
} }
/// Starts identity verification after the required reset-settling interval. /// Services elapsed settling, admission and acquisition deadlines. This must
/// /// be called periodically throughout the driver's lifetime, including while
/// This operation only submits the next asynchronous SPI transfer; it /// a transfer is active. It never starts an unsolicited sample read.
/// never waits for a peripheral or sensor status flag. pub fn on_periodic_tick(&mut self, now: Timestamp) -> Mpu6000Event<Bus::StartError> {
pub fn advance_initialization(&mut self) -> Result<(), Error<Bus::StartError>> { if let Some(error) = self.check_deadline(now) {
if self.state != State::WaitingForReset { return Mpu6000Event::Fault(error);
return Ok(()); }
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); /// Accepts a burst-read request. Busy admission remains pending for periodic
if let Err(error) = self.submit(2) { /// retry. The eventual accepted submission time becomes the snapshot
self.state = State::Dormant; /// timestamp, not the ISR time or a physical sampling epoch.
pub fn request_sample(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
if let Some(error) = self.check_deadline(now) {
return Err(error); return Err(error);
} }
self.state = State::ReadingIdentity; if !self.is_ready() {
Ok(())
}
/// Starts one fourteen-byte sensor read when initialization is complete.
pub fn request_sample(&mut self, timestamp: Timestamp) -> Result<(), Error<Bus::StartError>> {
if self.state != State::Ready {
return Err(Error::NotReady); return Err(Error::NotReady);
} }
self.queue(Operation::Sample, now);
self.prepare_read(ACCEL_XOUT_H, TRANSFER_CAPACITY); self.submit_pending(now)
self.submit(TRANSFER_CAPACITY)?;
self.state = State::ReadingSample { timestamp };
Ok(())
} }
/// Advances the state machine from an already-acknowledged DMA terminal event. /// Advances the state machine from an already-acknowledged DMA terminal event.
pub fn on_transfer_complete( pub fn on_transfer_complete(
&mut self, &mut self,
completion: SpiCompletion, completion: SpiCompletion,
now: Timestamp,
) -> Mpu6000Event<Bus::StartError> { ) -> Mpu6000Event<Bus::StartError> {
self.bus.complete_transfer(completion); if completion == SpiCompletion::NotPending {
match completion { return Mpu6000Event::NotPending;
SpiCompletion::NotPending => Mpu6000Event::NotPending, }
SpiCompletion::TransferFault => { if let Some(error) = self.check_deadline(now) {
self.state = State::Dormant; 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) 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. /// Returns whether an IMU data-ready event can start a sensor transfer.
pub fn is_ready(&self) -> bool { pub fn is_ready(&self) -> bool {
self.state == State::Ready matches!(self.state, State::Ready { .. })
} }
fn advance_after_completion(&mut self) -> Mpu6000Event<Bus::StartError> { fn advance_after_completion(
match self.state { &mut self,
State::Dormant => Mpu6000Event::NotPending, operation: Operation,
State::Resetting => { requested: Timestamp,
self.state = State::WaitingForReset; now: Timestamp,
Mpu6000Event::Initializing ) -> Mpu6000Event<Bus::StartError> {
} match operation {
State::WaitingForReset => Mpu6000Event::NotPending, Operation::Reset => self.wait(Operation::ResetSignalPaths, now, RESET_SETTLE_US),
State::ReadingIdentity => { Operation::ResetSignalPaths => self.wait(Operation::Identity, now, RESET_SETTLE_US),
Operation::Identity => {
let identity = self.receive[1]; let identity = self.receive[1];
if identity != EXPECTED_WHO_AM_I { if identity != EXPECTED_WHO_AM_I {
self.state = State::Dormant; self.state = State::Faulted;
return Mpu6000Event::Fault(Error::UnexpectedIdentity(identity)); 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; let next = index + 1;
if next == CONFIGURATION.len() { if index == 0 {
self.state = State::Ready; self.wait(Operation::Configure(next), now, WAKE_SETTLE_US);
Mpu6000Event::Initialized } 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 { } else {
self.start_configuration(next) self.queue(Operation::Configure(next), now);
} }
} }
State::Ready => Mpu6000Event::NotPending, Operation::Sample => {
State::ReadingSample { timestamp } => {
let mut bytes = [0; SAMPLE_BYTES]; let mut bytes = [0; SAMPLE_BYTES];
bytes.copy_from_slice(&self.receive[1..]); bytes.copy_from_slice(&self.receive[1..]);
self.state = State::Ready; self.state = State::Ready { since: now };
Mpu6000Event::Sample(sample::decode(&bytes, timestamp, self.config)) 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<Bus::StartError> { fn prepare_configuration(&mut self, index: usize) {
let (register, value) = CONFIGURATION[index]; let (register, value) = CONFIGURATION[index];
let value = match register { let value = match register {
CONFIG => self.config.digital_low_pass_filter.register_value(), CONFIG => self.config.digital_low_pass_filter.register_value(),
@@ -198,16 +274,6 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
_ => value, _ => value,
}; };
self.prepare_write(register, 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) { fn prepare_read(&mut self, register: u8, length: usize) {
@@ -223,21 +289,106 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
self.transmit[..2].copy_from_slice(&[register, value]); self.transmit[..2].copy_from_slice(&[register, value]);
} }
fn submit(&mut self, length: usize) -> Result<(), Error<Bus::StartError>> { fn queue(&mut self, operation: Operation, now: Timestamp) {
let (bus, transmit, receive) = (&mut self.bus, &self.transmit, &mut self.receive); self.state = State::Pending {
bus.start_transfer(&transmit[..length], &mut receive[..length]) operation,
.map_err(Error::Start) 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<Bus::StartError>> {
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<Error<Bus::StartError>> {
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)] #[cfg(test)]
mod tests { mod tests {
use super::{ use super::test_bus::{FakeBus, FakeError};
AccelerometerRange, DigitalLowPassFilter, Error, GyroscopeRange, Mpu6000, Mpu6000Config, use super::*;
Mpu6000Event, TRANSFER_CAPACITY,
};
use kaptan_spi::{SpiCompletion, SpiDevice};
use kaptan_time::Timestamp;
const TEST_CONFIG: Mpu6000Config = Mpu6000Config { const TEST_CONFIG: Mpu6000Config = Mpu6000Config {
accelerometer: AccelerometerRange::PlusMinus4G, accelerometer: AccelerometerRange::PlusMinus4G,
@@ -246,79 +397,87 @@ mod tests {
sample_rate_divider: 9, sample_rate_divider: 9,
}; };
#[derive(Debug, Eq, PartialEq)] fn t(us: u64) -> Timestamp {
enum FakeError { Timestamp::from_microseconds(us + 100_000)
Start,
} }
struct FakeBus { fn start(driver: &mut Mpu6000<FakeBus>) {
receive: [u8; TRANSFER_CAPACITY], driver
length: usize, .begin_initialization(Timestamp::from_microseconds(0))
fail_start: bool, .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 initialized() -> Mpu6000<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() {
let mut bus = FakeBus::ready(); let mut bus = FakeBus::ready();
bus.receive[1] = 0x68; bus.response[1] = 0x68;
let mut driver = Mpu6000::new(bus, TEST_CONFIG); let mut driver = Mpu6000::new(bus, TEST_CONFIG);
start(&mut driver);
driver.begin_initialization().unwrap(); driver.on_transfer_complete(SpiCompletion::Complete, t(100));
assert_eq!( driver.on_periodic_tick(t(100_100));
driver.on_transfer_complete(SpiCompletion::Complete), assert_eq!(&driver.bus.transmitted[..2], &[0x68, 7]);
Mpu6000Event::Initializing driver.on_transfer_complete(SpiCompletion::Complete, t(100_200));
); driver.on_periodic_tick(t(200_200));
driver.advance_initialization().unwrap(); driver.on_transfer_complete(SpiCompletion::Complete, t(200_300));
for _ in 0..9 { driver.on_transfer_complete(SpiCompletion::Complete, t(200_400));
assert_eq!( driver.on_periodic_tick(t(300_400));
driver.on_transfer_complete(SpiCompletion::Complete), for index in 1..9 {
Mpu6000Event::Initializing driver.on_transfer_complete(SpiCompletion::Complete, t(300_400 + index * 100));
);
} }
assert_eq!( assert_eq!(
driver.on_transfer_complete(SpiCompletion::Complete), driver.on_periodic_tick(t(311_200)),
Mpu6000Event::Initialized Mpu6000Event::Initialized
); );
assert!(driver.is_ready()); 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] #[test]
fn rejects_an_unexpected_identity() { fn rejects_an_unexpected_identity() {
let mut bus = FakeBus::ready(); let mut bus = FakeBus::ready();
bus.receive[1] = 0x70; bus.response[1] = 0x70;
let mut driver = Mpu6000::new(bus, TEST_CONFIG); let mut driver = Mpu6000::new(bus, TEST_CONFIG);
driver.begin_initialization().unwrap(); driver.queue(Operation::Identity, t(0));
let _ = driver.on_transfer_complete(SpiCompletion::Complete); driver.submit_pending(t(0)).unwrap();
driver.advance_initialization().unwrap();
assert!(matches!( assert!(matches!(
driver.on_transfer_complete(SpiCompletion::Complete), driver.on_transfer_complete(SpiCompletion::Complete, t(100)),
Mpu6000Event::Fault(Error::UnexpectedIdentity(0x70)) Mpu6000Event::Fault(Error::UnexpectedIdentity(0x70))
)); ));
} }
@@ -330,22 +489,23 @@ mod tests {
0x00, 0x00, // 36.53 C 0x00, 0x00, // 36.53 C
0x40, 0x10, 0xBF, 0xF0, 0x00, 0x00, // gyro: 500, -500, 0 dps 0x40, 0x10, 0xBF, 0xF0, 0x00, 0x00, // gyro: 500, -500, 0 dps
]; ];
let mut bus = FakeBus::ready(); let mut driver = initialized();
bus.receive[1] = 0x68; driver.bus.response[1..].copy_from_slice(&burst);
let mut driver = Mpu6000::new(bus, TEST_CONFIG); driver.request_sample(t(312_000)).unwrap();
assert_eq!(driver.receive, [0; TRANSFER_CAPACITY]);
driver.begin_initialization().unwrap(); assert_eq!(
let _ = driver.on_transfer_complete(SpiCompletion::Complete); driver.on_transfer_complete(SpiCompletion::NotPending, t(312_100)),
driver.advance_initialization().unwrap(); Mpu6000Event::NotPending
for _ in 0..9 { );
let _ = driver.on_transfer_complete(SpiCompletion::Complete); assert_eq!(driver.receive, [0; TRANSFER_CAPACITY]);
} // Overwrite staging bytes and move the whole driver while work is pending.
let _ = driver.on_transfer_complete(SpiCompletion::Complete); // The fake bus, like the DMA route, owns the pending response itself.
driver.bus.receive[1..].copy_from_slice(&burst); driver.transmit.fill(0xff);
driver driver.bus.response.fill(0xff);
.request_sample(Timestamp::from_microseconds(42)) let mut driver =
.unwrap(); core::mem::replace(&mut driver, Mpu6000::new(FakeBus::ready(), TEST_CONFIG));
let Mpu6000Event::Sample(sample) = driver.on_transfer_complete(SpiCompletion::Complete) let Mpu6000Event::Sample(sample) =
driver.on_transfer_complete(SpiCompletion::Complete, t(312_200))
else { else {
panic!("sample transfer must complete"); panic!("sample transfer must complete");
}; };
@@ -356,18 +516,184 @@ mod tests {
[8_726, -8_726, 0] [8_726, -8_726, 0]
); );
assert_eq!(sample.temperature_centidegrees_celsius, 3_653); 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] #[test]
fn reports_a_route_transfer_fault() { fn reports_a_route_transfer_fault() {
let mut bus = FakeBus::ready(); let mut bus = FakeBus::ready();
bus.receive[1] = 0x68; bus.response[1] = 0x68;
let mut driver = Mpu6000::new(bus, TEST_CONFIG); let mut driver = Mpu6000::new(bus, TEST_CONFIG);
driver.begin_initialization().unwrap(); start(&mut driver);
assert!(matches!( assert!(matches!(
driver.on_transfer_complete(SpiCompletion::TransferFault), driver.on_transfer_complete(SpiCompletion::TransferFault, t(100)),
Mpu6000Event::Fault(Error::TransferFault) 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));
}
} }
firmware/libs/kaptan-drivers/src/mpu6000/sample.rs
+96 -2
View File
@@ -1,4 +1,10 @@
//! MPU6000 burst-sample decoding. //! 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_sensor::imu::ImuSnapshot;
use kaptan_time::Timestamp; 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 ((raw as i32 * 1_000) / config.accelerometer.counts_per_g()) as i16
} }
const fn scale_angular_rate(raw: i16, config: Mpu6000Config) -> i16 { const fn scale_angular_rate(raw: i16, config: Mpu6000Config) -> i32 {
((raw as i32 * 17_453) / config.gyroscope.milliradians_per_second_divisor()) as i16 (raw as i32 * 17_453) / config.gyroscope.milliradians_per_second_divisor()
} }
const fn scale_temperature(raw: i16) -> i16 { const fn scale_temperature(raw: i16) -> i16 {
((raw as i32 * 100) / 340 + 3_653) as 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);
}
}
}
firmware/libs/kaptan-drivers/src/mpu6000/service.rs
+59 -46
View File
@@ -39,28 +39,18 @@ where
Clock: MonotonicClock, Clock: MonotonicClock,
Bus: SpiDevice, 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<Bus::StartError>> { pub fn begin(&mut self) -> Result<(), Error<Bus::StartError>> {
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<Bus::StartError> { pub fn on_periodic_tick(&mut self) -> Mpu6000ServiceEvent<Bus::StartError> {
if self.imu.is_ready() { service_event(self.imu.on_periodic_tick(self.clock.now()))
return match self.imu.request_sample(self.clock.now()) {
Ok(()) | Err(Error::NotReady) => Mpu6000ServiceEvent::Idle,
Err(error) => Mpu6000ServiceEvent::Fault(error),
};
} }
let _ = self.clock.now(); /// Handles one coalesced data-ready notification. Time is captured here in
match self.imu.advance_initialization() { /// the worker, at read submission, not at IRQ entry or physical sampling.
Ok(()) => Mpu6000ServiceEvent::Idle,
Err(error) => Mpu6000ServiceEvent::Fault(error),
}
}
/// Handles one coalesced data-ready event.
pub fn on_data_ready(&mut self) -> Mpu6000ServiceEvent<Bus::StartError> { pub fn on_data_ready(&mut self) -> Mpu6000ServiceEvent<Bus::StartError> {
if !self.imu.is_ready() { if !self.imu.is_ready() {
return Mpu6000ServiceEvent::Idle; return Mpu6000ServiceEvent::Idle;
@@ -77,22 +67,27 @@ where
&mut self, &mut self,
completion: SpiCompletion, completion: SpiCompletion,
) -> Mpu6000ServiceEvent<Bus::StartError> { ) -> Mpu6000ServiceEvent<Bus::StartError> {
match self.imu.on_transfer_complete(completion) { service_event(self.imu.on_transfer_complete(completion, self.clock.now()))
}
}
fn service_event<E>(event: Mpu6000Event<E>) -> Mpu6000ServiceEvent<E> {
match event {
Mpu6000Event::NotPending | Mpu6000Event::Initializing => Mpu6000ServiceEvent::Idle, Mpu6000Event::NotPending | Mpu6000Event::Initializing => Mpu6000ServiceEvent::Idle,
Mpu6000Event::Initialized => Mpu6000ServiceEvent::Initialized, Mpu6000Event::Initialized => Mpu6000ServiceEvent::Initialized,
Mpu6000Event::Sample(sample) => Mpu6000ServiceEvent::Sample(sample), Mpu6000Event::Sample(sample) => Mpu6000ServiceEvent::Sample(sample),
Mpu6000Event::Fault(error) => Mpu6000ServiceEvent::Fault(error), Mpu6000Event::Fault(error) => Mpu6000ServiceEvent::Fault(error),
} }
}
} }
#[cfg(test)] #[cfg(test)]
mod tests { mod tests {
use super::{Mpu6000Service, Mpu6000ServiceEvent}; use super::{Mpu6000Service, Mpu6000ServiceEvent};
use crate::mpu6000::test_bus::FakeBus;
use crate::mpu6000::{ use crate::mpu6000::{
AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000, Mpu6000Config, AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000, Mpu6000Config,
}; };
use kaptan_spi::{SpiCompletion, SpiDevice}; use kaptan_spi::SpiCompletion;
use kaptan_time::{MonotonicClock, Timestamp}; use kaptan_time::{MonotonicClock, Timestamp};
const TEST_CONFIG: Mpu6000Config = Mpu6000Config { const TEST_CONFIG: Mpu6000Config = Mpu6000Config {
@@ -102,62 +97,80 @@ mod tests {
sample_rate_divider: 9, sample_rate_divider: 9,
}; };
struct Clock; struct Clock(u64);
impl MonotonicClock for Clock { impl MonotonicClock for Clock {
fn now(&mut self) -> Timestamp { 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] #[test]
fn accepts_a_sample_only_after_initialization() { fn accepts_a_sample_only_after_initialization() {
let mut response = [0; 15]; let mut bus = FakeBus::ready();
response[1] = 0x68; bus.response[1] = 0x68;
let mut service = Mpu6000Service::new(Clock, Mpu6000::new(Bus { response }, TEST_CONFIG)); let mut service = Mpu6000Service::new(Clock(0), Mpu6000::new(bus, TEST_CONFIG));
service.begin().unwrap(); service.begin().unwrap();
assert!(matches!(service.on_data_ready(), Mpu6000ServiceEvent::Idle)); 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!( assert!(matches!(
service.on_transfer_complete(SpiCompletion::Complete), service.on_transfer_complete(SpiCompletion::Complete),
Mpu6000ServiceEvent::Idle Mpu6000ServiceEvent::Idle
)); ));
assert!(matches!( service.clock.0 = 200_000;
service.on_periodic_tick(), service.on_periodic_tick();
Mpu6000ServiceEvent::Idle service.on_transfer_complete(SpiCompletion::Complete);
)); service.clock.0 = 300_000;
for _ in 0..9 { 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!( assert!(matches!(
service.on_transfer_complete(SpiCompletion::Complete), service.on_transfer_complete(SpiCompletion::Complete),
Mpu6000ServiceEvent::Idle Mpu6000ServiceEvent::Idle
)); ));
} }
service.clock.0 = 410_000;
assert!(matches!( assert!(matches!(
service.on_transfer_complete(SpiCompletion::Complete), service.on_periodic_tick(),
Mpu6000ServiceEvent::Initialized 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!( assert!(matches!(
service.on_periodic_tick(), service.on_periodic_tick(),
Mpu6000ServiceEvent::Idle Mpu6000ServiceEvent::Idle
)); ));
service.clock.0 = 110_050;
assert!(matches!(
service.on_periodic_tick(),
Mpu6000ServiceEvent::Fault(super::Error::TransferTimeout)
));
assert!(matches!( assert!(matches!(
service.on_transfer_complete(SpiCompletion::Complete), service.on_transfer_complete(SpiCompletion::Complete),
Mpu6000ServiceEvent::Sample(_) Mpu6000ServiceEvent::Idle
)); ));
} }
} }
firmware/libs/kaptan-drivers/src/mpu6000/test_bus.rs
+97
View File
@@ -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<CompletionError>,
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(())
}
}
firmware/libs/kaptan-drivers/src/mtf01/mod.rs
+53 -133
View File
@@ -1,14 +1,7 @@
//! MicoAir MTF-01 Microlink UART frame decoder. //! 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_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot};
use kaptan_time::{MonotonicClock, Timestamp}; use kaptan_time::Timestamp;
const FRAME_HEADER: u8 = 0xEF; const FRAME_HEADER: u8 = 0xEF;
const RANGE_SENSOR_MESSAGE_ID: u8 = 0x51; const RANGE_SENSOR_MESSAGE_ID: u8 = 0x51;
@@ -26,11 +19,7 @@ enum State {
Checksum, Checksum,
} }
/// One complete MTF-01 range and optical-flow report. /// MTF-01 range and optical-flow report in device units.
///
/// 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.
#[derive(Clone, Copy, Debug, Eq, PartialEq)] #[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct Mtf01Report { pub struct Mtf01Report {
/// Flight-controller monotonic time at frame reception. /// Flight-controller monotonic time at frame reception.
@@ -58,34 +47,6 @@ pub struct Mtf01Report {
} }
impl 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. /// Returns the valid range portion of this report.
pub const fn range_snapshot(self) -> Option<RangeSnapshot> { pub const fn range_snapshot(self) -> Option<RangeSnapshot> {
if self.distance_valid && self.distance_millimetres != 0 { 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 /// Accepts message ID 0x51 with a 20-byte payload. Device and system IDs are
/// documented range-sensor message ID and 20-byte payload length. Device and /// unrestricted, matching MicoAir's reference decoder.
/// system IDs are deliberately accepted as transmitted, matching MicoAir's
/// reference decoder and allowing sensor IDs configured outside factory
/// defaults.
pub struct Mtf01 { pub struct Mtf01 {
state: State, state: State,
checksum: u8, checksum: u8,
@@ -155,9 +113,8 @@ impl Mtf01 {
/// Accepts one UART byte and returns a complete, checksum-validated MTF-01 /// Accepts one UART byte and returns a complete, checksum-validated MTF-01
/// report when available. /// report when available.
/// ///
/// `timestamp` should be the controller monotonic time at receipt of this /// `timestamp` is the controller's monotonic time at byte receipt.
/// byte. It is used only for a completed frame, so it records the arrival /// The report retains the checksum byte's timestamp.
/// time of the report rather than an arbitrary intermediate byte.
pub fn receive_byte(&mut self, timestamp: Timestamp, byte: u8) -> Option<Mtf01Report> { pub fn receive_byte(&mut self, timestamp: Timestamp, byte: u8) -> Option<Mtf01Report> {
match self.state { match self.state {
State::Header => { State::Header => {
@@ -198,10 +155,7 @@ impl Mtf01 {
} }
State::Checksum => { State::Checksum => {
let valid = byte == self.checksum; let valid = byte == self.checksum;
// The checksum terminates the frame even when its value is // A checksum of 0xEF ends the frame and is not a new header.
// 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.
self.reset(); self.reset();
if valid { if valid {
return Some(self.snapshot(timestamp)); return Some(self.snapshot(timestamp));
@@ -233,18 +187,22 @@ impl Mtf01 {
} }
fn snapshot(&self, timestamp: Timestamp) -> Mtf01Report { fn snapshot(&self, timestamp: Timestamp) -> Mtf01Report {
Mtf01Report::new( Mtf01Report {
timestamp, timestamp,
u32::from_le_bytes(self.payload[0..4].try_into().unwrap()), sensor_time_milliseconds: u32::from_le_bytes(self.payload[0..4].try_into().unwrap()),
u32::from_le_bytes(self.payload[4..8].try_into().unwrap()), distance_millimetres: u32::from_le_bytes(self.payload[4..8].try_into().unwrap()),
self.payload[8], distance_strength: self.payload[8],
self.payload[9], distance_precision: self.payload[9],
self.payload[10] == 1, distance_valid: self.payload[10] == 1,
i16::from_le_bytes(self.payload[12..14].try_into().unwrap()), flow_velocity_x_centimetres_per_second_at_one_metre: i16::from_le_bytes(
i16::from_le_bytes(self.payload[14..16].try_into().unwrap()), self.payload[12..14].try_into().unwrap(),
self.payload[16], ),
self.payload[17] == 1, 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: Clock,
decoder: Mtf01,
}
impl<Clock> Mtf01Service<Clock> {
/// 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<Clock: MonotonicClock> Mtf01Service<Clock> {
/// Consumes one UART byte and returns one validated MTF-01 report.
pub fn on_received_byte(&mut self, byte: u8) -> Option<Mtf01Report> {
self.decoder.receive_byte(self.clock.now(), byte)
}
}
#[cfg(test)] #[cfg(test)]
mod tests { mod tests {
use super::{Mtf01, Mtf01Report, Mtf01Service}; use super::{Mtf01, Mtf01Report};
use kaptan_time::{MonotonicClock, Timestamp}; use kaptan_time::Timestamp;
fn frame(payload: [u8; 20]) -> [u8; 27] { fn frame(payload: [u8; 20]) -> [u8; 27] {
let mut frame = [0_u8; 27]; let mut frame = [0_u8; 27];
@@ -323,18 +249,18 @@ mod tests {
assert_eq!( assert_eq!(
report, report,
Some(Mtf01Report::new( Some(Mtf01Report {
timestamp, timestamp,
0x1234_5678, sensor_time_milliseconds: 0x1234_5678,
1_234, distance_millimetres: 1_234,
200, distance_strength: 200,
7, distance_precision: 7,
true, distance_valid: true,
-1_234, flow_velocity_x_centimetres_per_second_at_one_metre: -1_234,
1_234, flow_velocity_y_centimetres_per_second_at_one_metre: 1_234,
250, flow_quality: 250,
true, flow_valid: true,
)) })
); );
} }
@@ -423,18 +349,18 @@ mod tests {
#[test] #[test]
fn exposes_only_valid_generic_observations() { fn exposes_only_valid_generic_observations() {
let mut report = Mtf01Report::new( let mut report = Mtf01Report {
Timestamp::from_microseconds(123), timestamp: Timestamp::from_microseconds(123),
7, sensor_time_milliseconds: 7,
2_000, distance_millimetres: 2_000,
8, distance_strength: 8,
9, distance_precision: 9,
true, distance_valid: true,
50, flow_velocity_x_centimetres_per_second_at_one_metre: 50,
-25, flow_velocity_y_centimetres_per_second_at_one_metre: -25,
12, flow_quality: 12,
true, flow_valid: true,
); };
assert_eq!(report.range_snapshot().unwrap().distance_millimetres, 2_000); assert_eq!(report.range_snapshot().unwrap().distance_millimetres, 2_000);
assert_eq!( assert_eq!(
report report
@@ -449,21 +375,15 @@ mod tests {
assert_eq!(report.optical_flow_snapshot(), None); 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] #[test]
fn timestamps_a_report_with_its_last_received_byte() { 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() let report = sample_frame()
.into_iter() .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(); .unwrap();
assert_eq!(report.timestamp, Timestamp::from_microseconds(127)); assert_eq!(report.timestamp, Timestamp::from_microseconds(127));
firmware/libs/kaptan-file-mavlink/src/lib.rs
+1 -1
View File
@@ -18,7 +18,7 @@ pub use protocol::{
Content, ControlRequest, ControlResponse, DataRequest, DataResponse, FileFailure, FileMetadata, Content, ControlRequest, ControlResponse, DataRequest, DataResponse, FileFailure, FileMetadata,
FileOperation, FilePath, FileRequest, FileResponse, FileStatus, Principal, Target, 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. /// Bytes transported by one `FILE_DATA` record.
pub const DATA_BYTES: usize = 246; pub const DATA_BYTES: usize = 246;
firmware/libs/kaptan-file-mavlink/src/protocol.rs
+3 -3
View File
@@ -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)] #[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct Principal { pub struct Principal {
system: u8, system: u8,
@@ -104,9 +104,9 @@ pub enum FileOperation {
pub enum FileStatus { pub enum FileStatus {
/// The operation succeeded. /// The operation succeeded.
Ok = 1, Ok = 1,
/// Another principal owns the single transfer slot. /// Another peer owns the single transfer slot.
Busy = 2, 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, InvalidTransfer = 3,
/// The operation is not valid for the transfer's current mode or state. /// The operation is not valid for the transfer's current mode or state.
InvalidState = 4, InvalidState = 4,
firmware/libs/kaptan-file-mavlink/src/service.rs
+219 -133
View File
@@ -6,6 +6,46 @@ use crate::{
Principal, Target, 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. /// Format-agnostic owner of the one file currently pinned or staged.
/// ///
/// The service calls `abort` after any provider failure, explicit abort, lease /// The service calls `abort` after any provider failure, explicit abort, lease
@@ -69,7 +109,7 @@ struct LastChunk {
} }
struct Session { struct Session {
owner: Principal, owner: TransferPeer,
transfer_id: u16, transfer_id: u16,
path: FilePath, path: FilePath,
mode: Mode, mode: Mode,
@@ -81,7 +121,7 @@ struct Session {
#[derive(Clone, Copy)] #[derive(Clone, Copy)]
struct Completed { struct Completed {
owner: Principal, owner: TransferPeer,
transfer_id: u16, transfer_id: u16,
request: FileRequest, request: FileRequest,
response: ControlResponse, 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 /// Untargeted requests are silently discarded. The caller may retry a
/// response whose transmission was not admitted: every state-changing /// 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<P: FileProvider>( pub fn dispatch<P: FileProvider>(
&mut self, &mut self,
provider: &mut P, provider: &mut P,
principal: Principal, peer: TransferPeer,
request: FileRequest, request: FileRequest,
) -> Option<FileResponse> { ) -> Option<DispatchResult> {
let addressed = match request { let addressed = match request {
FileRequest::Control(request) => request.target, FileRequest::Control(request) => request.target,
FileRequest::Data(request) => request.target, FileRequest::Data(request) => request.target,
@@ -128,16 +170,15 @@ impl FileService {
} }
Some(match request { Some(match request {
FileRequest::Control(request) if request.operation == FileOperation::Read => { FileRequest::Control(request) if request.operation == FileOperation::Read => {
self.read(provider, principal, request) self.read(provider, peer, request)
} }
FileRequest::Control(request) => { FileRequest::Control(request) => self.control(provider, peer, request),
FileResponse::Control(self.control(provider, principal, request)) FileRequest::Data(request) => self.data(provider, peer, request),
}
FileRequest::Data(request) => self.data(provider, principal, 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<P: FileProvider>(&mut self, provider: &mut P) -> bool { pub fn expire<P: FileProvider>(&mut self, provider: &mut P) -> bool {
let expired = matches!(self.state, TransferState::Open(_)); let expired = matches!(self.state, TransferState::Open(_));
if expired { if expired {
@@ -150,28 +191,32 @@ impl FileService {
fn control<P: FileProvider>( fn control<P: FileProvider>(
&mut self, &mut self,
provider: &mut P, provider: &mut P,
principal: Principal, peer: TransferPeer,
request: ControlRequest, request: ControlRequest,
) -> ControlResponse { ) -> DispatchResult {
match request.operation { match request.operation {
FileOperation::OpenRead => self.open(provider, principal, request, Mode::Read), FileOperation::OpenRead => self.open(provider, peer, request, Mode::Read),
FileOperation::OpenWrite => self.open(provider, principal, request, Mode::Write), FileOperation::OpenWrite => self.open(provider, peer, request, Mode::Write),
FileOperation::Read => unreachable!("read requests are dispatched as data responses"), FileOperation::Read => unreachable!("read requests are dispatched as data responses"),
FileOperation::Close => self.close(provider, principal, request), FileOperation::Close => self.close(provider, peer, request),
FileOperation::Abort => self.abort(provider, principal, request), FileOperation::Abort => self.abort(provider, peer, request),
FileOperation::WriteAck => { FileOperation::WriteAck => self.failure(
self.failure(principal, request, FileStatus::InvalidState, Content::EMPTY) peer.principal,
} request,
FileStatus::InvalidState,
Content::EMPTY,
),
} }
} }
fn open<P: FileProvider>( fn open<P: FileProvider>(
&mut self, &mut self,
provider: &mut P, provider: &mut P,
principal: Principal, peer: TransferPeer,
request: ControlRequest, request: ControlRequest,
mode: Mode, mode: Mode,
) -> ControlResponse { ) -> DispatchResult {
let principal = peer.principal;
if request.transfer_id == 0 || request.position != 0 { if request.transfer_id == 0 || request.position != 0 {
return self.failure( return self.failure(
principal, principal,
@@ -188,7 +233,7 @@ impl FileService {
}; };
let replace_open = if let TransferState::Open(session) = &self.state { 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); return self.failure(principal, request, FileStatus::Busy, Content::EMPTY);
} }
let same = session.transfer_id == request.transfer_id let same = session.transfer_id == request.transfer_id
@@ -229,7 +274,7 @@ impl FileService {
self.state = TransferState::Idle; self.state = TransferState::Idle;
} }
if let TransferState::Completed(completed) = &self.state { if let TransferState::Completed(completed) = &self.state {
if completed.owner != principal { if completed.owner != peer {
return self.failure(principal, request, FileStatus::Busy, Content::EMPTY); return self.failure(principal, request, FileStatus::Busy, Content::EMPTY);
} }
if completed.transfer_id == request.transfer_id { if completed.transfer_id == request.transfer_id {
@@ -258,7 +303,7 @@ impl FileService {
} }
}; };
self.state = TransferState::Open(Session { self.state = TransferState::Open(Session {
owner: principal, owner: peer,
transfer_id: request.transfer_id, transfer_id: request.transfer_id,
path, path,
mode, mode,
@@ -276,36 +321,27 @@ impl FileService {
fn read<P: FileProvider>( fn read<P: FileProvider>(
&mut self, &mut self,
provider: &mut P, provider: &mut P,
principal: Principal, peer: TransferPeer,
request: ControlRequest, request: ControlRequest,
) -> FileResponse { ) -> DispatchResult {
let principal = peer.principal;
if !canonical_non_open(&request) { if !canonical_non_open(&request) {
return FileResponse::Control(self.failure( return self.failure(principal, request, FileStatus::InvalidState, Content::EMPTY);
principal,
request,
FileStatus::InvalidState,
Content::EMPTY,
));
} }
let TransferState::Open(session) = &mut self.state else { 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 { if session.owner != peer || session.transfer_id != request.transfer_id {
return FileResponse::Control(response( return self.failure(
principal, principal,
request.operation, request,
request.transfer_id,
request.position,
FileMetadata {
size: request.file_size,
checksum_crc32c: request.checksum_crc32c,
},
FileStatus::InvalidTransfer, FileStatus::InvalidTransfer,
Content::EMPTY, Content::EMPTY,
)); );
} }
if session.mode != Mode::Read { if session.mode != Mode::Read {
return FileResponse::Control(response( return DispatchResult::control(
response(
principal, principal,
request.operation, request.operation,
request.transfer_id, request.transfer_id,
@@ -313,7 +349,9 @@ impl FileService {
session.metadata, session.metadata,
FileStatus::InvalidState, FileStatus::InvalidState,
Content::EMPTY, Content::EMPTY,
)); ),
false,
);
} }
if request.position != session.position { if request.position != session.position {
if let Some(last) = session if let Some(last) = session
@@ -321,14 +359,15 @@ impl FileService {
.as_ref() .as_ref()
.filter(|last| last.position == request.position) .filter(|last| last.position == request.position)
{ {
return FileResponse::Data(DataResponse { return DispatchResult::data(DataResponse {
target: principal.target(), target: principal.target(),
position: request.position, position: request.position,
transfer_id: request.transfer_id, transfer_id: request.transfer_id,
data: last.data, data: last.data,
}); });
} }
return FileResponse::Control(response( return DispatchResult::control(
response(
principal, principal,
request.operation, request.operation,
request.transfer_id, request.transfer_id,
@@ -336,10 +375,13 @@ impl FileService {
session.metadata, session.metadata,
FileStatus::InvalidPosition, FileStatus::InvalidPosition,
Content::EMPTY, Content::EMPTY,
)); ),
false,
);
} }
let Some(logical) = chunk_length(session.metadata.size, request.position) else { let Some(logical) = chunk_length(session.metadata.size, request.position) else {
return FileResponse::Control(response( return DispatchResult::control(
response(
principal, principal,
request.operation, request.operation,
request.transfer_id, request.transfer_id,
@@ -347,10 +389,13 @@ impl FileService {
session.metadata, session.metadata,
FileStatus::InvalidPosition, FileStatus::InvalidPosition,
Content::EMPTY, Content::EMPTY,
)); ),
false,
);
}; };
if logical == 0 { if logical == 0 {
return FileResponse::Control(response( return DispatchResult::control(
response(
principal, principal,
request.operation, request.operation,
request.transfer_id, request.transfer_id,
@@ -358,7 +403,9 @@ impl FileService {
session.metadata, session.metadata,
FileStatus::InvalidPosition, FileStatus::InvalidPosition,
Content::EMPTY, Content::EMPTY,
)); ),
false,
);
} }
let mut data = [0_u8; DATA_BYTES]; let mut data = [0_u8; DATA_BYTES];
if let Err(error) = provider.read(request.position, &mut data[..logical]) { if let Err(error) = provider.read(request.position, &mut data[..logical]) {
@@ -373,15 +420,14 @@ impl FileService {
error.status(), error.status(),
error.content(), error.content(),
); );
self.remember(principal, FileRequest::Control(request), response); return self.remember(peer, FileRequest::Control(request), response);
return FileResponse::Control(response);
} }
session.last_chunk = Some(LastChunk { session.last_chunk = Some(LastChunk {
position: request.position, position: request.position,
data, data,
}); });
session.position += logical as u32; session.position += logical as u32;
FileResponse::Data(DataResponse { DispatchResult::data(DataResponse {
target: principal.target(), target: principal.target(),
position: request.position, position: request.position,
transfer_id: request.transfer_id, transfer_id: request.transfer_id,
@@ -392,66 +438,64 @@ impl FileService {
fn data<P: FileProvider>( fn data<P: FileProvider>(
&mut self, &mut self,
provider: &mut P, provider: &mut P,
principal: Principal, peer: TransferPeer,
request: DataRequest, request: DataRequest,
) -> FileResponse { ) -> DispatchResult {
let principal = peer.principal;
let TransferState::Open(session) = &mut self.state else { 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 { if session.owner != peer || session.transfer_id != request.transfer_id {
return FileResponse::Control(data_failure( return DispatchResult::control(
principal, data_failure(principal, request, FileStatus::InvalidTransfer),
request, false,
FileStatus::InvalidTransfer, );
));
} }
if session.mode != Mode::Write { if session.mode != Mode::Write {
return FileResponse::Control(data_failure( return DispatchResult::control(
principal, data_failure(principal, request, FileStatus::InvalidState),
request, false,
FileStatus::InvalidState, );
));
} }
let logical = match chunk_length(session.metadata.size, request.position) { let logical = match chunk_length(session.metadata.size, request.position) {
Some(length) if length != 0 => length, Some(length) if length != 0 => length,
_ => { _ => {
return FileResponse::Control(data_failure( return DispatchResult::control(
principal, data_failure(principal, request, FileStatus::InvalidPosition),
request, false,
FileStatus::InvalidPosition, );
));
} }
}; };
if request.data[logical..].iter().any(|byte| *byte != 0) { if request.data[logical..].iter().any(|byte| *byte != 0) {
return FileResponse::Control(data_failure( return DispatchResult::control(
principal, data_failure(principal, request, FileStatus::RequestConflict),
request, false,
FileStatus::RequestConflict, );
));
} }
if request.position < session.position { if request.position < session.position {
let replayed = session let replayed = session
.last_chunk .last_chunk
.as_ref() .as_ref()
.is_some_and(|last| last.position == request.position && last.data == request.data); .is_some_and(|last| last.position == request.position && last.data == request.data);
return FileResponse::Control(if replayed { return DispatchResult::control(
if replayed {
write_ack(principal, session) write_ack(principal, session)
} else { } else {
data_failure(principal, request, FileStatus::RequestConflict) data_failure(principal, request, FileStatus::RequestConflict)
}); },
replayed,
);
} }
if request.position != session.position { if request.position != session.position {
return FileResponse::Control(data_failure( return DispatchResult::control(
principal, data_failure(principal, request, FileStatus::InvalidPosition),
request, false,
FileStatus::InvalidPosition, );
));
} }
if let Err(error) = provider.write(request.position, &request.data[..logical]) { if let Err(error) = provider.write(request.position, &request.data[..logical]) {
provider.abort(); provider.abort();
let response = data_provider_failure(principal, request, error); let response = data_provider_failure(principal, request, error);
self.remember(principal, FileRequest::Data(request), response); return self.remember(peer, FileRequest::Data(request), response);
return FileResponse::Control(response);
} }
session.checksum.update(&request.data[..logical]); session.checksum.update(&request.data[..logical]);
session.position += logical as u32; session.position += logical as u32;
@@ -459,22 +503,23 @@ impl FileService {
position: request.position, position: request.position,
data: request.data, data: request.data,
}); });
FileResponse::Control(write_ack(principal, session)) DispatchResult::control(write_ack(principal, session), true)
} }
fn close<P: FileProvider>( fn close<P: FileProvider>(
&mut self, &mut self,
provider: &mut P, provider: &mut P,
principal: Principal, peer: TransferPeer,
request: ControlRequest, request: ControlRequest,
) -> ControlResponse { ) -> DispatchResult {
let principal = peer.principal;
if !canonical_non_open(&request) { if !canonical_non_open(&request) {
return self.failure(principal, request, FileStatus::InvalidState, Content::EMPTY); return self.failure(principal, request, FileStatus::InvalidState, Content::EMPTY);
} }
let TransferState::Open(session) = &self.state else { 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( return self.failure(
principal, principal,
request, request,
@@ -494,14 +539,19 @@ impl FileService {
&& session.checksum.checksum() != session.metadata.checksum_crc32c && session.checksum.checksum() != session.metadata.checksum_crc32c
{ {
provider.abort(); provider.abort();
let response = self.failure( let response = response(
principal, principal,
request, request.operation,
request.transfer_id,
request.position,
FileMetadata {
size: request.file_size,
checksum_crc32c: request.checksum_crc32c,
},
FileStatus::ChecksumMismatch, FileStatus::ChecksumMismatch,
Content::EMPTY, Content::EMPTY,
); );
self.remember(principal, FileRequest::Control(request), response); return self.remember(peer, FileRequest::Control(request), response);
return response;
} }
let completion = provider.close(); let completion = provider.close();
let response = match completion { let response = match completion {
@@ -527,23 +577,23 @@ impl FileService {
) )
} }
}; };
self.remember(principal, FileRequest::Control(request), response); self.remember(peer, FileRequest::Control(request), response)
response
} }
fn abort<P: FileProvider>( fn abort<P: FileProvider>(
&mut self, &mut self,
provider: &mut P, provider: &mut P,
principal: Principal, peer: TransferPeer,
request: ControlRequest, request: ControlRequest,
) -> ControlResponse { ) -> DispatchResult {
let principal = peer.principal;
if !canonical_non_open(&request) { if !canonical_non_open(&request) {
return self.failure(principal, request, FileStatus::InvalidState, Content::EMPTY); return self.failure(principal, request, FileStatus::InvalidState, Content::EMPTY);
} }
let TransferState::Open(session) = &self.state else { 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( return self.failure(
principal, principal,
request, request,
@@ -562,8 +612,7 @@ impl FileService {
FileStatus::Ok, FileStatus::Ok,
Content::EMPTY, Content::EMPTY,
); );
self.remember(principal, FileRequest::Control(request), response); self.remember(peer, FileRequest::Control(request), response)
response
} }
fn open_response( fn open_response(
@@ -571,7 +620,8 @@ impl FileService {
principal: Principal, principal: Principal,
operation: FileOperation, operation: FileOperation,
session: &Session, session: &Session,
) -> ControlResponse { ) -> DispatchResult {
DispatchResult::control(
response( response(
principal, principal,
operation, operation,
@@ -580,6 +630,8 @@ impl FileService {
session.metadata, session.metadata,
FileStatus::Ok, FileStatus::Ok,
Content::EMPTY, Content::EMPTY,
),
true,
) )
} }
@@ -589,7 +641,8 @@ impl FileService {
request: ControlRequest, request: ControlRequest,
status: FileStatus, status: FileStatus,
content: Content, content: Content,
) -> ControlResponse { ) -> DispatchResult {
DispatchResult::control(
response( response(
principal, principal,
request.operation, request.operation,
@@ -601,20 +654,23 @@ impl FileService {
}, },
status, status,
content, 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 { match &self.state {
TransferState::Completed(completed) TransferState::Completed(completed)
if completed.owner == principal if completed.owner == peer
&& completed.transfer_id == request.transfer_id && completed.transfer_id == request.transfer_id
&& completed.request == FileRequest::Control(request) => && completed.request == FileRequest::Control(request) =>
{ {
completed.response DispatchResult::control(completed.response, true)
} }
TransferState::Completed(completed) 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( self.failure(
principal, principal,
@@ -633,31 +689,39 @@ impl FileService {
} }
} }
fn replay_data_or_failure( fn replay_data_or_failure(&self, peer: TransferPeer, request: DataRequest) -> DispatchResult {
&self, let principal = peer.principal;
principal: Principal,
request: DataRequest,
) -> ControlResponse {
match &self.state { match &self.state {
TransferState::Completed(completed) TransferState::Completed(completed)
if completed.owner == principal if completed.owner == peer
&& completed.transfer_id == request.transfer_id && completed.transfer_id == request.transfer_id
&& completed.request == FileRequest::Data(request) => && completed.request == FileRequest::Data(request) =>
{ {
completed.response DispatchResult::control(completed.response, true)
} }
TransferState::Completed(completed) 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(_) => { 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 { let transfer_id = match request {
FileRequest::Control(request) => request.transfer_id, FileRequest::Control(request) => request.transfer_id,
FileRequest::Data(request) => request.transfer_id, FileRequest::Data(request) => request.transfer_id,
@@ -668,6 +732,7 @@ impl FileService {
request, request,
response, response,
}); });
DispatchResult::control(response, true)
} }
} }
@@ -753,7 +818,10 @@ mod tests {
use super::*; use super::*;
const SERVICE: Target = Target::new(1, 1); 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"; const PATH: &[u8] = b"/test/data.bin";
#[derive(Clone, Copy, Eq, PartialEq)] #[derive(Clone, Copy, Eq, PartialEq)]
@@ -903,8 +971,12 @@ mod tests {
}) })
} }
fn control_response(response: Option<FileResponse>) -> ControlResponse { fn control_response(response: Option<DispatchResult>) -> ControlResponse {
let Some(FileResponse::Control(response)) = response else { let Some(DispatchResult {
response: FileResponse::Control(response),
..
}) = response
else {
panic!("expected control response"); panic!("expected control response");
}; };
response response
@@ -960,20 +1032,28 @@ mod tests {
assert_eq!(opened.file_size, 300); assert_eq!(opened.file_size, 300);
let first = service.dispatch(&mut provider, CLIENT, control(FileOperation::Read, 9, 0)); 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"); panic!("expected data");
}; };
assert_eq!(&first.data, &[0x5a; DATA_BYTES]); assert_eq!(&first.data, &[0x5a; DATA_BYTES]);
let retry = service.dispatch(&mut provider, CLIENT, control(FileOperation::Read, 9, 0)); 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); 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, &mut provider,
CLIENT, CLIENT,
control(FileOperation::Read, 9, DATA_BYTES as u32), control(FileOperation::Read, 9, DATA_BYTES as u32),
) else { )
else {
panic!("expected final data"); panic!("expected final data");
}; };
assert_eq!(&last.data[..54], &source[DATA_BYTES..]); assert_eq!(&last.data[..54], &source[DATA_BYTES..]);
@@ -1001,7 +1081,10 @@ mod tests {
); );
assert!(matches!( assert!(matches!(
service.dispatch(&mut provider, CLIENT, control(FileOperation::Read, 1, 0),), service.dispatch(&mut provider, CLIENT, control(FileOperation::Read, 1, 0),),
Some(FileResponse::Data(_)) Some(DispatchResult {
response: FileResponse::Data(_),
refresh_lease: true
})
)); ));
assert_eq!( assert_eq!(
control_response(service.dispatch( control_response(service.dispatch(
@@ -1035,7 +1118,10 @@ mod tests {
assert!(matches!( assert!(matches!(
service.dispatch(&mut provider, CLIENT, control(FileOperation::Read, 2, 0)), service.dispatch(&mut provider, CLIENT, control(FileOperation::Read, 2, 0)),
Some(FileResponse::Data(_)) Some(DispatchResult {
response: FileResponse::Data(_),
refresh_lease: true
})
)); ));
assert_eq!( assert_eq!(
control_response(service.dispatch( control_response(service.dispatch(
firmware/libs/kaptan-file-mavlink/tests/peer_lease.rs
+632
View File
@@ -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<u8>,
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<FileMetadata, FileFailure> {
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<Content, FileFailure> {
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);
}
firmware/libs/kaptan-flight-config/Cargo.toml
+5 -3
View File
@@ -10,6 +10,8 @@ workspace = true
[dependencies] [dependencies]
kaptan-config = { path = "../kaptan-config" } kaptan-config = { path = "../kaptan-config" }
kaptan-err-reportable = { path = "../kaptan-err-reportable" } kaptan-control = { path = "../kaptan-control" }
kaptan-log = { path = "../kaptan-log" } kaptan-navigation = { path = "../kaptan-navigation" }
minicbor = { version = "0.25", features = ["derive"] }
[dev-dependencies]
kaptan-math = { path = "../kaptan-math" }
firmware/libs/kaptan-flight-config/src/cbor.rs
-382
View File
@@ -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<E> {
/// 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<E>),
}
/// 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<CandidateProfile, CborError> {
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<W>(
candidate: CandidateProfile,
capabilities: &VehicleCapabilities,
writer: W,
) -> Result<(), CborEncodeError<W::Error>>
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<CandidateProfile, CborError> {
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<W>(
encoder: &mut Encoder<W>,
value: ConfigValue,
) -> Result<(), minicbor::encode::Error<W::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<ConfigValue, CborError> {
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<F>(
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)
}
}
firmware/libs/kaptan-flight-config/src/lib.rs
+8 -6
View File
@@ -1,17 +1,19 @@
//! Vehicle configuration schema and CBOR snapshot codec. //! Validated configuration for the portable flight runtime.
#![no_std] #![no_std]
pub mod cbor;
pub mod profile; pub mod profile;
pub use cbor::{ pub use profile::{CandidateProfile, VehicleCapabilities, VehicleProfile};
CborEncodeError, CborError, MAXIMUM_CANDIDATE_BYTES, decode_candidate, encode_candidate,
};
pub use profile::{CandidateProfile, FORMAT_VERSION, VehicleCapabilities, VehicleProfile};
/// Generated fingerprint of the complete vehicle configuration schema. /// Generated fingerprint of the complete vehicle configuration schema.
pub const SCHEMA_ID: [u8; 16] = <CandidateProfile as kaptan_config::Config>::SCHEMA_ID; pub const SCHEMA_ID: [u8; 16] = <CandidateProfile as kaptan_config::Config>::SCHEMA_ID;
#[cfg(test)] #[cfg(test)]
extern crate std; extern crate std;
#[cfg(test)]
extern crate self as kaptan_flight_config;
#[cfg(test)]
#[path = "../tests/support/mod.rs"]
mod test_support;
firmware/libs/kaptan-flight-config/src/profile/active.rs
+81 -197
View File
@@ -1,30 +1,28 @@
//! Immutable configuration projections consumed by runtime owners. //! Immutable configuration projections consumed by runtime owners.
/// Immutable compiled contract for the physical vehicle/board image. use kaptan_config::{Config, ConfigValue, ConfigViolation, FieldId, ValidationRule};
/// use kaptan_navigation::{
/// A runtime candidate can tune only values inside this contract. It cannot ContactGeometry, NavigationConfig, NavigationInstallation, NoiseDensities,
/// create pins, buses, sensor drivers, or actuator channels. };
use super::{
ActuatorLimits, CandidateProfile, ControlSettings, PidSettings, SafetySettings, ThrustSettings,
TimingSettings,
};
/// Compiled vehicle capabilities.
#[derive(Clone, Copy, Debug, PartialEq)] #[derive(Clone, Copy, Debug, PartialEq)]
pub struct VehicleCapabilities { pub struct VehicleCapabilities {
/// Number of board-declared logical actuator channels available to the /// Fixed allocation rows. Profile edits cannot change motor order or thrust signs.
/// 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.
mixer: [[f32; 4]; 4], mixer: [[f32; 4]; 4],
installation: NavigationInstallation,
} }
impl VehicleCapabilities { impl VehicleCapabilities {
/// Returns the reviewed four-actuator Quad-X allocation supported by P1. /// Four-actuator Quad-X allocation with an explicit navigation installation.
/// pub const fn quad_x(installation: NavigationInstallation) -> Self {
/// 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 {
Self { Self {
actuator_count: 4, installation,
mixer: [ mixer: [
[-1.0, 1.0, 1.0, 1.0], [-1.0, 1.0, 1.0, 1.0],
[-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. /// 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)] #[derive(Clone, Copy, Debug, PartialEq)]
pub struct VehicleProfile { pub struct VehicleProfile {
pub(crate) safety: SafetySettings, candidate: CandidateProfile,
pub(crate) timing: TimingSettings, capabilities: VehicleCapabilities,
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],
} }
impl VehicleProfile { impl VehicleProfile {
pub(super) fn validate(
candidate: &CandidateProfile,
capabilities: VehicleCapabilities,
) -> Result<Self, ConfigViolation> {
<CandidateProfile as Config>::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<ConfigValue> {
<CandidateProfile as Config>::field_value(&self.candidate, id)
}
pub const fn safety(self) -> SafetySettings { pub const fn safety(self) -> SafetySettings {
self.safety self.candidate.safety
} }
pub const fn timing(self) -> TimingSettings { pub const fn timing(self) -> TimingSettings {
self.timing self.candidate.timing
}
pub const fn aiding(self) -> AidingSettings {
self.aiding
} }
pub const fn control(self) -> ControlSettings { pub const fn control(self) -> ControlSettings {
self.control self.candidate.control
} }
pub const fn thrust(self) -> ThrustSettings { pub const fn thrust(self) -> ThrustSettings {
self.thrust self.candidate.thrust
} }
pub const fn rate_pid(self) -> PidSettings { pub const fn rate_pid(self) -> PidSettings {
self.rate_pid self.candidate.rate_pid
} }
pub const fn actuator_limits(self) -> ActuatorLimits { pub const fn actuator_limits(self) -> ActuatorLimits {
self.actuator_limits self.candidate.actuator_limits
}
/// Immutable boot-active policy for the MAVLink diagnostic sink.
pub const fn mavlink_logging(self) -> kaptan_log::LevelFilter {
self.mavlink_logging
} }
pub const fn mixer(self) -> [[f32; 4]; 4] { 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,
}
} }
} }
firmware/libs/kaptan-flight-config/src/profile/candidate.rs
+171 -264
View File
@@ -1,93 +1,55 @@
//! Private vehicle candidate and whole-profile validation boundary. //! Editable vehicle candidate and whole-profile validation boundary.
use kaptan_config::{ use kaptan_config::{
Config, ConfigModel, ConfigValue, ConfigViolation, FieldDescriptor, FieldId, FieldWriteError, Config, ConfigModel, ConfigValue, ConfigViolation, FieldDescriptor, FieldId, FieldWriteError,
ValidationRule,
}; };
use kaptan_log::LevelFilter;
use super::{ use super::{
ActuatorLimits, AidingSettings, ControlSettings, PidSettings, SafetySettings, ThrustSettings, ActuatorLimits, ControlSettings, PidSettings, SafetySettings, ThrustSettings, TimingSettings,
TimingSettings, VehicleCapabilities, VehicleProfile, VehicleCapabilities, VehicleProfile,
control::{
ActuatorLimitsCandidate, AidingCandidate, ControlCandidate, PidCandidate, ThrustCandidate,
},
mavlink_logging::MavlinkLoggingCandidate,
safety::SafetyCandidate,
timing::TimingCandidate,
}; };
/// Current persisted CBOR schema for the vehicle candidate.
pub const FORMAT_VERSION: u16 = 3;
/// Complete editable vehicle candidate. /// Complete editable vehicle candidate.
/// ///
/// Fields are private outside this crate. Local snapshot recovery and MAVLink /// Descriptor writes require complete validation before activation.
/// scalar writes both use the same descriptor/mutation and full-validation #[derive(Clone, Copy, Debug, PartialEq, Config)]
/// path. #[config(post = Self::validate_flight)]
#[derive(Clone, Copy, Config)]
pub struct CandidateProfile { pub struct CandidateProfile {
pub(super) safety: SafetyCandidate, pub safety: SafetySettings,
pub(super) timing: TimingCandidate, pub timing: TimingSettings,
pub(super) aiding: AidingCandidate, pub control: ControlSettings,
pub(super) control: ControlCandidate, pub thrust: ThrustSettings,
pub(super) thrust: ThrustCandidate, pub rate_pid: PidSettings,
pub(super) rate_pid: PidCandidate, pub actuator_limits: ActuatorLimits,
pub(super) actuator_limits: ActuatorLimitsCandidate, pub flight: super::FlightSettings,
pub(super) mavlink_logging: MavlinkLoggingCandidate,
} }
impl CandidateProfile { impl CandidateProfile {
/// Explicit factory candidate used by the vehicle-profile examples. fn validate_flight(&self) -> Result<(), ConfigViolation> {
/// use super::ids;
/// Board firmware supplies its own factory [`Config`] through the board use kaptan_config::ValidationRule;
/// composition root; configuration types never synthesize values through let field = if !(10_000..=20_000).contains(&self.timing.maximum_imu_interval_microseconds) {
/// [`Default`]. Some(ids::MAXIMUM_IMU_INTERVAL_MICROSECONDS)
pub const fn factory() -> Self { } else if self.timing.imu_freshness_microseconds != 20_000 {
Self { Some(ids::IMU_FRESHNESS_MICROSECONDS)
safety: SafetyCandidate { } else if !(200_000..=500_000).contains(&self.timing.local_state_freshness_microseconds) {
prearm_dwell_microseconds: 100_000, Some(ids::LOCAL_STATE_FRESHNESS_MICROSECONDS)
}, } else if self.timing.offboard_timeout_microseconds != 300_000 {
timing: TimingCandidate { Some(ids::OFFBOARD_TIMEOUT_MICROSECONDS)
maximum_imu_interval_microseconds: 50_000, } else if self.actuator_limits.minimum != 0. {
imu_freshness_microseconds: 50_000, Some(ids::ACTUATOR_MINIMUM)
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,
},
} }
// 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. /// Returns the complete transport-neutral descriptor registry.
pub fn descriptors() -> impl ExactSizeIterator<Item = &'static FieldDescriptor> + Clone { pub fn descriptors() -> impl ExactSizeIterator<Item = &'static FieldDescriptor> + Clone {
(0..Self::FIELD_COUNT).map(|index| { (0..Self::FIELD_COUNT).map(|index| {
@@ -95,130 +57,11 @@ impl CandidateProfile {
.expect("derived aggregate descriptor count must be exact") .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<VehicleProfile, ConfigViolation> {
<Self as Config>::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 { impl ConfigModel for CandidateProfile {
type Active = VehicleProfile; type Active = VehicleProfile;
type Context = VehicleCapabilities; type Context = VehicleCapabilities;
@@ -237,7 +80,7 @@ impl ConfigModel for CandidateProfile {
} }
fn active_field_value(active: &Self::Active, id: FieldId) -> Option<ConfigValue> { fn active_field_value(active: &Self::Active, id: FieldId) -> Option<ConfigValue> {
<Self as Config>::field_value(&Self::from_active(active), id) active.field_value(id)
} }
fn set_field(&mut self, id: FieldId, value: ConfigValue) -> Result<(), FieldWriteError> { 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::Active, ConfigViolation> { fn validate(&self, context: &Self::Context) -> Result<Self::Active, ConfigViolation> {
self.validate_complete(context) VehicleProfile::validate(self, *context)
} }
} }
@@ -253,18 +96,43 @@ impl ConfigModel for CandidateProfile {
mod tests { mod tests {
use kaptan_config::{ConfigModel, ConfigValue, FieldId, FieldWriteError, ValidationRule}; use kaptan_config::{ConfigModel, ConfigValue, FieldId, FieldWriteError, ValidationRule};
use super::{CandidateProfile, LevelFilter, VehicleCapabilities}; use super::{CandidateProfile, VehicleCapabilities};
use crate::profile::ids; 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] #[test]
fn generated_schema_fingerprint_is_stable() { fn generated_schema_fingerprint_is_stable() {
assert_eq!( assert_eq!(
crate::SCHEMA_ID, crate::SCHEMA_ID,
[ [
0xb9, 0x8b, 0xf9, 0xa0, 0x62, 0x3f, 0x6e, 0xef, 0x4e, 0x88, 0xc6, 0xd8, 0x89, 0x4c, 172, 176, 17, 7, 200, 246, 190, 201, 231, 26, 101, 3, 145, 64, 206, 150,
0x76, 0x8b,
] ]
); );
} }
@@ -278,12 +146,72 @@ mod tests {
#[test] #[test]
fn every_descriptor_has_a_readable_factory_value() { fn every_descriptor_has_a_readable_factory_value() {
let candidate = CandidateProfile::factory(); let candidate = fixture_candidate();
for descriptor in CandidateProfile::descriptors() { for descriptor in CandidateProfile::descriptors() {
assert!(candidate.field_value(descriptor.id).is_some()); 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] #[test]
fn aggregated_descriptor_ids_are_unique() { fn aggregated_descriptor_ids_are_unique() {
for (index, descriptor) in CandidateProfile::descriptors().enumerate() { for (index, descriptor) in CandidateProfile::descriptors().enumerate() {
@@ -293,11 +221,19 @@ mod tests {
.all(|other| other.id != descriptor.id) .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] #[test]
fn cross_field_invalid_edit_is_retained_only_in_the_candidate() { fn cross_field_invalid_edit_is_retained_only_in_the_candidate() {
let mut candidate = CandidateProfile::factory(); let mut candidate = fixture_candidate();
candidate candidate
.set_field( .set_field(
ids::MAXIMUM_IMU_INTERVAL_MICROSECONDS, ids::MAXIMUM_IMU_INTERVAL_MICROSECONDS,
@@ -312,13 +248,42 @@ mod tests {
#[test] #[test]
fn local_write_error_retains_exact_field_and_rule() { fn local_write_error_retains_exact_field_and_rule() {
let mut candidate = CandidateProfile::factory(); let mut candidate = fixture_candidate();
assert_eq!( assert_eq!(
candidate.set_field(ids::MASS_KG, ConfigValue::F32(f32::NAN)), candidate.set_field(ids::MASS_KG, ConfigValue::F32(f32::NAN)),
Err(FieldWriteError::non_finite(ids::MASS_KG)) 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] #[test]
fn descriptor_bounds_do_not_conflict_with_derived_field_validation() { fn descriptor_bounds_do_not_conflict_with_derived_field_validation() {
for descriptor in CandidateProfile::descriptors() { for descriptor in CandidateProfile::descriptors() {
@@ -326,7 +291,7 @@ mod tests {
.into_iter() .into_iter()
.flatten() .flatten()
{ {
let mut candidate = CandidateProfile::factory(); let mut candidate = fixture_candidate();
candidate candidate
.set_field(descriptor.id, bound) .set_field(descriptor.id, bound)
.expect("descriptor bound must pass the descriptor-backed setter"); .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()
);
}
}
} }
firmware/libs/kaptan-flight-config/src/profile/control.rs
+85 -48
View File
@@ -2,28 +2,10 @@ use kaptan_config::{Config, ConfigViolation, ValidationRule};
use super::ids; use super::ids;
#[derive(Clone, Copy, Config)] /// Outer-loop controller policy.
pub(super) struct AidingCandidate { #[derive(Clone, Copy, Debug, PartialEq, Config)]
#[config( #[config(post = Self::validate_control)]
id = 6, pub struct ControlSettings {
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 {
#[config( #[config(
id = 8, id = 8,
key = "control.position_gain_per_s2", key = "control.position_gain_per_s2",
@@ -31,7 +13,7 @@ pub(super) struct ControlCandidate {
finite, finite,
range(min = 0.0, max = 100.0) range(min = 0.0, max = 100.0)
)] )]
pub(super) position_gain_per_second_squared: f32, pub position_gain_per_second_squared: f32,
#[config( #[config(
id = 9, id = 9,
key = "control.velocity_gain_per_s", key = "control.velocity_gain_per_s",
@@ -39,7 +21,7 @@ pub(super) struct ControlCandidate {
finite, finite,
range(min = 0.0, max = 100.0) range(min = 0.0, max = 100.0)
)] )]
pub(super) velocity_gain_per_second: f32, pub velocity_gain_per_second: f32,
#[config( #[config(
id = 10, id = 10,
key = "control.maximum_horizontal_acceleration_mps2", key = "control.maximum_horizontal_acceleration_mps2",
@@ -47,7 +29,7 @@ pub(super) struct ControlCandidate {
finite, finite,
range(min = 0.001, max = 100.0) range(min = 0.001, max = 100.0)
)] )]
pub(super) maximum_horizontal_acceleration_mps2: f32, pub maximum_horizontal_acceleration_mps2: f32,
#[config( #[config(
id = 11, id = 11,
key = "control.maximum_upward_acceleration_mps2", key = "control.maximum_upward_acceleration_mps2",
@@ -55,7 +37,7 @@ pub(super) struct ControlCandidate {
finite, finite,
range(min = 0.001, max = 100.0) range(min = 0.001, max = 100.0)
)] )]
pub(super) maximum_upward_acceleration_mps2: f32, pub maximum_upward_acceleration_mps2: f32,
#[config( #[config(
id = 12, id = 12,
key = "control.maximum_downward_acceleration_mps2", key = "control.maximum_downward_acceleration_mps2",
@@ -63,11 +45,43 @@ pub(super) struct ControlCandidate {
finite, finite,
range(min = 0.001, max = 100.0) 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)] #[config(post = Self::validate_thrust)]
pub(super) struct ThrustCandidate { pub struct ThrustSettings {
#[config( #[config(
id = 13, id = 13,
key = "thrust.mass_kg", key = "thrust.mass_kg",
@@ -75,7 +89,7 @@ pub(super) struct ThrustCandidate {
finite, finite,
range(min = 0.01, max = 100.0) range(min = 0.01, max = 100.0)
)] )]
pub(super) mass_kg: f32, pub mass_kg: f32,
#[config( #[config(
id = 14, id = 14,
key = "thrust.maximum_collective_thrust_n", key = "thrust.maximum_collective_thrust_n",
@@ -83,7 +97,7 @@ pub(super) struct ThrustCandidate {
finite, finite,
range(min = 0.1, max = 10_000.0) range(min = 0.1, max = 10_000.0)
)] )]
pub(super) maximum_collective_thrust_newtons: f32, pub maximum_collective_thrust_newtons: f32,
#[config( #[config(
id = 15, id = 15,
key = "thrust.maximum_tilt_tangent", key = "thrust.maximum_tilt_tangent",
@@ -91,11 +105,16 @@ pub(super) struct ThrustCandidate {
finite, finite,
range(min = 0.001, max = 10.0) 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> { 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( return Err(ConfigViolation::related(
ids::MAXIMUM_COLLECTIVE_THRUST_NEWTONS, ids::MAXIMUM_COLLECTIVE_THRUST_NEWTONS,
ids::MASS_KG, 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)] #[config(post = Self::validate_pid)]
pub(super) struct PidCandidate { pub struct PidSettings {
#[config( #[config(
id = 16, id = 16,
key = "control.rate_pid.proportional", key = "control.rate_pid.proportional",
@@ -116,7 +136,7 @@ pub(super) struct PidCandidate {
finite, finite,
range(min = 0.0, max = 10.0) range(min = 0.0, max = 10.0)
)] )]
pub(super) proportional: f32, pub proportional: f32,
#[config( #[config(
id = 17, id = 17,
key = "control.rate_pid.integral", key = "control.rate_pid.integral",
@@ -124,7 +144,7 @@ pub(super) struct PidCandidate {
finite, finite,
range(min = 0.0, max = 10.0) range(min = 0.0, max = 10.0)
)] )]
pub(super) integral: f32, pub integral: f32,
#[config( #[config(
id = 18, id = 18,
key = "control.rate_pid.derivative", key = "control.rate_pid.derivative",
@@ -132,7 +152,7 @@ pub(super) struct PidCandidate {
finite, finite,
range(min = 0.0, max = 10.0) range(min = 0.0, max = 10.0)
)] )]
pub(super) derivative: f32, pub derivative: f32,
#[config( #[config(
id = 19, id = 19,
key = "control.rate_pid.output_limit", key = "control.rate_pid.output_limit",
@@ -140,7 +160,7 @@ pub(super) struct PidCandidate {
finite, finite,
range(min = 0.001, max = 1.0) range(min = 0.001, max = 1.0)
)] )]
pub(super) output_limit: f32, pub output_limit: f32,
#[config( #[config(
id = 20, id = 20,
key = "control.rate_pid.integral_limit", key = "control.rate_pid.integral_limit",
@@ -148,9 +168,25 @@ pub(super) struct PidCandidate {
finite, finite,
range(min = 0.0, max = 1.0) 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> { fn validate_pid(&self) -> Result<(), ConfigViolation> {
if self.integral_limit > self.output_limit { if self.integral_limit > self.output_limit {
return Err(ConfigViolation::related( 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)] #[config(post = Self::validate_actuator_limits)]
pub(super) struct ActuatorLimitsCandidate { pub struct ActuatorLimits {
#[config( #[config(
id = 21, id = 21,
key = "actuator.minimum", key = "actuator.minimum",
@@ -173,7 +210,7 @@ pub(super) struct ActuatorLimitsCandidate {
finite, finite,
range(min = 0.0, max = 1.0) range(min = 0.0, max = 1.0)
)] )]
pub(super) minimum: f32, pub minimum: f32,
#[config( #[config(
id = 22, id = 22,
key = "actuator.maximum", key = "actuator.maximum",
@@ -181,7 +218,7 @@ pub(super) struct ActuatorLimitsCandidate {
finite, finite,
range(min = 0.0, max = 1.0) range(min = 0.0, max = 1.0)
)] )]
pub(super) maximum: f32, pub maximum: f32,
#[config( #[config(
id = 23, id = 23,
key = "actuator.slew_per_second", key = "actuator.slew_per_second",
@@ -189,9 +226,9 @@ pub(super) struct ActuatorLimitsCandidate {
finite, finite,
range(min = 0.001, max = 100.0) 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> { fn validate_actuator_limits(&self) -> Result<(), ConfigViolation> {
if self.minimum >= self.maximum { if self.minimum >= self.maximum {
return Err(ConfigViolation::related( return Err(ConfigViolation::related(
firmware/libs/kaptan-flight-config/src/profile/flight.rs
+88
View File
@@ -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,
}
firmware/libs/kaptan-flight-config/src/profile/ids.rs
+16 -16
View File
@@ -1,20 +1,20 @@
//! Symbolic projections used by relation checks and focused tests. //! Field IDs derived from their owning `#[config(id = ...)]` declarations.
//!
//! These constants never define an ID. Each projects the metadata generated
//! from its owning `#[config(id = ...)]` declaration.
use kaptan_config::FieldId; use kaptan_config::FieldId;
use super::{ use super::{ActuatorLimits, ControlSettings, PidSettings, ThrustSettings, TimingSettings};
control::{ActuatorLimitsCandidate, PidCandidate, ThrustCandidate},
timing::TimingCandidate,
};
pub(super) const MAXIMUM_IMU_INTERVAL_MICROSECONDS: FieldId = TimingCandidate::DESCRIPTORS[0].id; // Field IDs 6, 7 and 25 remain reserved.
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_IMU_INTERVAL_MICROSECONDS: FieldId = TimingSettings::DESCRIPTORS[0].id;
pub(super) const MAXIMUM_COLLECTIVE_THRUST_NEWTONS: FieldId = ThrustCandidate::DESCRIPTORS[1].id; pub(super) const IMU_FRESHNESS_MICROSECONDS: FieldId = TimingSettings::DESCRIPTORS[1].id;
pub(super) const RATE_PID_OUTPUT_LIMIT: FieldId = PidCandidate::DESCRIPTORS[3].id; pub(super) const LOCAL_STATE_FRESHNESS_MICROSECONDS: FieldId = TimingSettings::DESCRIPTORS[2].id;
pub(super) const RATE_PID_INTEGRAL_LIMIT: FieldId = PidCandidate::DESCRIPTORS[4].id; pub(super) const OFFBOARD_TIMEOUT_MICROSECONDS: FieldId = TimingSettings::DESCRIPTORS[3].id;
pub(super) const ACTUATOR_MINIMUM: FieldId = ActuatorLimitsCandidate::DESCRIPTORS[0].id; pub(super) const MAXIMUM_DOWNWARD_ACCELERATION_MPS2: FieldId = ControlSettings::DESCRIPTORS[4].id;
pub(super) const ACTUATOR_MAXIMUM: FieldId = ActuatorLimitsCandidate::DESCRIPTORS[1].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;
firmware/libs/kaptan-flight-config/src/profile/mavlink_logging.rs
-16
View File
@@ -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,
}
firmware/libs/kaptan-flight-config/src/profile/mod.rs
+7 -6
View File
@@ -3,13 +3,14 @@
mod active; mod active;
mod candidate; mod candidate;
mod control; mod control;
mod flight;
mod ids; mod ids;
mod mavlink_logging;
mod safety; mod safety;
mod timing; mod timing;
pub use active::{ pub use active::{VehicleCapabilities, VehicleProfile};
ActuatorLimits, AidingSettings, ControlSettings, PidSettings, SafetySettings, ThrustSettings, pub use candidate::CandidateProfile;
TimingSettings, VehicleCapabilities, VehicleProfile, pub use control::{ActuatorLimits, ControlSettings, PidSettings, ThrustSettings};
}; pub use flight::FlightSettings;
pub use candidate::{CandidateProfile, FORMAT_VERSION}; pub use safety::SafetySettings;
pub use timing::TimingSettings;
firmware/libs/kaptan-flight-config/src/profile/safety.rs
+4 -3
View File
@@ -1,7 +1,8 @@
use kaptan_config::Config; use kaptan_config::Config;
#[derive(Clone, Copy, Config)] /// Prearm timing policy, shared by editable candidates and active snapshots.
pub(super) struct SafetyCandidate { #[derive(Clone, Copy, Debug, Eq, PartialEq, Config)]
pub struct SafetySettings {
#[config( #[config(
id = 1, id = 1,
key = "safety.prearm_dwell_us", key = "safety.prearm_dwell_us",
@@ -9,5 +10,5 @@ pub(super) struct SafetyCandidate {
nonzero, nonzero,
range(min = 1, max = 10_000_000) range(min = 1, max = 10_000_000)
)] )]
pub(super) prearm_dwell_microseconds: u64, pub prearm_dwell_microseconds: u64,
} }
firmware/libs/kaptan-flight-config/src/profile/timing.rs
+19 -8
View File
@@ -2,9 +2,10 @@ use kaptan_config::{Config, ConfigViolation, ValidationRule};
use super::ids; 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)] #[config(post = Self::validate_timing)]
pub(super) struct TimingCandidate { pub struct TimingSettings {
#[config( #[config(
id = 2, id = 2,
key = "timing.imu_max_interval_us", key = "timing.imu_max_interval_us",
@@ -12,7 +13,7 @@ pub(super) struct TimingCandidate {
nonzero, nonzero,
range(min = 1, max = 1_000_000) range(min = 1, max = 1_000_000)
)] )]
pub(super) maximum_imu_interval_microseconds: u64, pub maximum_imu_interval_microseconds: u64,
#[config( #[config(
id = 3, id = 3,
key = "timing.imu_freshness_us", key = "timing.imu_freshness_us",
@@ -20,7 +21,7 @@ pub(super) struct TimingCandidate {
nonzero, nonzero,
range(min = 1, max = 1_000_000) range(min = 1, max = 1_000_000)
)] )]
pub(super) imu_freshness_microseconds: u64, pub imu_freshness_microseconds: u64,
#[config( #[config(
id = 4, id = 4,
key = "timing.local_state_freshness_us", key = "timing.local_state_freshness_us",
@@ -28,19 +29,29 @@ pub(super) struct TimingCandidate {
nonzero, nonzero,
range(min = 1, max = 10_000_000) range(min = 1, max = 10_000_000)
)] )]
pub(super) local_state_freshness_microseconds: u64, pub local_state_freshness_microseconds: u64,
#[config( #[config(
id = 5, id = 5,
key = "timing.link_freshness_us", key = "timing.offboard_timeout_us",
unit = "us", unit = "us",
nonzero, nonzero,
range(min = 1, max = 10_000_000) 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> { 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 { if self.maximum_imu_interval_microseconds > self.imu_freshness_microseconds {
return Err(ConfigViolation::related( return Err(ConfigViolation::related(
ids::MAXIMUM_IMU_INTERVAL_MICROSECONDS, ids::MAXIMUM_IMU_INTERVAL_MICROSECONDS,
firmware/libs/kaptan-flight-config/tests/control_domain.rs
+275
View File
@@ -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!(
<CandidateProfile as kaptan_config::Config>::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!(<CandidateProfile as kaptan_config::Config>::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);
}
}
firmware/libs/kaptan-flight-config/tests/support/mod.rs
+96
View File
@@ -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())
}
firmware/libs/kaptan-flight-runtime/Cargo.toml
+8 -2
View File
@@ -9,6 +9,12 @@ version.workspace = true
workspace = true workspace = true
[dependencies] [dependencies]
kaptan-config = { path = "../kaptan-config" }
kaptan-flight-config = { path = "../kaptan-flight-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" }
firmware/libs/kaptan-flight-runtime/src/controller.rs
+214
View File
@@ -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<Ned>,
attitude_gain_per_second: [f32; 3],
}
impl QuadController {
pub fn new(config: VehicleProfile) -> Result<Self, RuntimeConfigError> {
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<f32>,
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::<Ned, Ned>::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::<Ned, Ned>::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<Frd, Ned>) -> 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<Frd, Ned>) -> 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)
}
}
firmware/libs/kaptan-flight-runtime/src/execution.rs
+2147
View File
File diff suppressed because it is too large. Load diff
firmware/libs/kaptan-flight-runtime/src/lib.rs
+336 -131
View File
@@ -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 //! Callbacks must return errors, not panic. If an unwind crosses this boundary,
//! image's MAVLink diagnostic sink. MAVLink adapters receive mutable borrows //! discard the endpoint and callback owner. A halted CPU does not inhibit remote
//! for bounded dispatch or draining, but cannot own configuration lifecycle or //! outputs; independently enforced consumer expiry remains required.
//! link state.
#![no_std] #![no_std]
#![forbid(unsafe_code)]
#[cfg(test)] #[cfg(test)]
extern crate std; extern crate std;
use kaptan_config::{ConfigManager, ConfigStore, StartupError}; mod controller;
use kaptan_flight_config::{CandidateProfile, VehicleCapabilities}; mod execution;
use kaptan_log::{LogRecord, LogSinkQueue}; pub use execution::FlightEndpoint;
/// Default bounded allocation for the MAVLink diagnostic sink. Error records /// Core-owned time reserved for an admitted landing and its completion.
/// are partitioned from optional detail. pub const LANDING_BUDGET_US: u64 = 20_000_000;
type RuntimeMavlinkLogQueue = LogSinkQueue<8, 16>; 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. #[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct FlightRuntime<S> { pub struct CommandOwner(pub u64);
configuration: ConfigManager<CandidateProfile, S>, #[derive(Clone, Copy, Debug, Eq, PartialEq)]
mavlink_logs: RuntimeMavlinkLogQueue, pub struct TransportSessionId(pub u128);
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct RuntimeEpoch {
session: TransportSessionId,
generation: u64,
} }
impl RuntimeEpoch {
impl<S> FlightRuntime<S> pub const fn from_parts(session: TransportSessionId, generation: u64) -> Self {
where Self {
S: ConfigStore<CandidateProfile>, session,
{ generation,
/// 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<Self, StartupError<S::Error>> {
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<CandidateProfile, S> {
&self.configuration
} }
pub const fn session(self) -> TransportSessionId {
/// Composition-owned admission boundary for one preformatted event. self.session
pub fn publish_log_event(&mut self, record: LogRecord) {
self.mavlink_logs.publish_record(record);
} }
pub const fn generation(self) -> u64 {
/// Submits the next record and removes it only after transport admission. self.generation
pub fn submit_next_mavlink<R>(
&mut self,
submit: impl FnOnce(&LogRecord) -> (kaptan_log::TransportAdmission, R),
) -> Option<R> {
self.mavlink_logs.submit_next(submit)
} }
} }
#[derive(Clone, Copy, Debug, Eq, PartialEq, Ord, PartialOrd)]
#[cfg(test)] pub struct RequestSequence(pub u64);
mod tests { impl RequestSequence {
use kaptan_config::{ConfigModel, ConfigStore, ConfigValue}; pub const fn checked_next(self) -> Option<Self> {
use kaptan_log::TransportAdmission; match self.0.checked_add(1) {
use std::sync::Mutex; Some(v) => Some(Self(v)),
None => None,
use super::FlightRuntime;
static LOG_TEST: Mutex<()> = Mutex::new(());
struct Store;
impl ConfigStore<kaptan_flight_config::CandidateProfile> for Store {
type Error = ();
fn load(
&mut self,
_: &kaptan_flight_config::CandidateProfile,
) -> Result<Option<kaptan_flight_config::CandidateProfile>, Self::Error> {
Ok(None)
} }
fn persist(
&mut self,
_: &kaptan_flight_config::CandidateProfile,
) -> Result<(), Self::Error> {
Ok(())
}
}
#[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(())
);
}
#[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
);
} }
} }
#[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<ArmId>,
}
#[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<ArmId>,
}
/// 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<Timestamp>,
}
#[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<ArmId>,
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<ArmId> {
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<ArmId>,
pub phase: VehiclePhase,
pub phase_since: Timestamp,
pub phase_deadline: Option<Timestamp>,
pub flight_deadline: Option<Timestamp>,
pub fallback_latched: bool,
pub fallback_reason: Option<FallbackReason>,
pub terminal_reason: Option<TerminalReason>,
pub offboard: Option<RequestSequence>,
pub setpoint_sequence: Option<RequestSequence>,
pub setpoint_expires_at: Option<Timestamp>,
pub command_highwater: Option<RequestSequence>,
pub active_command: Option<CommandStatus>,
pub last_terminal_command: Option<CommandStatus>,
pub estimate: Option<FlightEstimate>,
pub ground_state: GroundState,
pub local_output_healthy: bool,
pub last_output_sequence: Option<u64>,
pub last_local_commit_at: Option<Timestamp>,
pub source_status: [SensorStatus; 5],
}
#[derive(Debug, PartialEq)]
pub struct EndpointError<E> {
pub primary: EndpointFailure<E>,
pub inhibition: Option<E>,
}
#[derive(Debug, PartialEq)]
pub enum EndpointFailure<E> {
ClockRegression,
EpochExhausted,
OutputSequenceExhausted,
NotDisarmed,
ControlDeadline,
Navigation(NavigationFailure),
Control,
Output(E),
Notification(E),
}
impl<E> From<EndpointFailure<E>> for EndpointError<E> {
fn from(primary: EndpointFailure<E>) -> Self {
Self {
primary,
inhibition: None,
}
}
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum RuntimeConfigError {
Navigation,
Control,
Timing,
Safety,
}
firmware/libs/kaptan-flight-runtime/tests/lifecycle.rs
+586
View File
@@ -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<u64>) {
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<u64>,
bounce: Option<u64>,
) {
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));
}
firmware/libs/kaptan-flight-runtime/tests/navigation.rs
+164
View File
@@ -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()));
}
firmware/libs/kaptan-flight-runtime/tests/replay.rs
+413
View File
@@ -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());
}
}
firmware/libs/kaptan-flight-runtime/tests/support/mod.rs
+173
View File
@@ -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<SensorRecord> {
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<OutputRecord>,
pub commands: Vec<CommandStatus>,
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)
}
firmware/libs/kaptan-i2c/Cargo.toml
+3
View File
@@ -7,3 +7,6 @@ version.workspace = true
[lints] [lints]
workspace = true workspace = true
[dependencies]
kaptan-time = { path = "../kaptan-time" }
firmware/libs/kaptan-i2c/src/lib.rs
+32 -6
View File
@@ -6,6 +6,8 @@
#![no_std] #![no_std]
use kaptan_time::Timestamp;
/// Terminal outcome reported by an I²C controller. /// Terminal outcome reported by an I²C controller.
#[derive(Clone, Copy, Debug, Eq, PartialEq)] #[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum I2cCompletion { pub enum I2cCompletion {
@@ -17,6 +19,18 @@ pub enum I2cCompletion {
TransferFault, 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. /// Owns a non-blocking I²C controller route and its in-flight storage.
/// ///
/// Each operation sends a STOP on successful completion. Controllers copy a /// 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 /// Releases controller ownership after a hardware ISR acknowledged the
/// terminal event and, for a successful read, copies into `read`. /// terminal event and, for a successful read, copies into `read`.
/// ///
/// This must not poll or wait. `read` must match the length submitted to /// This must not poll or wait. `NotPending` is entirely nonmutating. A
/// [`Self::start_read`] for the completed read transaction. /// successful read requires a matching destination length and controller
fn complete_transaction(&mut self, completion: I2cCompletion, read: Option<&mut [u8]>); /// 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. /// Cancels an in-flight transaction after a platform-selected timeout.
/// ///
/// Implementations must release controller-owned transaction state and /// Requests STOP without waiting. Admission remains closed until STOP is
/// arrange for any required STOP or peripheral recovery without waiting. /// proven. This is not permission to reuse or free in-flight storage.
fn cancel_transaction(&mut self); 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>;
} }
firmware/libs/kaptan-math/src/kani_proofs.rs
+26
View File
@@ -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);
}
firmware/libs/kaptan-math/src/lib.rs
+93 -164
View File
@@ -1,19 +1,18 @@
//! Minimal `f32` math shared by Kaptan navigation and control. //! Fixed-size `f32` vectors and unit quaternions with typed coordinate frames.
//!
//! 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.
#![no_std] #![no_std]
#![forbid(unsafe_code)]
use core::marker::PhantomData; use core::marker::PhantomData;
use kaptan_err_reportable::KaptanError; 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. /// 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)] #[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
pub enum MathError { pub enum MathError {
/// An operand or intermediate value was not finite. /// 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. /// A vector or quaternion did not have a representable non-zero norm.
#[kaptan_error(id = 6002)] #[kaptan_error(id = 6002)]
ZeroNorm, 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)] #[kaptan_error(id = 6003)]
RotationIncrementOutOfRange, RotationIncrementOutOfRange,
/// Rotation-matrix axes were not a finite, right-handed orthonormal basis. /// Rotation-matrix axes were not a finite, right-handed orthonormal basis.
@@ -36,11 +35,16 @@ pub enum Ned {}
/// Aircraft forward-right-down body frame. /// Aircraft forward-right-down body frame.
#[derive(Clone, Copy, Debug, Eq, PartialEq)] #[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum Frd {} 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. /// 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::<Frd>::zero();
/// let local = Vector3::<Ned>::zero();
/// let _ = body.add(local);
/// ```
#[derive(Debug, PartialEq)] #[derive(Debug, PartialEq)]
pub struct Vector3<Frame> { pub struct Vector3<Frame> {
x: f32, x: f32,
@@ -67,20 +71,16 @@ impl<Frame> Vector3<Frame> {
} }
} }
/// Returns the x component.
pub const fn x(self) -> f32 { pub const fn x(self) -> f32 {
self.x self.x
} }
/// Returns the y component.
pub const fn y(self) -> f32 { pub const fn y(self) -> f32 {
self.y self.y
} }
/// Returns the z component.
pub const fn z(self) -> f32 { pub const fn z(self) -> f32 {
self.z self.z
} }
/// Returns the zero vector.
pub const fn zero() -> Self { pub const fn zero() -> Self {
Self::new(0.0, 0.0, 0.0) Self::new(0.0, 0.0, 0.0)
} }
@@ -124,79 +124,56 @@ impl<Frame> Vector3<Frame> {
self.dot(self) self.dot(self)
} }
/// Euclidean norm using a bounded Newton iteration. /// Euclidean norm, scaled to avoid intermediate overflow and underflow.
pub fn norm(self) -> f32 { 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. /// Returns a unit vector, rejecting zero and non-finite input.
/// /// Scaling prevents overflow when the direction is representable.
/// Scaling before normalization prevents an otherwise finite, large vector
/// from overflowing while its direction is still perfectly representable.
pub fn normalized(self) -> Option<Self> { pub fn normalized(self) -> Option<Self> {
if !self.is_finite() { if !self.is_finite() {
return None; return None;
} }
let scale = self.x.abs().max(self.y.abs()).max(self.z.abs()); let scale = self.x.abs().max(self.y.abs()).max(self.z.abs());
if scale <= 1.0e-8 { if scale == 0.0 {
return None; 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()); let norm = sqrt(scaled.norm_squared());
// `scale` is a finite non-zero component, so `scaled` contains one // `scale` is a finite non-zero component, so `scaled` contains one
// component with magnitude one. Its squared norm is therefore finite // component with magnitude one. The squared norm is in [1, 3], so
// and at least one; keep that proof local rather than carrying an // multiplying components in [-1, 1] by its reciprocal root is finite.
// unreachable fallback after the public input validation above.
debug_assert!(norm.is_finite()); debug_assert!(norm.is_finite());
debug_assert!(norm > 0.0); debug_assert!(norm > 0.0);
let normalized = scaled.scale(1.0 / norm); let normalized = scaled.scale(1.0 / norm);
normalized.is_finite().then_some(normalized) debug_assert!(normalized.is_finite());
} Some(normalized)
}
/// Two `f32` components expressed in one explicit frame.
#[derive(Debug, PartialEq)]
pub struct Vector2<Frame> {
x: f32,
y: f32,
_frame: PhantomData<Frame>,
}
impl<Frame> Copy for Vector2<Frame> {}
impl<Frame> Clone for Vector2<Frame> {
fn clone(&self) -> Self {
*self
}
}
impl<Frame> Vector2<Frame> {
/// 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()
} }
} }
/// A normalized Hamilton rotation from `From` into `To`. /// A normalized Hamilton rotation from `From` into `To`.
/// ///
/// Source and destination frames are zero-sized type parameters, so a caller /// A rotation accepts vectors in its source frame.
/// cannot apply a valid quaternion to a vector in an unrelated frame. The /// ```compile_fail,E0308
/// navigation convention is body-to-NED. /// use kaptan_math::{Frd, Ned, UnitQuaternion, Vector3};
/// let body_to_local = UnitQuaternion::<Frd, Ned>::identity();
/// let _ = body_to_local.rotate(Vector3::<Ned>::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::<Frd, Ned>::identity();
/// let _ = body_to_local.compose(body_to_local);
/// ```
#[derive(Clone, Copy, Debug, PartialEq)] #[derive(Clone, Copy, Debug, PartialEq)]
pub struct UnitQuaternion<From, To> { pub struct UnitQuaternion<From, To> {
w: f32, w: f32,
@@ -220,16 +197,13 @@ impl<From, To> UnitQuaternion<From, To> {
} }
/// Normalizes raw Hamilton components, rejecting non-finite or zero input. /// Normalizes raw Hamilton components, rejecting non-finite or zero input.
/// /// Scaling prevents intermediate norm overflow.
/// Values are scaled before the norm is evaluated, so valid directions
/// with very large finite components do not overflow to an identity-like
/// result.
pub fn from_components(w: f32, x: f32, y: f32, z: f32) -> Result<Self, MathError> { pub fn from_components(w: f32, x: f32, y: f32, z: f32) -> Result<Self, MathError> {
if !w.is_finite() | !x.is_finite() | !y.is_finite() | !z.is_finite() { if !w.is_finite() | !x.is_finite() | !y.is_finite() | !z.is_finite() {
return Err(MathError::NonFinite); return Err(MathError::NonFinite);
} }
let scale = w.abs().max(x.abs()).max(y.abs()).max(z.abs()); 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); return Err(MathError::ZeroNorm);
} }
let sw = w / scale; let sw = w / scale;
@@ -314,10 +288,8 @@ impl<From, To> UnitQuaternion<From, To> {
} }
impl<Frame> UnitQuaternion<Frame, Frame> { impl<Frame> UnitQuaternion<Frame, Frame> {
/// Builds an incremental same-frame rotation from a rotation vector in /// Builds a same-frame rotation using a polynomial valid up to two radians.
/// radians. The flight estimator admits increments no larger than two /// Larger rotation vectors are rejected.
/// radians before calling this bounded polynomial, avoiding a hidden
/// `libm` dependency in the rate path.
pub fn from_small_rotation_vector(rotation: Vector3<Frame>) -> Result<Self, MathError> { pub fn from_small_rotation_vector(rotation: Vector3<Frame>) -> Result<Self, MathError> {
if !rotation.is_finite() { if !rotation.is_finite() {
return Err(MathError::NonFinite); return Err(MathError::NonFinite);
@@ -326,13 +298,21 @@ impl<Frame> UnitQuaternion<Frame, Frame> {
if !rotation_squared.is_finite() { if !rotation_squared.is_finite() {
return Err(MathError::NonFinite); return Err(MathError::NonFinite);
} }
if rotation_squared > 4.0 { if rotation_squared > MAXIMUM_ROTATION_VECTOR_NORM_SQUARED {
return Err(MathError::RotationIncrementOutOfRange); return Err(MathError::RotationIncrementOutOfRange);
} }
let half_squared = 0.25 * rotation_squared; 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 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 // The expression is close to unit length for its stated domain. A
// normalization removes accumulated polynomial/rounding error. // normalization removes accumulated polynomial/rounding error.
Self::from_components( Self::from_components(
@@ -407,10 +387,8 @@ impl UnitQuaternion<Frd, Ned> {
/// Bounded square root for positive finite `f32` values. /// Bounded square root for positive finite `f32` values.
/// ///
/// A scaled exponent estimate plus four Newton iterations provides bounded /// Uses an exponent estimate and four Newton iterations. Returns `NaN` for
/// vector/quaternion normalization precision across the finite `f32` range. /// negative or non-finite input.
/// Negative/non-finite input is rejected as `NaN` rather than silently
/// converted.
pub fn sqrt(value: f32) -> f32 { pub fn sqrt(value: f32) -> f32 {
if !value.is_finite() || value < 0.0 { if !value.is_finite() || value < 0.0 {
return f32::NAN; return f32::NAN;
@@ -418,16 +396,11 @@ pub fn sqrt(value: f32) -> f32 {
if value == 0.0 { if value == 0.0 {
return value; return value;
} }
// The exponent shortcut below assumes a normal input. Scale a // The exponent estimate requires normal input. Scale subnormals by 2^24
// subnormal by an exact power of two, then undo half that exponent after // and divide their square root by 2^12.
// the same bounded iteration. This keeps the smallest finite positive
// `f32` in the reviewed accuracy envelope without a libm dependency.
if value < f32::MIN_POSITIVE { if value < f32::MIN_POSITIVE {
return sqrt(value * 16_777_216.0) / 4_096.0; 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 estimate = f32::from_bits((value.to_bits() >> 1) + 0x1fc0_0000);
let mut iteration = 0; let mut iteration = 0;
while iteration < 4 { 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. /// Applies the SO(3) right Jacobian to a vector in one frame.
/// ///
/// The same bounded two-radian increment domain as /// Uses the two-radian domain of [`UnitQuaternion::from_small_rotation_vector`]
/// [`UnitQuaternion::from_small_rotation_vector`] avoids a hidden `libm` /// and Taylor polynomials for `(1 - cos(theta)) / theta²` and
/// dependency. The coefficients are fixed Taylor polynomials for /// `(theta - sin(theta)) / theta³`.
/// `(1 - cos(theta)) / theta²` and `(theta - sin(theta)) / theta³`.
pub fn right_jacobian_apply<Frame>( pub fn right_jacobian_apply<Frame>(
rotation: Vector3<Frame>, rotation: Vector3<Frame>,
vector: Vector3<Frame>, vector: Vector3<Frame>,
@@ -473,18 +445,21 @@ fn jacobian_apply<Frame>(
if !theta_squared.is_finite() { if !theta_squared.is_finite() {
return Err(MathError::NonFinite); return Err(MathError::NonFinite);
} }
if theta_squared > 4.0 { if theta_squared > MAXIMUM_ROTATION_VECTOR_NORM_SQUARED {
return Err(MathError::RotationIncrementOutOfRange); return Err(MathError::RotationIncrementOutOfRange);
} }
let theta_fourth = theta_squared * theta_squared; let theta_fourth = theta_squared * theta_squared;
let theta_sixth = theta_fourth * theta_squared; let theta_sixth = theta_fourth * theta_squared;
let theta_eighth = theta_fourth * theta_fourth; 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 let first_coefficient = 0.5 - theta_squared / 24.0 + theta_fourth / 720.0
- theta_sixth / 40_320.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 let second_coefficient = 1.0 / 6.0 - theta_squared / 120.0 + theta_fourth / 5_040.0
- theta_sixth / 362_880.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 cross = rotation.cross(vector);
let result = vector let result = vector
.add(cross.scale(first_cross_sign * first_coefficient)) .add(cross.scale(first_cross_sign * first_coefficient))
@@ -665,12 +640,6 @@ mod tests {
] { ] {
assert!(!invalid.is_finite()); assert!(!invalid.is_finite());
} }
let two = super::Vector2::<Frd>::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::<Frd>::new(f32::NAN, 0.0).is_finite());
} }
#[test] #[test]
@@ -750,75 +719,35 @@ mod tests {
] { ] {
assert!(!vector.is_finite()); assert!(!vector.is_finite());
} }
for components in [ for (w, x, y, z) in [
(f32::NAN, 1.0, 0.0, 0.0), (f32::NAN, 1.0, 0.0, 0.0),
(1.0, f32::NAN, 0.0, 0.0), (1.0, f32::NAN, 0.0, 0.0),
(1.0, 0.0, f32::NAN, 0.0), (1.0, 0.0, f32::NAN, 0.0),
(1.0, 0.0, 0.0, f32::NAN), (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!( assert_eq!(
UnitQuaternion::<Frd, Ned>::from_components( UnitQuaternion::<Frd, Ned>::from_components(w, x, y, z),
components.0,
components.1,
components.2,
components.3,
),
Err(MathError::NonFinite) Err(MathError::NonFinite)
); );
} }
assert_eq!( for (w, x, y, z) in [
UnitQuaternion::<Frd, Ned>::from_components(f32::INFINITY, 0.0, 0.0, 0.0), (f32::NAN, 0.0, 0.0, 0.0),
Err(MathError::NonFinite) (1.0, f32::NAN, 0.0, 0.0),
); (1.0, 0.0, f32::NAN, 0.0),
assert_eq!( (1.0, 0.0, 0.0, f32::NAN),
UnitQuaternion::<Frd, Ned>::from_components(1.0, f32::INFINITY, 0.0, 0.0), (2.0, 0.0, 0.0, 0.0),
Err(MathError::NonFinite)
);
assert_eq!(
UnitQuaternion::<Frd, Ned>::from_components(1.0, 0.0, f32::INFINITY, 0.0),
Err(MathError::NonFinite)
);
assert_eq!(
UnitQuaternion::<Frd, Ned>::from_components(1.0, 0.0, 0.0, f32::INFINITY),
Err(MathError::NonFinite)
);
for quaternion in [
UnitQuaternion::<Frd, Ned> {
w: f32::NAN,
x: 0.0,
y: 0.0,
z: 0.0,
_frames: PhantomData,
},
UnitQuaternion::<Frd, Ned> {
w: 1.0,
x: f32::NAN,
y: 0.0,
z: 0.0,
_frames: PhantomData,
},
UnitQuaternion::<Frd, Ned> {
w: 1.0,
x: 0.0,
y: f32::NAN,
z: 0.0,
_frames: PhantomData,
},
UnitQuaternion::<Frd, Ned> {
w: 1.0,
x: 0.0,
y: 0.0,
z: f32::NAN,
_frames: PhantomData,
},
UnitQuaternion::<Frd, Ned> {
w: 2.0,
x: 0.0,
y: 0.0,
z: 0.0,
_frames: PhantomData,
},
] { ] {
let quaternion = UnitQuaternion::<Frd, Ned> {
w,
x,
y,
z,
_frames: PhantomData,
};
assert!(!quaternion.is_normalized()); assert!(!quaternion.is_normalized());
} }
for axes in [ for axes in [
firmware/libs/kaptan-math/tests/so3_reference.rs
+122
View File
@@ -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::<f64>().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::<Frd>::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::<Frd>::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::<Frd, Frd>::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::<Frd>::new(scale, 0.0, 0.0).norm(), scale);
}
}
#[test]
fn norm_distinguishes_zero_invalid_input_and_unrepresentable_magnitude() {
assert_eq!(Vector3::<Frd>::zero().norm(), 0.0);
for invalid in [f32::NAN, f32::INFINITY, f32::NEG_INFINITY] {
assert!(Vector3::<Frd>::new(1.0, invalid, 2.0).norm().is_nan());
}
let large = Vector3::<Frd>::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);
}
firmware/libs/kaptan-mavlink/Cargo.toml
-4
View File
@@ -9,14 +9,10 @@ version.workspace = true
workspace = true workspace = true
[features] [features]
# Sensor-domain adapters are physical telemetry only.
sensor-telemetry = ["dep:kaptan-sensor"] sensor-telemetry = ["dep:kaptan-sensor"]
[dependencies] [dependencies]
kaptan-err-reportable = { path = "../kaptan-err-reportable" }
kaptan-dma = { path = "../kaptan-dma" }
kaptan-sensor = { path = "../kaptan-sensor", optional = true } kaptan-sensor = { path = "../kaptan-sensor", optional = true }
kaptan-tx = { path = "../kaptan-tx" }
mavlink-core = { version = "0.18", default-features = false, features = [ mavlink-core = { version = "0.18", default-features = false, features = [
"embedded", "embedded",
] } ] }
firmware/libs/kaptan-mavlink/build.rs
+2 -6
View File
@@ -5,9 +5,7 @@ fn main() {
let definitions = manifest_directory.join("definitions"); let definitions = manifest_directory.join("definitions");
let dialect = definitions.join("kaptan.xml"); let dialect = definitions.join("kaptan.xml");
println!("cargo::rerun-if-changed={}", definitions.display()); println!("cargo::rerun-if-changed={}", definitions.display());
// The upstream generator conditionally emits derives for these optional // The generator emits these feature gates, but this crate leaves them disabled.
// ecosystem crates. Kaptan deliberately does not expose those incomplete
// features, so tell rustc the generated inactive cfgs are intentional.
println!("cargo::rustc-check-cfg=cfg(feature, values(\"serde\", \"arbitrary\", \"ts-rs\"))"); println!("cargo::rustc-check-cfg=cfg(feature, values(\"serde\", \"arbitrary\", \"ts-rs\"))");
let output_directory = std::env::var("OUT_DIR").unwrap(); let output_directory = std::env::var("OUT_DIR").unwrap();
mavlink_bindgen::generate( mavlink_bindgen::generate(
@@ -15,9 +13,7 @@ fn main() {
&output_directory, &output_directory,
) )
.unwrap(); .unwrap();
// Bindgen's index feature-gates every dialect module. Kaptan has exactly // Bypass bindgen's feature-gated index to load the Kaptan dialect unconditionally.
// one unconditional dialect, so load its generated file as a normal Rust
// module instead of exposing a redundant Cargo feature.
std::fs::write( std::fs::write(
PathBuf::from(output_directory).join("kaptan_module.rs"), PathBuf::from(output_directory).join("kaptan_module.rs"),
"#[allow(non_camel_case_types, non_snake_case)]\n\ "#[allow(non_camel_case_types, non_snake_case)]\n\
firmware/libs/kaptan-mavlink/definitions/kaptan.xml
+2 -2
View File
@@ -1,7 +1,6 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<mavlink> <mavlink>
<!-- The root is the only bindgen input. Every current message has one leaf <!-- The root is the only generator input. Each message has one private leaf. -->
under its interaction contract. exchange/ intentionally has no message. -->
<include>publish/log.xml</include> <include>publish/log.xml</include>
<include>publish/error.xml</include> <include>publish/error.xml</include>
<include>publish/heartbeat.xml</include> <include>publish/heartbeat.xml</include>
@@ -10,4 +9,5 @@
<include>publish/optical_flow.xml</include> <include>publish/optical_flow.xml</include>
<include>transfer/file_control.xml</include> <include>transfer/file_control.xml</include>
<include>transfer/file_data.xml</include> <include>transfer/file_data.xml</include>
<messages/>
</mavlink> </mavlink>
firmware/libs/kaptan-mavlink/definitions/publish/error.xml
+1 -1
View File
@@ -4,7 +4,7 @@
<dialect>0</dialect> <dialect>0</dialect>
<messages> <messages>
<message id="51021" name="ERROR"> <message id="51021" name="ERROR">
<description>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.</description> <description>Reportable firmware error emitted when an Err leaves an annotated function. Logging macros emit LOG instead. Protocol failures are reported in their operation responses.</description>
<field type="uint16_t" name="error_id">Stable explicitly assigned error catalog ID. Zero is reserved.</field> <field type="uint16_t" name="error_id">Stable explicitly assigned error catalog ID. Zero is reserved.</field>
<field type="uint8_t[96]" name="raised_at">Zero-padded compile-time file:line of the reporting point.</field> <field type="uint8_t[96]" name="raised_at">Zero-padded compile-time file:line of the reporting point.</field>
</message> </message>
firmware/libs/kaptan-mavlink/definitions/publish/heartbeat.xml
+1 -1
View File
@@ -11,7 +11,7 @@
</enums> </enums>
<messages> <messages>
<message id="51027" name="HEARTBEAT"> <message id="51027" name="HEARTBEAT">
<description>Periodic liveness and discovery publication for the initialized output-inhibited firmware composition.</description> <description>Liveness and discovery while initialized with outputs inhibited.</description>
<field type="uint8_t" name="state" enum="HEARTBEAT_STATE"/> <field type="uint8_t" name="state" enum="HEARTBEAT_STATE"/>
</message> </message>
</messages> </messages>
firmware/libs/kaptan-mavlink/definitions/publish/imu.xml
+2 -2
View File
@@ -4,10 +4,10 @@
<dialect>0</dialect> <dialect>0</dialect>
<messages> <messages>
<message id="51028" name="IMU"> <message id="51028" name="IMU">
<description>One accepted body-frame inertial observation in the native bounded sensor representation.</description> <description>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.</description>
<field type="uint64_t" name="time_us">Flight-controller monotonic acquisition time in microseconds.</field> <field type="uint64_t" name="time_us">Flight-controller monotonic acquisition time in microseconds.</field>
<field type="int16_t[3]" name="acceleration_milligravity">Body-frame acceleration in milligravity.</field> <field type="int16_t[3]" name="acceleration_milligravity">Body-frame acceleration in milligravity.</field>
<field type="int16_t[3]" name="angular_rate_milliradians_per_second">Body-frame angular rate in milliradians per second.</field> <field type="int32_t[3]" name="angular_rate_milliradians_per_second">Body-frame angular rate in milliradians per second. Preserves the full MPU6000 plus or minus 2000 degrees per second decoded range.</field>
<field type="int16_t" name="temperature_centidegrees_celsius">Sensor temperature in centidegrees Celsius.</field> <field type="int16_t" name="temperature_centidegrees_celsius">Sensor temperature in centidegrees Celsius.</field>
</message> </message>
</messages> </messages>
firmware/libs/kaptan-mavlink/definitions/publish/log.xml
+1 -1
View File
@@ -21,7 +21,7 @@
</enums> </enums>
<messages> <messages>
<message id="51020" name="LOG"> <message id="51020" name="LOG">
<description>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.</description> <description>Runtime log emitted by Kaptan logging macros, including error.</description>
<field type="uint8_t" name="level" enum="LOG_LEVEL">Kaptan wire level.</field> <field type="uint8_t" name="level" enum="LOG_LEVEL">Kaptan wire level.</field>
<field type="uint8_t" name="flags" enum="LOG_FLAGS">TRUNCATED=1. Receivers ignore unrecognized bits.</field> <field type="uint8_t" name="flags" enum="LOG_FLAGS">TRUNCATED=1. Receivers ignore unrecognized bits.</field>
<field type="uint8_t" name="message_length">Number of valid UTF-8 bytes in message; at most 160.</field> <field type="uint8_t" name="message_length">Number of valid UTF-8 bytes in message; at most 160.</field>
firmware/libs/kaptan-mavlink/definitions/publish/range.xml
+1 -1
View File
@@ -5,7 +5,7 @@
<messages> <messages>
<message id="51029" name="RANGE"> <message id="51029" name="RANGE">
<description>One accepted downward range observation from the board's selected vertical sensor route.</description> <description>One accepted downward range observation from the board's selected vertical sensor route.</description>
<field type="uint64_t" name="time_us">Flight-controller monotonic measurement-request time in microseconds.</field> <field type="uint64_t" name="time_us">Producer timestamp in flight-controller monotonic microseconds: measurement request for LIDAR-Lite, frame reception for MTF-01.</field>
<field type="uint32_t" name="distance_millimetres">Measured downward distance in millimetres.</field> <field type="uint32_t" name="distance_millimetres">Measured downward distance in millimetres.</field>
</message> </message>
</messages> </messages>
Loaded 100 of 211 files, more files were not shown because too many files have changed in this diff. Show more