feat(firmware): validate configuration and migrate sensor-driven flight APIs
Validate configuration through its owning schemas and keep board declarations separate from shared F722 resource ownership. Bound USB transitions, isolate transport failures and retain restart-only activation. Migrate navigation and flight consumers with the sensor and schema contracts. Require explicit installation data and synchronous output authority; board motor outputs remain inhibited. Validation: assembled-prefix host tests, strict Clippy, platform/USB tests and both ARM release builds. Final-source coverage and scoped Miri/Kani evidence are retained separately from hardware qualification.
This commit is contained in:
Onur Özkan committed
1 parent
fb1b8af2d1
commit
f9a61a086f
211 files changed
+45519
-12602
No files matched your search
+153
-55
@@ -12,22 +12,72 @@ F722 platform / RTIC application ──► data channel ──► MAVLink teleme
|
||||
├── physical sensor drivers
|
||||
└── startup motor-pin inhibition
|
||||
|
||||
portable libraries
|
||||
math ─► navigation ─► control ─► safety
|
||||
Portable flight libraries (not connected to physical actuators)
|
||||
FlightEndpoint ──► Navigation / control / phases / safety
|
||||
│
|
||||
└── caller-owned synchronous output sink
|
||||
```
|
||||
|
||||
Each F722 board contract pairs a board-owned `UserConfig` schema and explicit
|
||||
`FACTORY_USER_CONFIG` with a board-owned `ImmutableConfig`. The immutable root
|
||||
is one literal typestate manifest and the sole source of output inhibition,
|
||||
physical routes, driver setup, sensor mount, bootstrap line rates, status LED,
|
||||
image role, and service frequency. The user root contains only settings
|
||||
consumed by that compiled driver graph.
|
||||
Each F722 board combines `F722UserConfig` with an immutable hardware manifest.
|
||||
The manifest selects the IMU route, driver settings, mount, serial inventory,
|
||||
status LED, optional I²C sensor, motor pins and service frequency. User settings
|
||||
select UART roles, baud and MAVLink settings. Motor pins are inhibited at startup.
|
||||
|
||||
The data channel owns rate selection, mailbox consumption, message encoding,
|
||||
and bounded TX classification. Board code supplies physical routes, target
|
||||
schema, and factory values; it does not receive generic output authority.
|
||||
`F722ImmutableConfig<R>` retains the IMU route type through initialization and
|
||||
interrupt acknowledgement. The sealed `Mpu6000Spi1` and `Icm42688Spi3` routes
|
||||
select IMU wiring and backend independently of serial inventory and LED wiring.
|
||||
|
||||
Configuration and runtime observability are separate planes:
|
||||
Each board declares one `PortCatalog<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
|
||||
FILE_CONTROL / FILE_DATA
|
||||
@@ -42,50 +92,97 @@ kaptan-file-mavlink ──► virtual FileProvider
|
||||
kaptan-log / kaptan-err-reportable
|
||||
│
|
||||
▼
|
||||
kaptan-diagnostics-mavlink ──► platform-owned encoder / DMA TX
|
||||
kaptan-diagnostics-mavlink ──► per-link encoder / bounded TX
|
||||
```
|
||||
|
||||
The file dialect is intentionally Kaptan-native rather than standard MAVLink
|
||||
FTP, `PARAM_*`, or a QGC adapter. Its two messages know only bounded paths,
|
||||
bytes, positions, checksums, transfer ownership, and retries. The configuration
|
||||
provider maps `/config/user.kcfg` to a canonical self-describing artifact.
|
||||
KCFG carries generated metadata and a complete committed value set; the target
|
||||
streams it without retaining a whole-file buffer. Saving imports and validates
|
||||
one complete replacement candidate. The core distinguishes only immutable
|
||||
boot-active values from the latest committed values. A save cannot mutate
|
||||
controller or estimator state in flight.
|
||||
The Kaptan-native file dialect owns bounded transfers and retries across links.
|
||||
Its provider maps saved, active and factory-reset endpoints to streaming KCFG v3
|
||||
and the generic configuration lifecycle.
|
||||
|
||||
`ConfigManager` owns boot-active values, known committed values or unknown durable
|
||||
state, and the factory candidate. F722 supplies its UART-disabled setup fallback
|
||||
through `recover_with`. Readable invalid settings remain inspectable for USB
|
||||
repair. Saving or factory reset never changes active consumers until reboot.
|
||||
|
||||
Target-owned stores use private persistence formats. F722 alternates compact
|
||||
`KCSP` snapshots between two erase sectors, retaining only the old and
|
||||
replacement records across the commit boundary. That format is not exposed as the public KCFG file.
|
||||
Motor allocation/mixer topology is immutable compiled vehicle capability data,
|
||||
never a scalar field or persistence member. F722 recovers operational settings
|
||||
before constructing consumers. Its local USB CDC-ACM and directly wired UART
|
||||
accept only exact-target, CRC-valid Kaptan file requests while physical outputs
|
||||
remain inhibited. CRC is integrity-only; a shared or remote GCS still needs
|
||||
production authentication, source binding, and persistent anti-replay state.
|
||||
Disarmed activation and flight-consumer reconstruction remain separate reviewed
|
||||
work.
|
||||
`KCSP` snapshots between two erase sectors, retaining only the old and replacement
|
||||
records across the commit boundary. That format is not exposed as KCFG. Motor
|
||||
allocation/mixer topology remains compiled vehicle capability data.
|
||||
|
||||
The MAVLink dialect root directly includes one-message XML leaves under
|
||||
`publish/` and `transfer/`. The `exchange/` contract remains empty until a
|
||||
current consumer justifies a message. There are no category index XML files or
|
||||
separate enum catalogs. The build watches the definitions tree and generates
|
||||
one `generated::kaptan` module; leaves are never separately generated Rust
|
||||
modules. `kaptan-log` itself has
|
||||
no MAVLink, platform, time, allocation, or filesystem dependency; its sole
|
||||
configuration coupling is compile-time enum metadata for `LevelFilter`.
|
||||
`kaptan-err-reportable` independently owns explicitly assigned reportable-error
|
||||
identity and a bounded nonblocking ingress; it has no logging, MAVLink,
|
||||
platform, filesystem, or clock dependency. Event creation is independent from
|
||||
sink filtering, allowing a future blackbox and MAVLink to select different
|
||||
policies. The platform drains reportable errors before its bounded critical and
|
||||
best-effort log queues and removes a pending record only after TX admission.
|
||||
`LOG` carries one conventional level plus one bounded text field; `ERROR`
|
||||
carries an explicitly assigned 16-bit ID and the bounded `file:line` of the
|
||||
reporting point. Its message kind already identifies a reportable firmware
|
||||
error. Kaptan-native heartbeat and sensor publications remain separate from
|
||||
diagnostics.
|
||||
## I/O ownership
|
||||
|
||||
Sensor handoff uses owned `LatestMailbox<T>` storage split into exactly one
|
||||
non-cloneable producer and one non-cloneable consumer. Publishing replaces an
|
||||
unread value, and taking removes it at most once. Each operation copies or takes
|
||||
an `Option<T>` inside the platform critical section. No callback, allocation,
|
||||
retry or wait occurs inside that section. Storage stays borrowed while endpoints
|
||||
are in use, and moving, dropping or forgetting an endpoint holds no lock. The
|
||||
backend must exclude every participating context, using Cortex-M single-core exclusion on
|
||||
F722 and the `critical-section/std` backend for host threads.
|
||||
|
||||
UART TX and RX are interrupt-driven, without UART DMA. Each I/O owner retains
|
||||
the peripheral, TX queue and a 512-byte RX buffer with a typed `Timestamp` per
|
||||
byte captured at IRQ receipt. The worker takes at most 16 bytes per UART per
|
||||
turn under a short lock. Protocol parsing, scheduling and persistence run
|
||||
outside UART locks. Overflow or a hardware RX fault discards the damaged backlog
|
||||
and resets the decoder before later bytes. These bounds do not establish a
|
||||
measured latency guarantee.
|
||||
|
||||
`kaptan-tx::TransmitQueue` owns bounded admission and one logically active frame.
|
||||
Pending admission cannot replace that active frame. The queue itself is movable
|
||||
and does not promise DMA address stability. UART byte writes and USB endpoint
|
||||
writes borrow or copy its bytes synchronously. MAVLink owns encoding, while
|
||||
platform I/O owners handle transmission.
|
||||
|
||||
USB uses the repository-patched Synopsys driver. Runtime register waits allow
|
||||
higher-priority interrupts and stop after 5 ms or 1,080,000 polls per wait. These
|
||||
are fault limits, not measured execution times. A fault holds USB in peripheral
|
||||
reset until restart and discards its queued data. Brief critical sections protect
|
||||
USB ownership and endpoint buffers. Boot initialization still runs under RTIC's
|
||||
startup interrupt mask.
|
||||
|
||||
SPI1 and SPI3 DMA each use a once-claimed static pair of 32-byte TX/RX buffers.
|
||||
`start_transfer` copies in, so hardware retains no caller slice. Completion
|
||||
copies out only after checking stream disable, SPI quiescence, transfer counts
|
||||
and faults. Failure to establish shutdown or drain RX quarantines the storage
|
||||
and prevents reuse. Moving, dropping or forgetting the owner never releases
|
||||
that claim. The F722 implementation relies on DMA-accessible SRAM, disabled
|
||||
D-cache and device barriers. Physical SPI shutdown, timing and fault behavior
|
||||
still require hardware validation.
|
||||
|
||||
## Monotonic time
|
||||
|
||||
TIM2 is the canonical F722 clock. The configured 108-MHz timer input and PSC=107
|
||||
produce a free-running 32-bit counter at 1 MHz with ARR=`u32::MAX`. All readers
|
||||
and the CC1 service IRQ extend one shared `u64` microsecond epoch inside the same
|
||||
critical section. A consumer may remain silent across wraps if other readers
|
||||
or IRQ service keep extending that epoch. Combined observations must be strictly
|
||||
less than 2^32 microseconds apart, about 71.58 minutes, including interrupt
|
||||
blackouts and debugger halts. NMI and fault handlers must not access this clock.
|
||||
|
||||
CC1 schedules the current 100-Hz service at epoch-aligned 10-ms deadlines.
|
||||
Delayed service skips missed periods in constant work and emits at most one
|
||||
notification per IRQ. Rearming uses two bounded attempts before pending a
|
||||
recovery invocation. Callbacks and notifications run after clock exclusion ends.
|
||||
TIM2 is not reset for each service tick, and no update IRQ or timer DMA is used.
|
||||
Its IRQ is enabled only at the successful end of initialization.
|
||||
|
||||
Ordinary WFI retains the timer clock. Deep sleep, STOP/Standby, clock-tree changes,
|
||||
counter reset/regression and debugger halts spanning a full wrap are unsupported
|
||||
and require platform reset. Uptime must leave one service period before `u64`
|
||||
microsecond overflow. DWT is used only for the CPU-running SRAM flash-poll timeout
|
||||
at 216 MHz, not sensor timestamps or service deadlines. These contracts do not
|
||||
establish measured oscillator accuracy, IRQ latency, jitter or worst-case timing.
|
||||
|
||||
## Diagnostics
|
||||
|
||||
`kaptan-log` has no MAVLink, platform, time, allocation or filesystem dependency.
|
||||
Its configuration coupling is compile-time enum metadata for `LevelFilter`.
|
||||
`kaptan-err-reportable` independently owns explicit error identities and a bounded
|
||||
nonblocking ingress, without logging, transport or clock dependencies.
|
||||
Event creation is independent from sink filtering. The worker drains reportable
|
||||
errors before logs and offers each diagnostic to all MAVLink links. Heartbeat
|
||||
and sensor publications use separate scheduling.
|
||||
|
||||
See [Configuration](configuration.md) for KCFG and the file-transfer contract,
|
||||
[Logs](logs.md) for the receiver contract, and [Errors](errors.md)
|
||||
@@ -96,7 +193,8 @@ for derived-error and failure-reporting scope.
|
||||
- A peripheral owner is local to the platform runtime.
|
||||
- Portable crates use typed units and validated values rather than raw wire or
|
||||
timer representations.
|
||||
- A safety permit is epoch-bound and must be revalidated at the only nonzero
|
||||
output boundary.
|
||||
- A failed observation or numerical check does not partially commit estimator,
|
||||
controller, allocator, or safety state.
|
||||
- `SafetySupervisor::evaluate_and_commit` checks current evidence at the
|
||||
synchronous enabled-output boundary. No transferable permit escapes it.
|
||||
- Rejected numerical updates do not partially commit computational candidates.
|
||||
Fault latches, inhibition and ingress replay watermarks follow their explicit
|
||||
failure contracts rather than rolling back authority or replay history.
|
||||
@@ -1,19 +1,13 @@
|
||||
# GEPRC GEP-F722-HD V2
|
||||
|
||||
The GEPRC GEP-F722-HD V2 board declaration uses the ICM-42688-P on SPI3 and
|
||||
MAVLink telemetry on USB CDC-ACM and USART2. It declares no vertical sensor stream. The
|
||||
manufacturer's [GEP-F722-HD V2 manual](https://geprc.com/wp-content/uploads/2025/05/GEP-F722-HD-v2%E9%A3%9E%E6%8E%A7_%E4%BA%A7%E5%93%81%E8%AF%B4%E6%98%8E%E4%B9%A6-.pdf)
|
||||
labels the external UART2 pads `T2` and `R2`.
|
||||
The GEPRC GEP-F722-HD V2 uses the ICM-42688-P on SPI3.
|
||||
The manufacturer's [GEP-F722-HD V2 manual](https://geprc.com/wp-content/uploads/2025/05/GEP-F722-HD-v2%E9%A3%9E%E6%8E%A7_%E4%BA%A7%E5%93%81%E8%AF%B4%E6%98%8E%E4%B9%A6-.pdf)
|
||||
labels UART1 as `T1`/`R1` and UART2 as `T2`/`R2`.
|
||||
|
||||
Its board-local `UserConfig` factory selects endpoint `1/1`, IMU and
|
||||
heartbeat publication rates, and an enabled MAVLink diagnostic sink with an
|
||||
`Info` level filter. The web configurator may replace those
|
||||
target-advertised values in the journal. Range
|
||||
and optical-flow fields are absent because this image has no vertical-sensor
|
||||
route. Its `ImmutableConfig` keeps the ICM-42688-P route, yaw-180° mount,
|
||||
USART2 pins and baud, active-high PA13 status LED, and PB0/PB1/PB4/PB5 output
|
||||
inhibition outside the writable schema. These selections appear together in
|
||||
the board's immutable typestate literal.
|
||||
The board declares its yaw-180° IMU mount, UART catalog, active-high PA13 LED
|
||||
and no I²C range sensor.
|
||||
Before fallible initialization, the platform holds the four documented motor
|
||||
pins PB0/PB1/PB4/PB5 low.
|
||||
|
||||
## Build and package
|
||||
|
||||
@@ -53,9 +47,10 @@ make firmware/tools/configurator
|
||||
Open `http://127.0.0.1:4861` and connect `/dev/serial/by-id/<kaptan-device>` or
|
||||
the discovered `/dev/ttyACM*` path from the Connection section.
|
||||
|
||||
## UART fallback
|
||||
## UART connection
|
||||
|
||||
Connect a 3.3 V USB-UART adapter with crossed data lines and common ground:
|
||||
First assign UART2 to `KaptanMavlink` over USB, save and restart. Then connect
|
||||
a 3.3 V USB-UART adapter with crossed data lines and common ground:
|
||||
|
||||
| Adapter | Flight controller |
|
||||
| --- | --- |
|
||||
@@ -65,11 +60,7 @@ Connect a 3.3 V USB-UART adapter with crossed data lines and common ground:
|
||||
|
||||
Enter `/dev/serial/by-id/<adapter>` in the same configurator's Connection section.
|
||||
|
||||
Open `http://127.0.0.1:4861`, Save the discovered fields, and manually
|
||||
power-cycle the controller after Save completes. The flash journal is outside
|
||||
the application image, so the exact range-limited DFU command above preserves
|
||||
it. A mass erase or full-chip erase removes committed settings and restores
|
||||
board factory defaults at the next boot. The image remains telemetry-only and
|
||||
applies startup motor-pin inhibition. The unsigned local configuration path
|
||||
assumes one directly connected client; disconnect every other transmitter from
|
||||
`R2` while configuring over UART.
|
||||
Use UART2's boot-active baud. If its role was changed or recovery disabled it,
|
||||
connect over normal USB. Follow [save/reset/reload](../configuration.md#web-configurator)
|
||||
before expecting new settings to apply. Disconnect other transmitters from `R2`
|
||||
while configuring. Physical motor control is not yet implemented.
|
||||
@@ -3,10 +3,20 @@
|
||||
Board pages describe supported hardware topology, build products, and safe
|
||||
verification. They do not declare a board flight-ready.
|
||||
|
||||
| Board | MCU family | Primary IMU | MAVLink links |
|
||||
| Board | IMU route | Serial inventory | Factory UART roles |
|
||||
| --- | --- | --- | --- |
|
||||
| [Kakute F7 Mini V3](kakute-f7-mini-v3.md) | STM32F722 | MPU-6000 | USB CDC-ACM, USART1 |
|
||||
| [GEPRC GEP-F722-HD V2](geprc-f722-v2.md) | STM32F722 | ICM-42688-P | USB CDC-ACM, USART2 |
|
||||
| [Kakute F7 Mini V3](kakute-f7-mini-v3.md) | `Mpu6000Spi1` | UART1, UART2, UART3, UART4, UART6 | All disabled |
|
||||
| [GEPRC GEP-F722-HD V2](geprc-f722-v2.md) | `Icm42688Spi3` | UART1, UART2, UART3, UART4, UART6 | All disabled |
|
||||
|
||||
USB CDC-ACM always uses Kaptan MAVLink and has no configurable role. ROM DFU is a separate flashing mode. UART roles and baud are
|
||||
restart-only user settings. Both factories use 115200 baud, endpoint `1/1`,
|
||||
20-Hz IMU, 2-Hz heartbeat, 20-Hz range/flow settings and the `Info` log filter.
|
||||
Range/flow publications require an enabled sensor source.
|
||||
|
||||
See [Configuration](../configuration.md) for role choices, supported baud values,
|
||||
save/reset/reload and invalid-storage USB repair. The range-limited DFU workflows
|
||||
on these pages preserve the journal outside the application image. Mass erase
|
||||
removes saved settings and selects factory values at the next boot.
|
||||
|
||||
All board images share the motor-pin inhibit boundary described in
|
||||
[Safety boundary](../safety-boundary.md).
|
||||
@@ -1,20 +1,14 @@
|
||||
# Kakute F7 Mini V3
|
||||
|
||||
The Holybro Kakute F7 Mini V3 board declaration uses the MPU-6000 on SPI1,
|
||||
MAVLink telemetry on USB CDC-ACM and USART1, and the MTF01 range/optical-flow
|
||||
route on UART4.
|
||||
The Holybro Kakute F7 Mini V3 uses the MPU-6000 on SPI1.
|
||||
The checked-in manufacturer datasheet is
|
||||
`firmware/boards/kakute-f7-mini-v3/datasheet.pdf`.
|
||||
|
||||
Its board-local `UserConfig` factory selects endpoint `1/1`, IMU, range,
|
||||
optical-flow and heartbeat publication rates, and an enabled MAVLink diagnostic
|
||||
sink with an `Info` level filter. The web configurator may replace those
|
||||
target-advertised values in the journal. Its `ImmutableConfig` keeps the
|
||||
MPU-6000 route, body-aligned
|
||||
mount and concrete driver setup, USART1 pins and baud, MTF01 route and baud,
|
||||
active-low PD2 status LED, and PB0/PB1/PB4/PB5 output inhibition outside the
|
||||
writable schema. These selections appear together in the board's immutable
|
||||
typestate literal.
|
||||
The board maps MPU axes `(X, Y, Z)` to `(-Y, X, Z)`, matching the manufacturer's
|
||||
`KAKUTEF7MINIV3` target, and supplies the driver settings. It declares the UART
|
||||
catalog, active-low PD2 LED and no I²C range sensor.
|
||||
Before fallible initialization, the platform holds all six documented
|
||||
motor pins PB0/PB1/PB4/PB5/PB6/PB7 low.
|
||||
|
||||
## Build and package
|
||||
|
||||
@@ -59,9 +53,10 @@ make firmware/tools/configurator
|
||||
Open `http://127.0.0.1:4861` and connect `/dev/serial/by-id/<kaptan-device>` or
|
||||
the discovered `/dev/ttyACM*` path from the Connection section.
|
||||
|
||||
## UART fallback
|
||||
## UART connection
|
||||
|
||||
Use a 3.3 V USB-to-UART adapter with crossed data lines and common ground:
|
||||
First assign UART1 to `KaptanMavlink` over USB, save and restart. Then connect
|
||||
a 3.3 V USB-UART adapter with crossed data lines and common ground:
|
||||
|
||||
| Adapter | Flight controller |
|
||||
| --- | --- |
|
||||
@@ -71,11 +66,7 @@ Use a 3.3 V USB-to-UART adapter with crossed data lines and common ground:
|
||||
|
||||
Enter `/dev/serial/by-id/<adapter>` in the same configurator's Connection section.
|
||||
|
||||
Open `http://127.0.0.1:4861`, edit the discovered fields, press Save, and
|
||||
manually power-cycle the controller after Save completes. The flash journal is
|
||||
outside the application image, so the exact range-limited DFU command above
|
||||
preserves it. A mass erase or full-chip erase removes committed settings and
|
||||
restores board factory defaults at the next boot. The image remains
|
||||
telemetry-only and does not enable motor control. The unsigned local
|
||||
configuration path assumes one directly connected client; disconnect every
|
||||
other transmitter from `R1` while configuring over UART.
|
||||
Use the UART's boot-active baud. If its role was changed or recovery disabled
|
||||
UARTs, connect over normal USB. Follow [save/reset/reload](../configuration.md#web-configurator)
|
||||
before expecting new settings to apply. Disconnect other transmitters from the
|
||||
selected RX pad while configuring. Physical motor control is not yet implemented.
|
||||
+221
-106
@@ -1,16 +1,56 @@
|
||||
# Configuration
|
||||
|
||||
Kaptan has one generic configuration model in `kaptan-config`. Target schemas
|
||||
declare typed fields and validation once; derive code generates the stable
|
||||
field registry and schema fingerprint. Boards supply the sole erased-flash
|
||||
factory values. The browser contains no board, driver, sensor, schema, or
|
||||
parameter catalog.
|
||||
`kaptan-config` owns typed fields, validation and the committed-versus-active
|
||||
lifecycle. The F722 platform supplies the shared user schema and factory values.
|
||||
Boards declare their UART catalog and immutable hardware context. The
|
||||
configurator renders target metadata without a board or parameter catalog.
|
||||
|
||||
## Current deployment matrix
|
||||
## Flight configuration
|
||||
|
||||
| Deployment | Configuration consumed today | Remote writes |
|
||||
`kaptan-flight-config::CandidateProfile` owns the flight policy, gains, thrust
|
||||
model, navigation noise densities and timing constraints. It is consumed by the
|
||||
portable runtime and its tests. The board firmware uses its own board schema
|
||||
and **KCFG v3** service.
|
||||
Field IDs **6, 7 and 25** remain reserved.
|
||||
The other field IDs retain their meanings and numbering.
|
||||
|
||||
Construct the candidate's raw sections and supply a `NavigationInstallation`
|
||||
through `VehicleCapabilities::quad_x`. It declares sources, calibration and
|
||||
reference identities, range mounting and quantization, contact geometry,
|
||||
magnetic field and local pressure/temperature. Complete validation produces
|
||||
the immutable `VehicleProfile`. The current runtime supports Quad-X allocation
|
||||
and four equal contact spheres on a body-frame rectangle.
|
||||
|
||||
The current controller descriptors include fields 41..44:
|
||||
|
||||
| ID | Key | Meaning |
|
||||
| --- | --- | --- |
|
||||
| STM32F722 image | Board factory settings, or the newest valid flash-journal snapshot. | `/config/user.kcfg` over local USB CDC-ACM or the directly wired UART while motor pins remain inhibited. |
|
||||
| 41 | `control.attitude_roll_pitch_gain_per_s` | Roll/pitch attitude gain in 1/s. |
|
||||
| 42 | `control.attitude_yaw_gain_per_s` | Yaw attitude gain in 1/s. |
|
||||
| 43 | `control.yaw_rate_pid.proportional` | Yaw rate proportional gain. |
|
||||
| 44 | `control.yaw_rate_pid.integral` | Yaw rate integral gain. |
|
||||
|
||||
Roll/pitch rate gains use the rate-PID fields. Derivative and contribution limits
|
||||
are shared.
|
||||
|
||||
Use the generated `kaptan-flight-config::SCHEMA_ID` from the source/build being
|
||||
used. Schema identity describes the field contract; it does not qualify a
|
||||
physical vehicle or identify calibrated values.
|
||||
|
||||
The test fixture has a 10-ms control period, 20-ms control deadline, 250-ms
|
||||
maximum command-admission TTL, 300-ms offboard stream timeout and one-second
|
||||
fallback hold. Its configured output lifetime ceiling is 50 ms, further limited
|
||||
by the 20-ms control deadline and live evidence. The navigation time-error
|
||||
ceiling is 2000 us. These are library contracts, not measured F722 timing
|
||||
guarantees or a qualified aircraft profile.
|
||||
|
||||
Navigation owns `MAXIMUM_FREE_ACCELERATION_MPS2 = 25`. Complete-profile
|
||||
validation checks maximum collective thrust `T_max <= m * (25 - g)` using `f64`
|
||||
arithmetic, and requires thrust above weight. The bound applies to free flight,
|
||||
not contact impulses. Mixer and installation remain compiled capabilities.
|
||||
|
||||
The 1.2-kg/18-N fixture and its installation values exist only in test support.
|
||||
They do not calibrate physical sensors or identify a propulsion system.
|
||||
|
||||
## Web configurator
|
||||
|
||||
@@ -27,155 +67,230 @@ as `/dev/serial/by-id/<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
|
||||
path instead.
|
||||
|
||||
The web tool defaults to MAVLink source system `255` and baud `115200`.
|
||||
`--source-system` and `--baud` remain optional command-line overrides. Every
|
||||
simultaneously connected GCS must use a different nonzero system ID so transfer
|
||||
ownership and retries cannot alias another client.
|
||||
The tool defaults to MAVLink source system `255` and baud `115200`.
|
||||
`--source-system` and `--baud` are command-line overrides. Match the adapter baud
|
||||
to the UART's boot-active value. For example, from `firmware/`, launch
|
||||
`cargo run -p kaptan-configurator --locked --offline -- --baud 230400`.
|
||||
Peers sharing a link need distinct nonzero source identities, but those
|
||||
identities do not authenticate them.
|
||||
|
||||
Open `http://127.0.0.1:4861`. On discovery, the configurator downloads the
|
||||
complete `/config/user.kcfg` artifact, validates it, and renders the metadata
|
||||
and values contained in that file. Save produces one complete edited KCFG
|
||||
artifact and uploads it. The target validates the complete candidate before
|
||||
atomically persisting it. The boot-active runtime does not change; manually
|
||||
power-cycle the controller after a successful save when the UI reports that a
|
||||
restart is required.
|
||||
1. Connect and wait for discovery to read both saved and boot-active artifacts.
|
||||
Each field shows **Boot-active**, **Saved** and **Draft** values.
|
||||
2. Edit the draft and press **Save**. The target checks the downloaded baseline,
|
||||
validates the complete replacement and persists it. After success, the tool
|
||||
reloads both artifacts to establish the next save's baseline.
|
||||
3. To replace saved settings with the target's factory values, press
|
||||
**Reset saved configuration**. This is a persistence operation, not a reboot.
|
||||
It also works when saved state is unknown, provided active metadata was read.
|
||||
4. Save and factory reset leave active roles, baud, identity, rates and logging
|
||||
unchanged. When restart is required, manually reset or power-cycle the board,
|
||||
then reconnect and check the reloaded values. A UART may now use another baud
|
||||
or no longer carry MAVLink. Normal USB remains the configuration route.
|
||||
|
||||
Saving erases one 128-KiB STM32F7 journal sector synchronously. Expect roughly
|
||||
one second without telemetry or sensor servicing while the target flash
|
||||
controller is busy; the host gives the final close a separate five-second
|
||||
timeout. Keep the vehicle unpowered except for setup power and do not disconnect
|
||||
it during Save.
|
||||
A stale baseline returns `REQUEST_CONFLICT` without writing flash. The tool
|
||||
discards that draft. Disconnect and reconnect to reload before editing again.
|
||||
If saved state is unknown, ordinary saves are blocked. Reconnect to inspect it,
|
||||
then use factory reset or reboot recovery as described below.
|
||||
|
||||
The artifact carries generated field IDs, native scalar kinds, display keys,
|
||||
units, bounds, enum choices, factory values, committed values, schema identity,
|
||||
and the derived restart-required state. A new board or field therefore needs no
|
||||
configurator release. `u8`, `u16`, and enum-backed values retain their native
|
||||
width rather than being widened at the configuration boundary.
|
||||
A changed save or explicit factory reset synchronously erases a 128-KiB journal
|
||||
sector. Flash activity can interrupt telemetry and sensor servicing. The host
|
||||
allows five seconds for the final close, which is not a measured erase-time
|
||||
guarantee. Keep setup power connected until the operation completes. An unchanged
|
||||
ordinary replacement skips flash writes.
|
||||
|
||||
The Incoming MAVLink table groups live, CRC-valid Kaptan packets by source and
|
||||
message ID and shows receive frequency and total count. Selecting a message
|
||||
watches its latest decoded value. The separate Runtime logs view retains
|
||||
individual `LOG` and `ERROR` events in receive order, with local receive time,
|
||||
level, and readable text.
|
||||
See [MAVLink](mavlink.md) and [Logs](logs.md) for the packet and runtime-log views.
|
||||
|
||||
## Board ownership
|
||||
|
||||
Each F722 board declares one `UserConfig` root for the settings consumed by its
|
||||
compiled driver graph. The explicit `FACTORY_USER_CONFIG` literal is the sole
|
||||
erased-flash value. There is no `Default` implementation and no platform-wide
|
||||
union of board schemas.
|
||||
Each F722 board selects `F722UserConfig<Catalog, N>`. The shared factory values
|
||||
leave every configurable UART disabled. USB always carries Kaptan MAVLink and
|
||||
has no configurable role. Board catalogs determine which UART fields exist.
|
||||
|
||||
Both current boards expose MAVLink identity, IMU and heartbeat publication
|
||||
rates, and the MAVLink log level filter. Kakute also exposes range and
|
||||
optical-flow rates because its immutable hardware manifest contains an MTF01.
|
||||
Those fields are absent from GEPRC rather than present-but-disabled.
|
||||
Both current boards expose MAVLink identity, publication rates, log filtering,
|
||||
and `ports` fields from `kaptan-ports`. Range and optical-flow rates are present
|
||||
on both boards because either can assign an inventoried UART to MTF01. Their
|
||||
streams are enabled only when the selected sensor service supplies them.
|
||||
|
||||
The adjacent `IMMUTABLE_CONFIG` literal remains the sole source of facts that
|
||||
cannot safely be edited as scalar configuration:
|
||||
The adjacent `IMMUTABLE_CONFIG` manifest holds:
|
||||
|
||||
- output-inhibit and status-LED routes;
|
||||
- IMU model, bus, interrupt route, mount, and driver setup;
|
||||
- MAVLink UART and optional vertical-sensor routes and bootstrap baud rates;
|
||||
- the fixed F722 USB CDC-ACM transport;
|
||||
- service-timer frequency; and
|
||||
- compiled image role.
|
||||
- the IMU route, buses and interrupt wiring
|
||||
- sensor mount and driver setup
|
||||
- the available serial inventory, status LED and optional I²C range route
|
||||
- service-timer frequency.
|
||||
|
||||
The F722 platform consumes stable semantic field IDs through the generic
|
||||
`Config::field_value` boundary. It does not know a board section, Rust field,
|
||||
or aggregate shape. Whole-candidate validation checks both generated local
|
||||
rules and relationships such as publication budget and immutable hardware
|
||||
capabilities.
|
||||
Motor pins are inhibited at startup. The board images have no physical actuator
|
||||
driver.
|
||||
|
||||
UART roles and baud are writable settings applied only at boot. Each inventoried
|
||||
UART supports `Disabled = 0`, `KaptanMavlink = 1` (`Role::Mavlink`), and
|
||||
`Mtf01 = 2`. Disabled ports do not initialize their peripheral or pins.
|
||||
One board-owned `PortCatalog<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 is the public, canonical import/export representation. It is independent
|
||||
of MAVLink and of the private storage format. A file contains:
|
||||
|
||||
- `KCFG` magic and one format version;
|
||||
- generated 16-byte schema identity and exact field count;
|
||||
- one canonical self-describing record for every field; and
|
||||
- `KCFG` magic, format version **3** and a restart flag
|
||||
- generated 16-byte schema identity and exact field count
|
||||
- one record per field with ID, native kind, requested/current value, factory
|
||||
value and expected-base value at native width, plus field flags, bounds,
|
||||
display key, unit and enum choices
|
||||
- a big-endian CRC-32C trailer covering the preceding artifact bytes.
|
||||
|
||||
The trailer uses network byte order deliberately. The generic file dialect
|
||||
also calculates CRC-32C over every complete file byte; network-order storage
|
||||
keeps that independent transfer checksum content-specific instead of producing
|
||||
the fixed residue obtained by appending a reflected CRC in little-endian order.
|
||||
Exports put the selected view's value in both current and expected-base slots.
|
||||
Edits change only requested values. On ordinary save, the manager compares every
|
||||
expected-base scalar's native kind and bits with current committed values before
|
||||
whole-candidate validation, no-op detection or persistence. This is a full-value
|
||||
compare-and-swap (CAS). Native-kind-and-bit equality also governs no-op and
|
||||
restart comparisons. Positive and negative floating-point zero differ, while
|
||||
identical NaN payload bits compare equal when the field's rules allow NaN.
|
||||
It is not a CRC, generation counter or history check. A change away from and
|
||||
back to the same values still matches.
|
||||
|
||||
Field flag bits 0 and 1 indicate minimum and maximum bounds. Bit 2 is `finite`
|
||||
and bit 3 is `nonzero`. Other bits are reserved zero. These constraints are
|
||||
independent: `finite` rejects NaN and infinities, while `nonzero` rejects numeric
|
||||
zero, including both floating-point signed zeros. `nonzero` alone does not imply
|
||||
finite values. Any declared floating-point range rejects NaN as unordered even
|
||||
without `finite`. Infinities are subject to the declared bounds and `finite`
|
||||
rule. These flags are descriptor metadata and contribute to schema identity.
|
||||
|
||||
The configurator exchanges floating-point values with the browser as strings.
|
||||
Finite decimal strings preserve signed zero. Nonfinite values use exact IEEE
|
||||
bit strings, `bits:0x` followed by 8 hexadecimal digits for `f32` or 16 for `f64`,
|
||||
preserving NaN sign and payload. This representation does not bypass validation.
|
||||
|
||||
CRC-32C detects accidental corruption. The big-endian trailer avoids the fixed
|
||||
residue produced by a little-endian reflected CRC under the outer transfer CRC,
|
||||
but neither checksum proves unique content or authenticates a peer.
|
||||
|
||||
An imported file must match the target-generated schema, descriptor metadata,
|
||||
factory values, field order, native scalar kinds, and exact length. Unknown,
|
||||
duplicate, missing, malformed, noncanonical, out-of-range, and relationally
|
||||
invalid values are rejected. The target stages only the typed candidate and a
|
||||
small parser state; it never buffers the complete public file. Exports are also
|
||||
generated by range directly from compiled metadata and current committed
|
||||
values. One small export plan containing the exact length and checksums is
|
||||
prepared when a read opens and reused for every requested range.
|
||||
invalid requested values are rejected. The target stages typed requested and
|
||||
expected-base candidates with a small parser state, not a whole-file buffer.
|
||||
Exports are generated by range from metadata and the selected view, with one
|
||||
length/checksum plan prepared at open.
|
||||
|
||||
Schema identity is required because a complete file can otherwise be validly
|
||||
encoded while belonging to another firmware schema. It is generated from the
|
||||
schema and is not a hand-maintained board ID.
|
||||
The board-agnostic `decode_kcfg` API instead borrows a complete input artifact
|
||||
and requires caller-owned 8192-byte field-ID scratch. Hosted callers can allocate
|
||||
that bitmap directly on the heap. Decoding visits fields once, with work
|
||||
O(input bytes + 8192), and the returned view borrows only the input, so scratch
|
||||
is immediately reusable. This dynamic decoder is distinct from the bounded
|
||||
typed streaming importer used by firmware.
|
||||
|
||||
The artifact header's restart bit describes the committed values in an export.
|
||||
It is not an instruction accepted from an upload: after persistence the target
|
||||
derives restart state by comparing committed values with immutable boot-active
|
||||
values.
|
||||
Schema identity detects schema mismatch. It is not a device identity,
|
||||
authentication token or proof of all hardware compatibility. Complete-candidate
|
||||
validation against the immutable context remains authoritative.
|
||||
|
||||
The restart bit reports manager state, including on the active export. It is not
|
||||
an upload instruction. The target derives it from committed versus boot-active
|
||||
values, or sets it when durable state is unknown.
|
||||
|
||||
## Generic file dialect
|
||||
|
||||
Kaptan uses two custom messages rather than standard MAVLink FTP or a family of
|
||||
configuration-specific requests:
|
||||
|
||||
```text
|
||||
FILE_CONTROL
|
||||
FILE_DATA
|
||||
```
|
||||
Kaptan uses custom `FILE_CONTROL` and `FILE_DATA` messages rather than standard
|
||||
MAVLink FTP or configuration-specific messages.
|
||||
|
||||
`FILE_CONTROL` opens, closes, aborts, and acknowledges a transfer. It
|
||||
carries a client-selected nonzero `u16` transfer ID, exact target, position,
|
||||
complete file size, CRC-32C, operation/status, and at most 64 bytes of path or
|
||||
result detail. A source does not reuse an ID during the ten-second replay lease.
|
||||
A new ID from the same source may supersede an open only before the first byte
|
||||
has advanced; a progressed abandoned transfer remains pinned until lease expiry.
|
||||
result detail. A peer does not reuse an ID during the ten-second replay lease.
|
||||
A new ID from the same source and ingress may supersede an open only before the
|
||||
first byte advances. A progressed abandoned transfer stays pinned until expiry.
|
||||
`FILE_DATA` uses the full MAVLink payload budget for a
|
||||
246-byte chunk. The logical final-chunk length follows from the opened file
|
||||
size, so no redundant length field is sent with every data packet. An upload
|
||||
request carries status `UNSPECIFIED`; a read-data response carries status `OK`.
|
||||
All failures remain typed `FILE_CONTROL` responses.
|
||||
|
||||
The service owns one bounded stop-and-wait transfer. Chunks are sequential;
|
||||
immediate retries are replay-safe, conflicting retries are rejected, and stale
|
||||
transfer IDs cannot mutate a later transfer. A receive fault or bounded lease
|
||||
expiry aborts staged provider state. The generic layer knows only paths, bytes,
|
||||
positions, checksums, ownership, and retry state. It does not parse KCFG or
|
||||
contain configuration policy.
|
||||
One stop-and-wait transfer is shared across all links. Ownership includes the
|
||||
claimed source system/component and the platform-supplied ingress identity.
|
||||
Chunks are sequential. Immediate retries compare exact retained bytes, not just
|
||||
checksums. Conflicting retries are rejected. Accepted owner activity renews the
|
||||
ten-second monotonic lease, while rejected traffic does not. Expiry aborts staged
|
||||
provider state. A receive discontinuity resets framing without itself releasing
|
||||
the file lease. Responses use the requesting link.
|
||||
|
||||
The current provider exposes exactly `/config/user.kcfg`. A successful write
|
||||
close has generic status `OK` and content beginning with the authoritative
|
||||
seven-byte KCFG result prefix: restart flag, primary field, related field, and
|
||||
validation-rule code. Its exact layout is `flags:u8 | primary_field:u16 LE |
|
||||
related_field:u16 LE | validation_rule:u16 LE`; flag bit 0, when set, means a
|
||||
For either writable KCFG endpoint, a successful close has generic status `OK`
|
||||
and an authoritative seven-byte KCFG result prefix:
|
||||
`flags:u8 | primary_field:u16 LE |
|
||||
related_field:u16 LE | validation_rule:u16 LE`. Flag bit 0, when set, means a
|
||||
restart is required, and every other flag bit is reserved zero. The field
|
||||
sentinel is `0xffff`; rule zero means no rule.
|
||||
sentinel is `0xffff`. Rule zero means no rule.
|
||||
KCFG `CONTENT_REJECTED` responses use the same prefix and may append bounded
|
||||
UTF-8 detail. The configurator rejects a successful close with a missing,
|
||||
malformed, or failure-bearing prefix. The file layer passes these bytes through
|
||||
without interpreting them.
|
||||
|
||||
Local USB CDC-ACM and the directly wired UART are the F722 transports. CRC
|
||||
validates framing integrity, not peer authenticity. Use one directly connected
|
||||
client and disconnect other transmitters from the board RX line while using the
|
||||
UART. A shared or remote GCS still requires signing, source/key binding,
|
||||
persistent anti-replay state, and provisioning before this inbound service can
|
||||
be exposed safely.
|
||||
See [MAVLink](mavlink.md) for the local-link trust boundary.
|
||||
|
||||
## Private persistence
|
||||
|
||||
F722 uses private persistence separate from the public KCFG artifact.
|
||||
The F722 private storage format is independent of the public KCFG artifact.
|
||||
|
||||
### F722 flash journal
|
||||
|
||||
F722 stores a compact private `KCSP` scalar snapshot inside a two-slot journal.
|
||||
Each entry stores its stable field ID, scalar kind, and value at that kind's
|
||||
native width; an `u8` is one byte rather than a widened integer slot. Each
|
||||
F722 stores a `KCSP` scalar snapshot inside a two-slot journal.
|
||||
Each entry stores its stable field ID, scalar kind and value at native width. Each
|
||||
journal record includes payload length, CRC-32C, commit marker, and a private
|
||||
monotonic sequence. Saving erases the inactive slot, programs and verifies the
|
||||
complete replacement, and writes the commit marker last. The previous valid
|
||||
|
||||
+13
-2
@@ -78,14 +78,25 @@ different reportable error is already pending.
|
||||
`PersistConfigurationError` ID that distinguishes journal geometry, model,
|
||||
sequence, flash-controller, bounds, unlock, and verification
|
||||
failures. Replaying the cached close result does not repeat the report or
|
||||
flash operation.
|
||||
flash operation. Any persistence error makes committed state unknown and
|
||||
blocks ordinary saves until reboot recovery or a successful explicit factory
|
||||
reset. Boot-active values remain unchanged.
|
||||
- A stale KCFG expected base returns `REQUEST_CONFLICT` before persistence.
|
||||
It is a reload-required editing conflict, not a reportable firmware error.
|
||||
Invalid boot configuration uses the UART-disabled USB setup path and retains
|
||||
readable saved values for repair. See [Configuration](configuration.md).
|
||||
- SPI completion errors never copy partial data into the caller's receive
|
||||
buffer. If bounded shutdown cannot establish quiescence, the SPI route
|
||||
quarantines its static DMA storage and refuses further transfers. This is
|
||||
distinct from transient `Busy`. Physical SPI shutdown and fault behavior
|
||||
remain hardware-validation work.
|
||||
- Reported `ReportableError` values reach the F722 MAVLink worker as a
|
||||
`ERROR`, subject to bounded transport admission. The payload is
|
||||
`{ error_id, raised_at[96] }`. `raised_at` is a zero-padded UTF-8
|
||||
`file:line` prefix for the reporting point.
|
||||
Dynamic human-readable context belongs in `LOG`, while
|
||||
structured protocol failures remain in their owning response message.
|
||||
- Control, navigation, and safety error types are derive-ready but
|
||||
- Control, navigation and safety error types are derive-ready but
|
||||
their live owners are not yet attached to a MAVLink worker.
|
||||
|
||||
F722 uses `panic-halt`, without live panic reporting or retained-record replay.
|
||||
|
||||
@@ -30,8 +30,8 @@ target-specific commands in [Development and verification](development.md).
|
||||
|
||||
| Path | Responsibility |
|
||||
| --- | --- |
|
||||
| `firmware/boards/` | Board-owned user schemas, factory values, immutable hardware declarations, and image composition roots. |
|
||||
| `firmware/platforms/stm32f7/` | Reviewed F722 RTIC platform, peripheral ownership, and shared linker layout. |
|
||||
| `firmware/boards/` | Hardware declarations, UART catalogs and image roots. |
|
||||
| `firmware/platforms/stm32f7/` | F722 RTIC platform, user schema, factory values, peripheral ownership and linker layout. |
|
||||
| `firmware/libs/` | Typed math, estimation, control, safety, protocol, and driver crates. |
|
||||
| `firmware/tools/configurator/` | Board-agnostic loopback web UI and serial bridge. |
|
||||
| `docs/` | This Markdown documentation site. |
|
||||
|
||||
+3
-3
@@ -4,9 +4,9 @@ Kaptan is an embedded Rust flight-controller project. This manual is the
|
||||
single source of project documentation: board support, safety boundaries,
|
||||
development workflow, and design references live here as Markdown.
|
||||
|
||||
> **Status:** the checked-in firmware is telemetry-only. It does not expose
|
||||
> physical motor authority, and this site does not make a physical-flight
|
||||
> release claim.
|
||||
> **Status:** The board images do not yet run the flight runtime or implement
|
||||
> motor drivers. Motor pins are inhibited at startup. Physical flight has not
|
||||
> been qualified.
|
||||
|
||||
## Index
|
||||
|
||||
|
||||
+11
-9
@@ -60,11 +60,12 @@ Rust error variant. Renaming or reordering a variant does not change it.
|
||||
diagnostics. Protocol failures remain in their owning response; for example, a
|
||||
rejected KCFG upload returns a `FILE_CONTROL` failure and readable
|
||||
bounded detail.
|
||||
Delivery is bounded and lossy under link pressure; heartbeat and safety
|
||||
Delivery is bounded and lossy under link pressure. Heartbeat and sensor
|
||||
telemetry use separate paths. Logs have no publication rate setting and are
|
||||
not periodic streams. A retained event leaves its MAVLink sink only once the
|
||||
lower transport admits it; temporary queue pressure only defers that same
|
||||
event.
|
||||
not periodic streams. The worker offers each retained event to USB and every
|
||||
MAVLink UART, then removes it once at least one transport admits it. Admission
|
||||
does not prove receipt or delivery to every link. If none admits it, the event
|
||||
remains pending. Opening USB does not take diagnostics away from UART.
|
||||
|
||||
## Filtering
|
||||
|
||||
@@ -73,8 +74,11 @@ Trace (1), Debug (2), Info (3), Warn (4), and Error (5). `Info`, for example,
|
||||
accepts Info, Warn, and Error log events. `Off` suppresses `LOG` on MAVLink
|
||||
without suppressing reportable `ERROR` events or logs sent to another
|
||||
sink. There is no separate enable flag and no MAVLink-specific set of extra
|
||||
runtime log levels. F722 uses the validated filter recovered at boot, falling
|
||||
back to its board factory when the journal is empty or invalid.
|
||||
runtime log levels. F722 uses the validated boot-active filter. Empty storage
|
||||
uses factory values. Invalid storage selects the setup fallback, which also
|
||||
uses the factory filter but disables UART roles. Saved changes and factory
|
||||
reset do not alter the active filter until reboot. See
|
||||
[configuration recovery](configuration.md#recovery-and-activation).
|
||||
|
||||
Events retain the compact typed `Level` and sink configuration retains the
|
||||
compact typed `LevelFilter`. Schema metadata supplies the filter choices to the
|
||||
@@ -82,6 +86,4 @@ configurator.
|
||||
Only the MAVLink adapter converts the five event levels to the typed Kaptan
|
||||
wire enum.
|
||||
|
||||
The log ingress is transport-neutral and unfiltered. F722 images accept the
|
||||
unsigned Kaptan file service on local USB CDC-ACM and their directly wired UART; that physical-link assumption does not extend to a shared
|
||||
or remote MAVLink network.
|
||||
The log ingress is transport-neutral and unfiltered.
|
||||
+55
-36
@@ -9,48 +9,67 @@ dialect. Inbound processing is narrowly scoped:
|
||||
- Signed frames are discarded because no signature verifier or persistent
|
||||
replay state exists.
|
||||
- CRC-valid unsigned MAVLink 2 frames are retained only when their message ID
|
||||
and extra CRC belong to the Kaptan dialect. Exact-target Kaptan file requests
|
||||
are the only inbound records with a current firmware consumer.
|
||||
and extra CRC belong to the Kaptan dialect. The board firmware consumes
|
||||
exact-target Kaptan file requests.
|
||||
|
||||
Shared or remote inbound authority is deliberately unsupported. Enabling it
|
||||
requires verified MAVLink 2 signing, configured source/target binding,
|
||||
persistent anti-replay state, and a provisioning/key-rotation policy as one
|
||||
reviewed feature.
|
||||
|
||||
Each F722 image exposes MAVLink on its board-selected UART and as a USB CDC-ACM
|
||||
serial device. Linux normally names the application USB link `/dev/ttyACM*`;
|
||||
this is distinct from the STM32 ROM DFU device used while the Boot pin is
|
||||
asserted. UART and USB have independent receive framers. A host that opens the
|
||||
CDC port and asserts DTR becomes the publication and diagnostic link; UART is
|
||||
used while USB is closed. A file response is always returned on the link that
|
||||
carried its request, so connecting USB cannot redirect a UART transaction.
|
||||
Each F722 board image initializes USB CDC-ACM as a MAVLink link independently
|
||||
of UART roles. Linux normally names it `/dev/ttyACM*`. It is separate from the
|
||||
STM32 ROM DFU device used for flashing. Every UART assigned `KaptanMavlink` at
|
||||
boot has its own link. USB needs an enumerated device and asserted DTR to carry
|
||||
traffic. Opening or closing it does not redirect UART publications.
|
||||
|
||||
Each link has independent scheduling, encoding, framing and bounded TX state.
|
||||
Current links share the same boot-active endpoint identity and publication
|
||||
rates, with one shared sensor snapshot per service turn. There are no per-link
|
||||
rate settings. A stalled link does not consume another link's publication state,
|
||||
though all links share the protocol worker and flash-operation stalls. File
|
||||
responses always return on the requesting link.
|
||||
|
||||
The current descriptor uses the pid.codes shared development identity
|
||||
`1209:0001` plus the STM32 factory device ID as its serial number. Obtain and
|
||||
substitute a project-specific PID before distributing hardware as a product.
|
||||
|
||||
These local links are operational file transports, not authenticated
|
||||
The normal telemetry links are operational file transports, not authenticated
|
||||
remote-control channels. Any transmitter physically connected to either link
|
||||
can issue file requests, so use a direct USB cable or USB-UART adapter and
|
||||
disconnect other transmitters while configuring. The platform retains its
|
||||
startup-configured low motor-output state throughout this workflow; no marker
|
||||
type is treated as GPIO ownership or command authority. A networked GCS must
|
||||
add verified signing, source binding, and durable replay protection before it
|
||||
can receive authority.
|
||||
type is treated as GPIO ownership or command authority.
|
||||
|
||||
The custom Kaptan file service admits one exact source principal for a bounded
|
||||
stop-and-wait transfer. A receive fault or lease expiry aborts its staged
|
||||
provider state. Completed control replies have a bounded replay path so a lost
|
||||
close response does not repeat a flash operation. This serialization does not
|
||||
authenticate its peer.
|
||||
One file service serializes transfers across USB and all MAVLink UARTs. Its
|
||||
owner is the claimed source system/component plus ingress identity, so the same
|
||||
wire identity on another port cannot take over a transfer. Completed replies
|
||||
have an exact bounded replay path so a lost close response does not repeat a
|
||||
flash operation. Neither source identity, schema identity nor CRC authenticates
|
||||
a peer. [Configuration](configuration.md#generic-file-dialect) defines the lease
|
||||
and retry contract.
|
||||
|
||||
Telemetry transmission uses bounded frame classes. A critical frame may
|
||||
replace pending best-effort traffic, but never a UART-DMA-owned or active USB
|
||||
frame. UART completion retries preserve ordering, while USB advances a retained
|
||||
frame across bounded CDC endpoint writes. DTR close, suspend, and
|
||||
deconfiguration pause USB transmission and return publications to UART. Only a
|
||||
bus reset discards USB transmission state. Closing DTR also discards partial
|
||||
input. File-transfer and response-replay state expire under the protocol lease,
|
||||
not on transport disconnect.
|
||||
Telemetry transmission uses bounded frame classes. A critical frame may replace
|
||||
pending best-effort traffic, but never an active frame. UART TX is IRQ-owned:
|
||||
each interrupt writes at most one byte and retires at most one completed frame.
|
||||
USB advances a retained frame through bounded CDC endpoint writes. DTR close,
|
||||
suspend and deconfiguration pause USB TX. Only a bus reset discards its TX state.
|
||||
Closing DTR discards partial input. UART receive faults reset framing. File and
|
||||
replay state remain governed by the protocol lease rather than disconnects.
|
||||
|
||||
USB receive backpressure masks only `GINTMSK.RXFLVLM` while application packet
|
||||
bytes remain unconsumed. The final byte cursor and gate update share the USB
|
||||
resource lock. The OTG FS NVIC vector stays enabled, as do IN/TX completion,
|
||||
reset and suspend interrupt sources. This does not make control reception
|
||||
independent: EP0 shares the RX FIFO and cannot bypass a blocked bulk packet at
|
||||
its head. Once buffered bytes are consumed, RXFLVLM is restored. An incomplete
|
||||
MAVLink frame alone does not keep RX masked.
|
||||
|
||||
UART role and baud changes take effect after reboot. See
|
||||
[Configuration](configuration.md) for allowed values and USB repair, and
|
||||
[Architecture](architecture.md#io-ownership) for RX buffering and ownership.
|
||||
Normal USB configuration recovery does not guarantee availability after an
|
||||
arbitrary clock, USB hardware or initialization failure.
|
||||
|
||||
The local web configurator exposes a bounded live view of CRC-valid inbound
|
||||
frames. It groups packets by source and message ID and measures receive
|
||||
@@ -71,35 +90,35 @@ one-message leaves grouped by interaction contract:
|
||||
| Directory | Contract |
|
||||
| --- | --- |
|
||||
| `publish/` | Zero-reply records for any interested receiver. |
|
||||
| `exchange/` | Exact-endpoint requests with one terminal response; currently empty. |
|
||||
| `transfer/` | Exact-endpoint, bounded multi-message sessions. |
|
||||
|
||||
The root includes every message leaf directly; there are no redundant category
|
||||
index files or separate enum catalogs. Placement is a wire-contract
|
||||
classification, while bindgen still generates one flat `generated::kaptan`
|
||||
module. Bare message names are scoped by that sole dialect and must not be
|
||||
merged with another MAVLink dialect. `exchange/` gains its first XML only when
|
||||
an exchange has a current consumer.
|
||||
merged with another MAVLink dialect.
|
||||
`LOG` is an outbound human-readable four-field record. `ERROR`
|
||||
is the separate bounded channel for reportable firmware errors, carrying a
|
||||
stable explicit ID plus the `file:line` of its reporting point, and needs no
|
||||
redundant level field. `FILE_CONTROL` and `FILE_DATA` implement
|
||||
Kaptan's generic virtual-file transport without standard MAVLink FTP
|
||||
compatibility. The current configurator reads and replaces
|
||||
`/config/user.kcfg`. See
|
||||
compatibility. The configurator reads saved and boot-active KCFG values and can
|
||||
save replacements or reset saved settings to factory values. See
|
||||
[Configuration](configuration.md), [Logs](logs.md), and [Errors](errors.md) for
|
||||
the exact contracts.
|
||||
|
||||
The current scheduled publications are deliberately native rather than
|
||||
The board firmware's scheduled publications are native rather than
|
||||
projections into a broader dialect:
|
||||
|
||||
| Publication | Current payload |
|
||||
| --- | --- |
|
||||
| `HEARTBEAT` (51027) | The single current initialized/output-inhibited state. |
|
||||
| `IMU` (51028) | Monotonic microseconds, body acceleration in mg, angular rate in mrad/s, and temperature in cdegC. |
|
||||
| `IMU` (51028) | `u64` monotonic microseconds, `i16[3]` body acceleration in mg, `i32[3]` angular rate in mrad/s, and `i16` temperature in cdegC. |
|
||||
| `RANGE` (51029) | Monotonic microseconds and downward distance in mm. |
|
||||
| `OPTICAL_FLOW` (51030) | Monotonic microseconds, integer ground-relative velocity in mm/s, distance in mm, and quality. |
|
||||
|
||||
These records preserve the firmware's bounded native values. They do not
|
||||
carry unused sensor IDs, repeated board topology, float projections, narrowed
|
||||
timestamps, or clamped measurements.
|
||||
|
||||
The IMU payload is 28 bytes with MAVLink extra CRC **22**. Wire field ordering
|
||||
places time at byte 0, angular rate at byte 8, acceleration at byte 20 and
|
||||
temperature at byte 26. The signed 32-bit gyro units preserve the full decoded
|
||||
MPU6000 ±2000°/s range through mounting, mailbox handoff and encoding.
|
||||
+74
-20
@@ -1,34 +1,88 @@
|
||||
# Safety boundary
|
||||
|
||||
Every checked-in F722 image drives PB0, PB1, PB4, and PB5 low before normal
|
||||
board initialization. This startup output inhibition configures each pin as a
|
||||
low, push-pull GPIO output; it runs before clock-tree setup, board routing,
|
||||
UART setup, or RTIC tasks. There is no marker or token standing in for the
|
||||
register writes.
|
||||
F722 startup drives PB0, PB1, PB4 and PB5 low, plus PB6 and PB7 on Kakute.
|
||||
It configures them as push-pull GPIO outputs before clock-tree setup, board
|
||||
routing, UART setup or RTIC tasks.
|
||||
|
||||
This is a firmware startup property, not a hardware latch. Reset temporarily
|
||||
returns GPIO configuration to the MCU reset state, and the code cannot prove
|
||||
external routing, pull resistors, ESC polarity, or ESC power behavior. Board
|
||||
support must document those electrical assumptions; continuous inhibition
|
||||
external routing, pull resistors, ESC polarity or ESC power behavior. Board
|
||||
support must document those electrical assumptions. Continuous inhibition
|
||||
through reset requires board hardware that provides it.
|
||||
|
||||
There is no checked-in platform actuator route:
|
||||
|
||||
- no DShot or PWM peripheral implementation is linked;
|
||||
- no TIM3/TIM5 motor task or DMA output path exists;
|
||||
- only CRC-valid Kaptan-dialect MAVLink input reaches the bounded virtual-file
|
||||
service, which has no actuator path;
|
||||
- no arm command is defined or admitted.
|
||||
|
||||
The framing and file-transfer crates do not grant hardware access or actuator
|
||||
authority.
|
||||
The board images have no DShot, PWM or other actuator driver. The portable
|
||||
flight runtime is not yet connected to physical motors. File requests grant
|
||||
no actuator access.
|
||||
|
||||
Run the structural guard after any platform or board change:
|
||||
|
||||
```sh
|
||||
make firmware -- test -p platform-stm32f7 \
|
||||
cd firmware
|
||||
cargo test -p platform-stm32f7 \
|
||||
--test platform_safety_boundary --locked --offline
|
||||
```
|
||||
|
||||
This boundary is deliberately stronger than an application-level disarm flag:
|
||||
it removes the physical actuator implementation from the platform image.
|
||||
## Shared runtime commit contract
|
||||
|
||||
`FlightEndpoint` uses `SafetySupervisor::evaluate_and_commit` to
|
||||
evaluate current evidence and invoke an enabled-output callback at most once
|
||||
while exclusively borrowing that supervisor. The callback synchronously retains
|
||||
a core-owned `OutputRecord` in caller-owned storage. This establishes local
|
||||
publication only. The receiving consumer must check the record's binding,
|
||||
sequence and absolute validity at application time. A queued record or successful
|
||||
USB write is not a reusable permission or proof of remote application.
|
||||
|
||||
Runtime ingress carries a producer session and runtime generation, with separate
|
||||
command-owner, arm, offboard and origin bindings. Commands and setpoints have
|
||||
independent strictly increasing sequences. Retransmission retains its original
|
||||
identity and bytes. Replays and wrong bindings cannot renew authority, and checked
|
||||
authority sequences cannot wrap. A local `begin_epoch` is allowed only while
|
||||
disarmed after successful inhibited publication. It resets navigation and request
|
||||
watermarks while preserving the producer's output sequence and clock history.
|
||||
These typed runtime identities do not authenticate a peer.
|
||||
|
||||
Observation admission receives independently captured processing time in that
|
||||
epoch. The measurement-time upper bound must not follow receipt time, receipt
|
||||
must not follow processing time, and conservative measurement age must satisfy
|
||||
the stream's freshness bound.
|
||||
Receipt alone does not establish accepted or current evidence. Explicit clock
|
||||
ticks advance navigation and supervision even when input stops. The current
|
||||
control period is 10 ms on an absolute due grid with a 20-ms control deadline.
|
||||
`supervise` checks time, cached health and deadlines without integrating control
|
||||
or renewing enabled output. A deadline miss is a fault, not permission to replay
|
||||
an old allocation indefinitely.
|
||||
|
||||
Returned enabled-output errors latch a fault and trigger an inhibition attempt.
|
||||
The sink must remain callable for that attempt. At terminal landing, the endpoint
|
||||
clears arm authority and attempts inhibition before publishing terminal command
|
||||
outcomes. Successful stop completion requires accepted inhibition; a rejected
|
||||
inhibition leaves the command failed. Callers must reconcile committed outcomes after
|
||||
both successful and failed core calls. This ordering does not prove physical
|
||||
touchdown or motor stop.
|
||||
|
||||
Output callbacks must not panic. Cleanup guarantees apply to
|
||||
returned errors, not unwinding. A host that catches an unwind must discard the
|
||||
affected endpoint and output owner rather than resume them or use an epoch reset
|
||||
as recovery. Platform panic containment owns the physical
|
||||
response. Abort or halt alone does not establish zero output.
|
||||
|
||||
## Core failure policy
|
||||
|
||||
`TakeoffHoverLand` starts only from `ArmedIdle` on a fresh command. Settled
|
||||
takeoff starts a 30-second hold, followed by landing under the existing deadline
|
||||
and landing reserve. It completes after accepted output inhibition. A superseding
|
||||
command cancels the mission. The board integration prerequisites above still apply.
|
||||
|
||||
With required navigation and output health present, offboard stream expiry
|
||||
after 300 ms latches fallback, holds for the configured duration (at most one
|
||||
second) from expiry, then lands.
|
||||
Late packets do not resume the expired stream. Loss of required IMU/range or
|
||||
other navigation capability, numerical failure, control-deadline failure or
|
||||
output-publication failure can terminate and inhibit instead. Hold-then-land is
|
||||
not promised without its required evidence.
|
||||
|
||||
The core's configured output lifetime is at most 50 ms, further shortened by
|
||||
capability evidence and the current 20-ms control deadline. A future physical
|
||||
composition must enforce output validity at its actuator boundary and establish
|
||||
its own hardware timing and failure behavior. The 1.2-kg/18-N test fixture is
|
||||
uncalibrated. Component verification does not qualify a vehicle.
|
||||
Generated
+23
-37
@@ -52,6 +52,7 @@ checksum = "8ec610d8f49840a5b376c69663b6369e71f4b34484b9b2eb29fb918d92516cb9"
|
||||
dependencies = [
|
||||
"bare-metal",
|
||||
"bitfield",
|
||||
"critical-section",
|
||||
"embedded-hal",
|
||||
"volatile-register",
|
||||
]
|
||||
@@ -222,6 +223,7 @@ dependencies = [
|
||||
name = "kaptan-data-channel"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"critical-section",
|
||||
"kaptan-err-reportable",
|
||||
"kaptan-mavlink",
|
||||
"kaptan-sensor",
|
||||
@@ -239,10 +241,6 @@ dependencies = [
|
||||
"kaptan-mavlink",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "kaptan-dma"
|
||||
version = "0.1.0"
|
||||
|
||||
[[package]]
|
||||
name = "kaptan-drivers"
|
||||
version = "0.1.0"
|
||||
@@ -283,9 +281,9 @@ name = "kaptan-flight-config"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"kaptan-config",
|
||||
"kaptan-err-reportable",
|
||||
"kaptan-log",
|
||||
"minicbor",
|
||||
"kaptan-control",
|
||||
"kaptan-math",
|
||||
"kaptan-navigation",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
@@ -293,15 +291,18 @@ name = "kaptan-flight-runtime"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"kaptan-config",
|
||||
"kaptan-control",
|
||||
"kaptan-flight-config",
|
||||
"kaptan-log",
|
||||
"kaptan-math",
|
||||
"kaptan-navigation",
|
||||
"kaptan-safety",
|
||||
"kaptan-time",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "kaptan-geprc-f722-v2"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"kaptan-config",
|
||||
"platform-stm32f7",
|
||||
"rtic",
|
||||
]
|
||||
@@ -309,12 +310,14 @@ dependencies = [
|
||||
[[package]]
|
||||
name = "kaptan-i2c"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"kaptan-time",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "kaptan-kakute-f7-mini-v3"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"kaptan-config",
|
||||
"platform-stm32f7",
|
||||
"rtic",
|
||||
]
|
||||
@@ -338,11 +341,8 @@ name = "kaptan-mavlink"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"bitflags",
|
||||
"kaptan-dma",
|
||||
"kaptan-err-reportable",
|
||||
"kaptan-sensor",
|
||||
"kaptan-time",
|
||||
"kaptan-tx",
|
||||
"mavlink-bindgen",
|
||||
"mavlink-core",
|
||||
"num-derive",
|
||||
@@ -353,12 +353,18 @@ dependencies = [
|
||||
name = "kaptan-navigation"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"kaptan-err-reportable",
|
||||
"kaptan-math",
|
||||
"kaptan-sensor",
|
||||
"kaptan-time",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "kaptan-ports"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"kaptan-config",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "kaptan-safety"
|
||||
version = "0.1.0"
|
||||
@@ -424,26 +430,6 @@ version = "2.8.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "cf8baf1c55e62ffcace7a9f06f4bd9cd3f0c4beb022d3b367256b91b87513d98"
|
||||
|
||||
[[package]]
|
||||
name = "minicbor"
|
||||
version = "0.25.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "c0452a60c1863c1f50b5f77cd295e8d2786849f35883f0b9e18e7e6e1b5691b0"
|
||||
dependencies = [
|
||||
"minicbor-derive",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "minicbor-derive"
|
||||
version = "0.15.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "bd2209fff77f705b00c737016a48e73733d7fbccb8b007194db148f03561fb70"
|
||||
dependencies = [
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn 2.0.119",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "nb"
|
||||
version = "0.1.3"
|
||||
@@ -491,16 +477,17 @@ version = "0.1.0"
|
||||
dependencies = [
|
||||
"cortex-m",
|
||||
"cortex-m-rt",
|
||||
"critical-section",
|
||||
"kaptan-config",
|
||||
"kaptan-data-channel",
|
||||
"kaptan-diagnostics-mavlink",
|
||||
"kaptan-dma",
|
||||
"kaptan-drivers",
|
||||
"kaptan-err-reportable",
|
||||
"kaptan-file-mavlink",
|
||||
"kaptan-i2c",
|
||||
"kaptan-log",
|
||||
"kaptan-mavlink",
|
||||
"kaptan-ports",
|
||||
"kaptan-sensor",
|
||||
"kaptan-spi",
|
||||
"kaptan-time",
|
||||
@@ -724,10 +711,9 @@ dependencies = [
|
||||
[[package]]
|
||||
name = "synopsys-usb-otg"
|
||||
version = "0.4.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "e948d523b316939545d8b21a48c27aef150ce25321b9f95ff7978647a806a6fe"
|
||||
dependencies = [
|
||||
"cortex-m",
|
||||
"critical-section",
|
||||
"embedded-hal",
|
||||
"usb-device",
|
||||
"vcell",
|
||||
|
||||
+5
-1
@@ -8,7 +8,6 @@ members = [
|
||||
"libs/kaptan-config-derive",
|
||||
"libs/kaptan-file-mavlink",
|
||||
"libs/kaptan-data-channel",
|
||||
"libs/kaptan-dma",
|
||||
"libs/kaptan-drivers",
|
||||
"libs/kaptan-err-reportable",
|
||||
"libs/kaptan-err-reportable-macros",
|
||||
@@ -20,6 +19,7 @@ members = [
|
||||
"libs/kaptan-log",
|
||||
"libs/kaptan-diagnostics-mavlink",
|
||||
"libs/kaptan-navigation",
|
||||
"libs/kaptan-ports",
|
||||
"libs/kaptan-safety",
|
||||
"libs/kaptan-sensor",
|
||||
"libs/kaptan-spi",
|
||||
@@ -28,6 +28,7 @@ members = [
|
||||
"platforms/stm32f7",
|
||||
"tools/configurator",
|
||||
]
|
||||
exclude = ["vendor/synopsys-usb-otg"]
|
||||
resolver = "2"
|
||||
|
||||
[workspace.package]
|
||||
@@ -49,3 +50,6 @@ codegen-units = 1
|
||||
lto = "fat"
|
||||
panic = "abort"
|
||||
strip = "symbols"
|
||||
|
||||
[patch.crates-io]
|
||||
synopsys-usb-otg = { path = "vendor/synopsys-usb-otg" }
|
||||
+6
-6
@@ -1,15 +1,13 @@
|
||||
# Firmware
|
||||
|
||||
This directory is the Kaptan flight-controller firmware workspace. It owns the
|
||||
Rust workspace manifest, board declarations, platform implementation, reusable
|
||||
firmware crates and firmware scripts.
|
||||
Kaptan flight-controller firmware workspace.
|
||||
|
||||
```text
|
||||
firmware/
|
||||
├── boards/ immutable board declarations and linker layouts
|
||||
├── boards/ board hardware declarations and entry points
|
||||
├── libs/ reusable no_std firmware crates
|
||||
├── platforms/ hardware platform composition
|
||||
├── tools/ firmware development tooling
|
||||
├── platforms/ hardware composition, configuration and linker layouts
|
||||
├── tools/ local configuration and telemetry tools
|
||||
└── scripts/ packaging, analysis, and coverage helpers
|
||||
```
|
||||
|
||||
@@ -25,3 +23,5 @@ test suite. Build and package every supported board with
|
||||
`make firmware/boards`, or select one board by its directory path.
|
||||
Run `make firmware clippy` for Rust lints and `make firmware docs` for API
|
||||
documentation with warnings denied.
|
||||
The board images do not yet run the flight runtime or implement motor drivers.
|
||||
Motor pins are inhibited at startup. Physical flight has not been qualified.
|
||||
@@ -10,10 +10,8 @@ build = "build.rs"
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
kaptan-config = { path = "../../libs/kaptan-config" }
|
||||
platform-stm32f7 = { path = "../../platforms/stm32f7" }
|
||||
# RTIC's attribute expansion emits absolute `rtic` support paths in the final
|
||||
# board crate; the task graph and all RTIC source remain platform-owned.
|
||||
# RTIC's macro emits absolute `rtic` paths in the board crate.
|
||||
rtic = { version = "2.3", default-features = false, features = [
|
||||
"thumbv7-backend",
|
||||
] }
|
||||
|
||||
@@ -1,79 +1,35 @@
|
||||
//! Declarative support for the GEPRC TAKER F722 BLS 60A V2 stack.
|
||||
//! Declarative support for the GEPRC GEP-F722-HD V2.
|
||||
|
||||
#![forbid(unsafe_code)]
|
||||
|
||||
use platform_stm32f7::kaptan_ports::{PortCatalog, PortDescriptor};
|
||||
use platform_stm32f7::{
|
||||
F7Board, F722Hardware, F722ImmutableConfig as ImmutableConfig, Icm42688, ImuMount, LevelFilter,
|
||||
Mavlink, Pa13ActiveHigh, Pb0Pb1Pb4Pb5Low, Pc3, VerticalSensor,
|
||||
F7Board, F722Hardware, F722ImmutableConfig, F722UserConfig, Icm42688Spi3, ImuMount, MotorPins,
|
||||
SerialInventory, StatusLed, UART1, UART2, UART3, UART4, UART6, VerticalSensor,
|
||||
};
|
||||
|
||||
/// User-writable MAVLink endpoint policy.
|
||||
#[derive(Clone, kaptan_config::Config)]
|
||||
struct MavlinkConfig {
|
||||
#[config(id = 101, key = "mavlink.system_id", unit = "id", range(min = 1))]
|
||||
system_id: u8,
|
||||
#[config(id = 102, key = "mavlink.component_id", unit = "id", range(min = 1))]
|
||||
component_id: u8,
|
||||
#[config(
|
||||
enum,
|
||||
id = 105,
|
||||
key = "mavlink.logging.level_filter",
|
||||
unit = "log_level"
|
||||
)]
|
||||
log_level_filter: LevelFilter,
|
||||
}
|
||||
|
||||
/// User-writable telemetry publication policy.
|
||||
#[derive(Clone, kaptan_config::Config)]
|
||||
struct TelemetryConfig {
|
||||
#[config(
|
||||
id = 103,
|
||||
key = "telemetry.imu_hz",
|
||||
unit = "Hz",
|
||||
range(min = 1, max = 100)
|
||||
)]
|
||||
imu_hz: u8,
|
||||
#[config(
|
||||
id = 104,
|
||||
key = "telemetry.heartbeat_hz",
|
||||
unit = "Hz",
|
||||
range(min = 1, max = 10)
|
||||
)]
|
||||
heartbeat_hz: u8,
|
||||
}
|
||||
|
||||
/// Complete user-writable configuration advertised by this board.
|
||||
#[derive(Clone, kaptan_config::Config)]
|
||||
pub struct UserConfig {
|
||||
mavlink: MavlinkConfig,
|
||||
telemetry: TelemetryConfig,
|
||||
}
|
||||
|
||||
/// ICM-42688-P-based GEP-F722-HD V2 paired with the 60 A stack.
|
||||
pub struct GeprcF722V2;
|
||||
|
||||
impl F7Board for GeprcF722V2 {
|
||||
type UserConfig = UserConfig;
|
||||
type ImmutableConfig = ImmutableConfig;
|
||||
impl PortCatalog<5> for GeprcF722V2 {
|
||||
const DESCRIPTORS: [PortDescriptor; 5] = [UART1, UART2, UART3, UART4, UART6];
|
||||
}
|
||||
|
||||
const FACTORY_USER_CONFIG: UserConfig = UserConfig {
|
||||
mavlink: MavlinkConfig {
|
||||
system_id: 1,
|
||||
component_id: 1,
|
||||
log_level_filter: LevelFilter::Info,
|
||||
},
|
||||
telemetry: TelemetryConfig {
|
||||
imu_hz: 20,
|
||||
heartbeat_hz: 2,
|
||||
},
|
||||
};
|
||||
const IMMUTABLE_CONFIG: ImmutableConfig = ImmutableConfig {
|
||||
impl F7Board for GeprcF722V2 {
|
||||
type UserConfig = F722UserConfig<Self, 5>;
|
||||
type ImmutableConfig = F722ImmutableConfig<Icm42688Spi3>;
|
||||
|
||||
const FACTORY_USER_CONFIG: Self::UserConfig = Self::UserConfig::FACTORY;
|
||||
const IMMUTABLE_CONFIG: Self::ImmutableConfig = F722ImmutableConfig {
|
||||
name: "GEPRC TAKER F722 V2 (ICM-42688-P)",
|
||||
service_hz: 100,
|
||||
hardware: F722Hardware::new()
|
||||
.output_inhibit(Pb0Pb1Pb4Pb5Low)
|
||||
.imu(Icm42688::on_spi3().with_drdy(Pc3).mounted(ImuMount::Yaw180))
|
||||
.mavlink(Mavlink::on_usart2().baud(115_200))
|
||||
.vertical_sensor(VerticalSensor::none())
|
||||
.status_led(Pa13ActiveHigh)
|
||||
.telemetry_only(),
|
||||
hardware: F722Hardware::new(
|
||||
Icm42688Spi3 {
|
||||
mount: ImuMount::Yaw180,
|
||||
},
|
||||
SerialInventory::new(&Self::DESCRIPTORS),
|
||||
StatusLed::ActiveHighPa13,
|
||||
VerticalSensor::None,
|
||||
MotorPins::Four,
|
||||
),
|
||||
};
|
||||
}
|
||||
@@ -9,10 +9,8 @@ version.workspace = true
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
kaptan-config = { path = "../../libs/kaptan-config" }
|
||||
platform-stm32f7 = { path = "../../platforms/stm32f7" }
|
||||
# RTIC's attribute expansion emits absolute `rtic` support paths in the final
|
||||
# board crate; the task graph and all RTIC source remain platform-owned.
|
||||
# RTIC's macro emits absolute `rtic` paths in the board crate.
|
||||
rtic = { version = "2.3", default-features = false, features = [
|
||||
"thumbv7-backend",
|
||||
] }
|
||||
|
||||
@@ -1,110 +1,42 @@
|
||||
//! Declarative Kakute F7 Mini V3 board description.
|
||||
//!
|
||||
//! This module deliberately contains no MCU registers, RTIC concepts, unsafe
|
||||
//! code, interrupt names, DMA handling, or task scheduling. Those concerns
|
||||
//! belong to the STM32F7 platform crate.
|
||||
|
||||
#![forbid(unsafe_code)]
|
||||
|
||||
use platform_stm32f7::kaptan_ports::{PortCatalog, PortDescriptor};
|
||||
use platform_stm32f7::{
|
||||
AccelerometerRange, DigitalLowPassFilter, F7Board, F722Hardware,
|
||||
F722ImmutableConfig as ImmutableConfig, GyroscopeRange, ImuMount, LevelFilter, Mavlink,
|
||||
Mpu6000, Mpu6000Config, Pa4, Pb0Pb1Pb4Pb5Low, Pd2ActiveLow, VerticalSensor,
|
||||
AccelerometerRange, DigitalLowPassFilter, F7Board, F722Hardware, F722ImmutableConfig,
|
||||
F722UserConfig, GyroscopeRange, ImuMount, MotorPins, Mpu6000Config, Mpu6000Spi1,
|
||||
SerialInventory, StatusLed, UART1, UART2, UART3, UART4, UART6, VerticalSensor,
|
||||
};
|
||||
|
||||
/// User-writable MAVLink endpoint policy.
|
||||
#[derive(Clone, kaptan_config::Config)]
|
||||
struct MavlinkConfig {
|
||||
#[config(id = 101, key = "mavlink.system_id", unit = "id", range(min = 1))]
|
||||
system_id: u8,
|
||||
#[config(id = 102, key = "mavlink.component_id", unit = "id", range(min = 1))]
|
||||
component_id: u8,
|
||||
#[config(
|
||||
enum,
|
||||
id = 105,
|
||||
key = "mavlink.logging.level_filter",
|
||||
unit = "log_level"
|
||||
)]
|
||||
log_level_filter: LevelFilter,
|
||||
}
|
||||
|
||||
/// User-writable telemetry publication policy.
|
||||
#[derive(Clone, kaptan_config::Config)]
|
||||
struct TelemetryConfig {
|
||||
#[config(
|
||||
id = 103,
|
||||
key = "telemetry.imu_hz",
|
||||
unit = "Hz",
|
||||
range(min = 1, max = 100)
|
||||
)]
|
||||
imu_hz: u8,
|
||||
#[config(
|
||||
id = 104,
|
||||
key = "telemetry.heartbeat_hz",
|
||||
unit = "Hz",
|
||||
range(min = 1, max = 10)
|
||||
)]
|
||||
heartbeat_hz: u8,
|
||||
#[config(
|
||||
id = 106,
|
||||
key = "telemetry.range_hz",
|
||||
unit = "Hz",
|
||||
range(min = 1, max = 100)
|
||||
)]
|
||||
range_hz: u8,
|
||||
#[config(
|
||||
id = 107,
|
||||
key = "telemetry.optical_flow_hz",
|
||||
unit = "Hz",
|
||||
range(min = 1, max = 100)
|
||||
)]
|
||||
optical_flow_hz: u8,
|
||||
}
|
||||
|
||||
/// Complete user-writable configuration advertised by this board.
|
||||
#[derive(Clone, kaptan_config::Config)]
|
||||
pub struct UserConfig {
|
||||
mavlink: MavlinkConfig,
|
||||
telemetry: TelemetryConfig,
|
||||
}
|
||||
|
||||
/// Holybro Kakute F7 Mini V3.
|
||||
pub struct KakuteF7MiniV3;
|
||||
|
||||
impl F7Board for KakuteF7MiniV3 {
|
||||
type UserConfig = UserConfig;
|
||||
type ImmutableConfig = ImmutableConfig;
|
||||
impl PortCatalog<5> for KakuteF7MiniV3 {
|
||||
const DESCRIPTORS: [PortDescriptor; 5] = [UART1, UART2, UART3, UART4, UART6];
|
||||
}
|
||||
|
||||
const FACTORY_USER_CONFIG: UserConfig = UserConfig {
|
||||
mavlink: MavlinkConfig {
|
||||
system_id: 1,
|
||||
component_id: 1,
|
||||
log_level_filter: LevelFilter::Info,
|
||||
},
|
||||
telemetry: TelemetryConfig {
|
||||
imu_hz: 20,
|
||||
heartbeat_hz: 2,
|
||||
range_hz: 20,
|
||||
optical_flow_hz: 20,
|
||||
},
|
||||
};
|
||||
const IMMUTABLE_CONFIG: ImmutableConfig = ImmutableConfig {
|
||||
impl F7Board for KakuteF7MiniV3 {
|
||||
type UserConfig = F722UserConfig<Self, 5>;
|
||||
type ImmutableConfig = F722ImmutableConfig<Mpu6000Spi1>;
|
||||
|
||||
const FACTORY_USER_CONFIG: Self::UserConfig = Self::UserConfig::FACTORY;
|
||||
const IMMUTABLE_CONFIG: Self::ImmutableConfig = F722ImmutableConfig {
|
||||
name: "Kakute F7 Mini V3",
|
||||
service_hz: 100,
|
||||
hardware: F722Hardware::new()
|
||||
.output_inhibit(Pb0Pb1Pb4Pb5Low)
|
||||
.imu(
|
||||
Mpu6000::on_spi1()
|
||||
.with_drdy(Pa4)
|
||||
.mounted(ImuMount::BodyAligned)
|
||||
.configured(Mpu6000Config {
|
||||
hardware: F722Hardware::new(
|
||||
Mpu6000Spi1 {
|
||||
mount: ImuMount::Yaw270,
|
||||
config: Mpu6000Config {
|
||||
accelerometer: AccelerometerRange::PlusMinus4G,
|
||||
gyroscope: GyroscopeRange::PlusMinus500DegreesPerSecond,
|
||||
digital_low_pass_filter: DigitalLowPassFilter::Bandwidth42Hz,
|
||||
sample_rate_divider: 9,
|
||||
}),
|
||||
)
|
||||
.mavlink(Mavlink::on_usart1().baud(115_200))
|
||||
.vertical_sensor(VerticalSensor::mtf01_on_uart4().baud(115_200))
|
||||
.status_led(Pd2ActiveLow)
|
||||
.telemetry_only(),
|
||||
},
|
||||
},
|
||||
SerialInventory::new(&Self::DESCRIPTORS),
|
||||
StatusLed::ActiveLowPd2,
|
||||
VerticalSensor::None,
|
||||
MotorPins::Six,
|
||||
),
|
||||
};
|
||||
}
|
||||
@@ -38,7 +38,7 @@ fn expand_scalars(
|
||||
checks: field_checks,
|
||||
} = expand_field(&crate_path, field)?;
|
||||
descriptors.push(descriptor);
|
||||
checks.extend(field_checks);
|
||||
checks.extend(field_checks.iter().cloned());
|
||||
let id = field.id.expect("scalar configuration fields have IDs");
|
||||
let ty = &field.ty;
|
||||
let field_ident = &field.ident;
|
||||
@@ -90,7 +90,11 @@ fn expand_scalars(
|
||||
};
|
||||
let previous = self.#field_ident;
|
||||
self.#field_ident = next;
|
||||
if let Err(violation) = self.__validate_local_configuration() {
|
||||
let validation = (|| -> Result<(), #crate_path::ConfigViolation> {
|
||||
#(#field_checks)*
|
||||
Ok(())
|
||||
})();
|
||||
if let Err(violation) = validation {
|
||||
self.#field_ident = previous;
|
||||
return Err(#crate_path::FieldWriteError::Violation(violation));
|
||||
}
|
||||
@@ -272,8 +276,35 @@ fn expand_field(crate_path: &syn::Path, field: &crate::parse::FieldModel) -> Res
|
||||
Check::Maximum(value) => Some(value),
|
||||
_ => None,
|
||||
});
|
||||
let finite = field
|
||||
.checks
|
||||
.iter()
|
||||
.any(|check| matches!(check, Check::Finite));
|
||||
let nonzero = field
|
||||
.checks
|
||||
.iter()
|
||||
.any(|check| matches!(check, Check::Nonzero));
|
||||
let float = matches!(&field.ty, syn::Type::Path(path)
|
||||
if path.path.is_ident("f32") || path.path.is_ident("f64"));
|
||||
if float && !finite && (minimum.is_some() || maximum.is_some()) {
|
||||
let field_ident = &field.ident;
|
||||
checks.push(quote! {
|
||||
if self.#field_ident.is_nan() {
|
||||
return Err(#crate_path::ConfigViolation::field(
|
||||
#crate_path::FieldId::new(#id),
|
||||
#crate_path::ValidationRule::Unordered,
|
||||
));
|
||||
}
|
||||
});
|
||||
}
|
||||
for check in &field.checks {
|
||||
checks.push(expand_check(crate_path, &field.ident, id, check.clone()));
|
||||
checks.push(expand_check(
|
||||
crate_path,
|
||||
&field.ident,
|
||||
id,
|
||||
check.clone(),
|
||||
float,
|
||||
));
|
||||
}
|
||||
let key = field.display_key.as_ref().expect("id fields have a key");
|
||||
let unit = field.unit.as_ref().expect("id fields have a unit");
|
||||
@@ -302,6 +333,8 @@ fn expand_field(crate_path: &syn::Path, field: &crate::parse::FieldModel) -> Res
|
||||
maximum: #maximum,
|
||||
enum_options: #enum_options,
|
||||
unit: #unit,
|
||||
finite: #finite,
|
||||
nonzero: #nonzero,
|
||||
}
|
||||
},
|
||||
checks,
|
||||
@@ -344,7 +377,18 @@ fn config_value_constructor_tokens(crate_path: &syn::Path, ty: &syn::Type) -> Re
|
||||
Ok(value)
|
||||
}
|
||||
|
||||
fn expand_check(crate_path: &syn::Path, field: &syn::Ident, id: u16, check: Check) -> TokenStream {
|
||||
fn expand_check(
|
||||
crate_path: &syn::Path,
|
||||
field: &syn::Ident,
|
||||
id: u16,
|
||||
check: Check,
|
||||
float: bool,
|
||||
) -> TokenStream {
|
||||
let zero = if float {
|
||||
quote! { 0.0 }
|
||||
} else {
|
||||
quote! { 0 }
|
||||
};
|
||||
match check {
|
||||
Check::Finite => quote! {
|
||||
if !self.#field.is_finite() {
|
||||
@@ -355,7 +399,7 @@ fn expand_check(crate_path: &syn::Path, field: &syn::Ident, id: u16, check: Chec
|
||||
}
|
||||
},
|
||||
Check::Nonzero => quote! {
|
||||
if self.#field == 0 {
|
||||
if self.#field == #zero {
|
||||
return Err(#crate_path::ConfigViolation::field(
|
||||
#crate_path::FieldId::new(#id),
|
||||
#crate_path::ValidationRule::Zero,
|
||||
|
||||
@@ -482,10 +482,12 @@ fn scalar_literal(expression: &Expr, ty: &str) -> Result<ScalarLiteral> {
|
||||
})?;
|
||||
Ok(ScalarLiteral::Unsigned(value))
|
||||
}
|
||||
"f32" => Ok(ScalarLiteral::Float(f64::from(
|
||||
float_literal(expression)? as f32
|
||||
))),
|
||||
"f64" => float_literal(expression).map(ScalarLiteral::Float),
|
||||
// Parse at the emitted literal's native precision. Parsing through f64
|
||||
// first can double-round an f32 bound across an ordering boundary.
|
||||
"f32" => Ok(ScalarLiteral::Float(f64::from(float_literal::<f32>(
|
||||
expression,
|
||||
)?))),
|
||||
"f64" => float_literal::<f64>(expression).map(ScalarLiteral::Float),
|
||||
_ => Err(syn::Error::new(
|
||||
expression.span(),
|
||||
"configuration metadata supports bool, i32, u8, u16, u32, u64, f32, and f64",
|
||||
@@ -542,7 +544,11 @@ fn unsigned_literal(expression: &Expr) -> Result<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 {
|
||||
Expr::Lit(expression) => (false, &expression.lit),
|
||||
Expr::Unary(expression) if matches!(expression.op, UnOp::Neg(_)) => {
|
||||
@@ -563,10 +569,10 @@ fn float_literal(expression: &Expr) -> Result<f64> {
|
||||
};
|
||||
let value = match literal {
|
||||
Lit::Int(value) => value
|
||||
.base10_parse::<f64>()
|
||||
.base10_parse::<T>()
|
||||
.map_err(|error| syn::Error::new(value.span(), error))?,
|
||||
Lit::Float(value) => value
|
||||
.base10_parse::<f64>()
|
||||
.base10_parse::<T>()
|
||||
.map_err(|error| syn::Error::new(value.span(), error))?,
|
||||
_ => {
|
||||
return Err(syn::Error::new(
|
||||
@@ -601,30 +607,14 @@ fn require_float(ty: &Type, span: &impl Spanned) -> Result<()> {
|
||||
}
|
||||
|
||||
fn require_numeric(ty: &Type, span: &impl Spanned) -> Result<()> {
|
||||
if scalar_type_name(ty).is_some_and(|name| {
|
||||
matches!(
|
||||
name,
|
||||
"f32"
|
||||
| "f64"
|
||||
| "i8"
|
||||
| "i16"
|
||||
| "i32"
|
||||
| "i64"
|
||||
| "i128"
|
||||
| "isize"
|
||||
| "u8"
|
||||
| "u16"
|
||||
| "u32"
|
||||
| "u64"
|
||||
| "u128"
|
||||
| "usize"
|
||||
)
|
||||
}) {
|
||||
if scalar_type_name(ty)
|
||||
.is_some_and(|name| matches!(name, "f32" | "f64" | "i32" | "u8" | "u16" | "u32" | "u64"))
|
||||
{
|
||||
Ok(())
|
||||
} else {
|
||||
Err(syn::Error::new(
|
||||
span.span(),
|
||||
"nonzero and range require a primitive numeric field",
|
||||
"nonzero and range require a supported primitive numeric field",
|
||||
))
|
||||
}
|
||||
}
|
||||
@@ -644,18 +634,11 @@ fn scalar_type_name(ty: &Type) -> Option<&str> {
|
||||
ident if ident == "bool" => "bool",
|
||||
ident if ident == "f32" => "f32",
|
||||
ident if ident == "f64" => "f64",
|
||||
ident if ident == "i8" => "i8",
|
||||
ident if ident == "i16" => "i16",
|
||||
ident if ident == "i32" => "i32",
|
||||
ident if ident == "i64" => "i64",
|
||||
ident if ident == "i128" => "i128",
|
||||
ident if ident == "isize" => "isize",
|
||||
ident if ident == "u8" => "u8",
|
||||
ident if ident == "u16" => "u16",
|
||||
ident if ident == "u32" => "u32",
|
||||
ident if ident == "u64" => "u64",
|
||||
ident if ident == "u128" => "u128",
|
||||
ident if ident == "usize" => "usize",
|
||||
_ => return None,
|
||||
})
|
||||
}
|
||||
@@ -290,3 +290,91 @@ fn recursive_tree_rejects_duplicate_display_keys() {
|
||||
Err(issue) if issue.rule() == ValidationRule::Inconsistent
|
||||
));
|
||||
}
|
||||
|
||||
#[derive(Config)]
|
||||
struct FloatRules {
|
||||
#[config(id = 1, key = "finite", unit = "", finite)]
|
||||
finite: f32,
|
||||
#[config(id = 2, key = "nonzero", unit = "", nonzero)]
|
||||
nonzero: f64,
|
||||
#[config(id = 3, key = "lower", unit = "", range(min = 0.0))]
|
||||
lower: f32,
|
||||
#[config(id = 4, key = "upper", unit = "", range(max = 1.0))]
|
||||
upper: f64,
|
||||
#[config(id = 5, key = "free", unit = "")]
|
||||
free: f64,
|
||||
#[config(id = 6, key = "nonzero32", unit = "", nonzero)]
|
||||
nonzero32: f32,
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn scalar_rules_preserve_local_repair_and_float_domains() {
|
||||
let mut values = FloatRules {
|
||||
finite: f32::NAN,
|
||||
nonzero: 0.0,
|
||||
lower: f32::NAN,
|
||||
upper: f64::NAN,
|
||||
free: f64::NAN,
|
||||
nonzero32: 0.0,
|
||||
};
|
||||
// Every other field is invalid. A local repair must still succeed.
|
||||
values
|
||||
.set_field(FieldId::new(1), ConfigValue::F32(1.0))
|
||||
.unwrap();
|
||||
for (id, value, rule) in [
|
||||
(
|
||||
1,
|
||||
ConfigValue::F32(f32::INFINITY),
|
||||
ValidationRule::NonFinite,
|
||||
),
|
||||
(2, ConfigValue::F64(0.0), ValidationRule::Zero),
|
||||
(2, ConfigValue::F64(-0.0), ValidationRule::Zero),
|
||||
(3, ConfigValue::F32(f32::NAN), ValidationRule::Unordered),
|
||||
(4, ConfigValue::F64(f64::NAN), ValidationRule::Unordered),
|
||||
(6, ConfigValue::F32(0.0), ValidationRule::Zero),
|
||||
(6, ConfigValue::F32(-0.0), ValidationRule::Zero),
|
||||
] {
|
||||
let id = FieldId::new(id);
|
||||
let before = values.field_value(id);
|
||||
assert_eq!(
|
||||
values.set_field(id, value),
|
||||
Err(FieldWriteError::Violation(ConfigViolation::field(id, rule)))
|
||||
);
|
||||
assert_eq!(values.field_value(id), before);
|
||||
}
|
||||
// Neither nonzero nor a one-sided range implies finite.
|
||||
for (id, value) in [
|
||||
(2, ConfigValue::F64(f64::from_bits(0xfff0_0000_0000_0042))),
|
||||
(3, ConfigValue::F32(f32::INFINITY)),
|
||||
(4, ConfigValue::F64(f64::NEG_INFINITY)),
|
||||
(5, ConfigValue::F64(f64::from_bits(0x7ff8_0000_0000_1234))),
|
||||
(6, ConfigValue::F32(f32::INFINITY)),
|
||||
] {
|
||||
values.set_field(FieldId::new(id), value).unwrap();
|
||||
assert_eq!(values.field_value(FieldId::new(id)), Some(value));
|
||||
}
|
||||
values.validate().unwrap();
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rule_metadata_changes_fingerprints() {
|
||||
let descriptor = FloatRules::DESCRIPTORS[0];
|
||||
assert!(descriptor.finite);
|
||||
assert!(!descriptor.nonzero);
|
||||
assert!(FloatRules::DESCRIPTORS[1].nonzero);
|
||||
let original = kaptan_config::validation::__schema_id(&[descriptor]);
|
||||
assert_ne!(
|
||||
original,
|
||||
kaptan_config::validation::__schema_id(&[kaptan_config::FieldDescriptor {
|
||||
finite: false,
|
||||
..descriptor
|
||||
}])
|
||||
);
|
||||
assert_ne!(
|
||||
original,
|
||||
kaptan_config::validation::__schema_id(&[kaptan_config::FieldDescriptor {
|
||||
nonzero: true,
|
||||
..descriptor
|
||||
}])
|
||||
);
|
||||
}
|
||||
@@ -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() {}
|
||||
@@ -0,0 +1,5 @@
|
||||
error: configuration range minimum exceeds maximum
|
||||
--> tests/ui/f32_double_rounded_bounds.rs:6:5
|
||||
|
|
||||
6 | value: f32,
|
||||
| ^^^^^
|
||||
@@ -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() {}
|
||||
@@ -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)]
|
||||
| ^^^^^^
|
||||
@@ -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() {}
|
||||
@@ -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)]
|
||||
| ^^^^^^
|
||||
@@ -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() {}
|
||||
@@ -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)]
|
||||
| ^^^^^^^
|
||||
@@ -2,7 +2,9 @@
|
||||
//!
|
||||
//! The artifact is independent of any transport and of the private flash
|
||||
//! journal. It contains the generated field metadata, one complete committed
|
||||
//! value set, and the board-selected factory values required by a generic UI.
|
||||
//! value set, the board-selected factory values, and exact expected-base values
|
||||
//! for guarded full replacement. Each field carries current, factory, then
|
||||
//! expected-base scalars at native width before bounds and text metadata.
|
||||
|
||||
use core::str;
|
||||
|
||||
@@ -24,10 +26,16 @@ const CHECKSUM_LENGTH: usize = 4;
|
||||
const FIELD_PREFIX_LENGTH: usize = 7;
|
||||
const FIELD_HAS_MINIMUM: u8 = 1 << 0;
|
||||
const FIELD_HAS_MAXIMUM: u8 = 1 << 1;
|
||||
const FIELD_FLAGS: u8 = FIELD_HAS_MINIMUM | FIELD_HAS_MAXIMUM;
|
||||
const FIELD_FINITE: u8 = 1 << 2;
|
||||
const FIELD_NONZERO: u8 = 1 << 3;
|
||||
const FIELD_FLAGS: u8 = FIELD_HAS_MINIMUM | FIELD_HAS_MAXIMUM | FIELD_FINITE | FIELD_NONZERO;
|
||||
|
||||
/// Caller-owned bitmap bytes required by dynamic KCFG decoding.
|
||||
/// Hosted callers can initialize this directly on the heap with a byte vector.
|
||||
pub const KCFG_FIELD_ID_SCRATCH_BYTES: usize = 8192;
|
||||
|
||||
/// Current canonical KCFG artifact format.
|
||||
const KCFG_VERSION: u8 = 1;
|
||||
const KCFG_VERSION: u8 = 3;
|
||||
/// KCFG header bytes preceding the first field record.
|
||||
const KCFG_HEADER_LENGTH: usize = FIELDS_OFFSET;
|
||||
/// KCFG trailer bytes containing a big-endian CRC-32C.
|
||||
@@ -208,7 +216,7 @@ pub enum KcfgEncodeError {
|
||||
}
|
||||
|
||||
/// Failure while decoding untrusted KCFG bytes.
|
||||
#[derive(Debug)]
|
||||
#[derive(Clone, Copy, Debug, PartialEq)]
|
||||
pub enum KcfgDecodeError {
|
||||
/// Input ends before a required fixed or variable-length component.
|
||||
Truncated,
|
||||
@@ -309,12 +317,15 @@ pub struct KcfgField<'a> {
|
||||
kind: ValueKind,
|
||||
current: ConfigValue,
|
||||
factory: ConfigValue,
|
||||
expected_base: ConfigValue,
|
||||
minimum: Option<ConfigValue>,
|
||||
maximum: Option<ConfigValue>,
|
||||
key: &'a str,
|
||||
unit: &'a str,
|
||||
options: KcfgEnumOptions<'a>,
|
||||
current_offset: usize,
|
||||
finite: bool,
|
||||
nonzero: bool,
|
||||
}
|
||||
|
||||
impl<'a> KcfgField<'a> {
|
||||
@@ -338,6 +349,12 @@ impl<'a> KcfgField<'a> {
|
||||
self.factory
|
||||
}
|
||||
|
||||
/// Exact committed scalar observed when this draft was first exported.
|
||||
/// Editing requested values preserves this optimistic concurrency guard.
|
||||
pub const fn expected_base_value(self) -> ConfigValue {
|
||||
self.expected_base
|
||||
}
|
||||
|
||||
/// Inclusive generated lower bound.
|
||||
pub const fn minimum(self) -> Option<ConfigValue> {
|
||||
self.minimum
|
||||
@@ -362,6 +379,16 @@ impl<'a> KcfgField<'a> {
|
||||
pub const fn enum_options(self) -> KcfgEnumOptions<'a> {
|
||||
self.options
|
||||
}
|
||||
|
||||
/// Whether NaN and infinities are forbidden.
|
||||
pub const fn finite(self) -> bool {
|
||||
self.finite
|
||||
}
|
||||
|
||||
/// Whether numeric zero is forbidden.
|
||||
pub const fn nonzero(self) -> bool {
|
||||
self.nonzero
|
||||
}
|
||||
}
|
||||
|
||||
/// Iterator over validated field records in canonical artifact order.
|
||||
@@ -428,7 +455,15 @@ impl<'a> KcfgArtifact<'a> {
|
||||
}
|
||||
|
||||
/// Validates and borrows one board-agnostic KCFG artifact.
|
||||
pub fn decode_kcfg(input: &[u8]) -> Result<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 {
|
||||
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]);
|
||||
field_ids.fill(0);
|
||||
let mut cursor = FIELDS_OFFSET;
|
||||
for _ in 0..field_count {
|
||||
let (_, next) = parse_field(input, cursor, checksum_offset)?;
|
||||
let (field, next) = parse_field(input, cursor, checksum_offset)?;
|
||||
if mark_seen(field_ids, usize::from(field.id.get())) {
|
||||
return Err(KcfgDecodeError::DuplicateField(field.id));
|
||||
}
|
||||
cursor = next;
|
||||
}
|
||||
if cursor != checksum_offset {
|
||||
@@ -466,7 +505,6 @@ pub fn decode_kcfg(input: &[u8]) -> Result<KcfgArtifact<'_>, KcfgDecodeError> {
|
||||
field_count,
|
||||
restart_required: artifact_flags & KCFG_RESTART_REQUIRED != 0,
|
||||
};
|
||||
reject_duplicate_fields(&artifact)?;
|
||||
Ok(artifact)
|
||||
}
|
||||
|
||||
@@ -482,7 +520,7 @@ pub fn kcfg_length<C: ConfigModel>() -> Result<usize, KcfgEncodeError> {
|
||||
))?;
|
||||
validate_descriptor(descriptor)?;
|
||||
let width = descriptor.kind.width();
|
||||
let values = 2
|
||||
let values = 3
|
||||
+ usize::from(descriptor.minimum.is_some())
|
||||
+ usize::from(descriptor.maximum.is_some());
|
||||
let mut field_length = FIELD_PREFIX_LENGTH
|
||||
@@ -507,11 +545,7 @@ pub fn kcfg_length<C: ConfigModel>() -> Result<usize, KcfgEncodeError> {
|
||||
Ok(length)
|
||||
}
|
||||
|
||||
/// Prepared immutable metadata for one canonical generated artifact.
|
||||
///
|
||||
/// A file endpoint can retain this small value for the lifetime of an open
|
||||
/// read and reconstruct range exporters without recomputing the file length or
|
||||
/// either checksum for every transport chunk.
|
||||
/// Cached length and checksums for an immutable export.
|
||||
#[derive(Clone, Copy)]
|
||||
pub struct KcfgExportPlan {
|
||||
length: usize,
|
||||
@@ -520,12 +554,8 @@ pub struct KcfgExportPlan {
|
||||
restart_required: bool,
|
||||
}
|
||||
|
||||
/// Allocation-free view of one canonical generated artifact.
|
||||
///
|
||||
/// The exporter can fill an arbitrary transport range directly from the typed
|
||||
/// configuration. Firmware therefore does not need a buffer sized for the
|
||||
/// complete public file. Construction also computes both the embedded KCFG
|
||||
/// checksum and the checksum of the complete file bytes.
|
||||
/// Exports arbitrary byte ranges directly from typed configuration.
|
||||
/// Construction computes both the KCFG checksum and the complete-file checksum.
|
||||
pub struct KcfgExporter<'a, C: ConfigModel> {
|
||||
current: &'a C,
|
||||
factory: &'a C,
|
||||
@@ -611,19 +641,31 @@ impl<'a, C: ConfigModel> KcfgExporter<'a, C> {
|
||||
}
|
||||
}
|
||||
|
||||
/// Complete requested values and the exact committed base observed by an editor.
|
||||
#[derive(Clone, Debug, PartialEq)]
|
||||
pub struct KcfgUpdate<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.
|
||||
///
|
||||
/// Only the candidate plus eight scalar bytes are retained. Generated
|
||||
/// metadata and factory bytes are checked as they arrive, and a field setter
|
||||
/// Only two typed candidates plus fixed scalar/parser state are retained.
|
||||
/// Generated metadata and factory bytes are checked as they arrive, and a field setter
|
||||
/// is called only after its complete canonical record has been received.
|
||||
/// Discarding the importer discards every partially imported candidate.
|
||||
pub struct KcfgImporter<C: ConfigModel> {
|
||||
candidate: C,
|
||||
expected_base: C,
|
||||
error: Option<KcfgDecodeError>,
|
||||
phase: ImportPhase,
|
||||
header_offset: usize,
|
||||
field_index: u16,
|
||||
field_offset: usize,
|
||||
current_bytes: [u8; 8],
|
||||
expected_base_bytes: [u8; 8],
|
||||
checksum_bytes: [u8; CHECKSUM_LENGTH],
|
||||
checksum_offset: usize,
|
||||
crc: Crc32c,
|
||||
@@ -646,31 +688,48 @@ impl<C: ConfigModel> KcfgImporter<C> {
|
||||
emit_body(factory, factory, false, |_| true)?;
|
||||
Ok(Self {
|
||||
candidate: factory.clone(),
|
||||
expected_base: factory.clone(),
|
||||
error: None,
|
||||
phase: ImportPhase::Header,
|
||||
header_offset: 0,
|
||||
field_index: 0,
|
||||
field_offset: 0,
|
||||
current_bytes: [0; 8],
|
||||
expected_base_bytes: [0; 8],
|
||||
checksum_bytes: [0; CHECKSUM_LENGTH],
|
||||
checksum_offset: 0,
|
||||
crc: Crc32c::new(),
|
||||
})
|
||||
}
|
||||
|
||||
/// Consumes the next contiguous file bytes.
|
||||
/// Consumes the next contiguous file bytes. Any parser error is terminal:
|
||||
/// subsequent pushes and finish return the original error.
|
||||
pub fn push(&mut self, factory: &C, input: &[u8]) -> Result<(), KcfgDecodeError> {
|
||||
if let Some(error) = self.error {
|
||||
return Err(error);
|
||||
}
|
||||
for byte in input {
|
||||
self.push_byte(factory, *byte)?;
|
||||
if let Err(error) = self.push_byte(factory, *byte) {
|
||||
self.error = Some(error);
|
||||
return Err(error);
|
||||
}
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Returns the complete candidate after checksum and schema validation.
|
||||
pub fn finish(self) -> Result<C, KcfgDecodeError> {
|
||||
/// Returns requested and expected-base candidates after checksum and schema
|
||||
/// validation. Relational validation belongs to the manager at commit time.
|
||||
pub fn finish(self) -> Result<KcfgUpdate<C>, KcfgDecodeError> {
|
||||
if let Some(error) = self.error {
|
||||
return Err(error);
|
||||
}
|
||||
if self.phase != ImportPhase::Done {
|
||||
return Err(KcfgDecodeError::Truncated);
|
||||
}
|
||||
Ok(self.candidate)
|
||||
Ok(KcfgUpdate {
|
||||
candidate: self.candidate,
|
||||
expected_base: self.expected_base,
|
||||
})
|
||||
}
|
||||
|
||||
fn push_byte(&mut self, factory: &C, byte: u8) -> Result<(), KcfgDecodeError> {
|
||||
@@ -743,6 +802,7 @@ impl<C: ConfigModel> KcfgImporter<C> {
|
||||
}
|
||||
match expected_field_byte(descriptor, factory, self.field_offset) {
|
||||
Some(ExpectedFieldByte::Current(index)) => self.current_bytes[index] = byte,
|
||||
Some(ExpectedFieldByte::ExpectedBase(index)) => self.expected_base_bytes[index] = byte,
|
||||
Some(ExpectedFieldByte::Factory(expected)) if byte != expected => {
|
||||
return Err(KcfgDecodeError::FactoryMismatch(descriptor.id));
|
||||
}
|
||||
@@ -762,7 +822,16 @@ impl<C: ConfigModel> KcfgImporter<C> {
|
||||
self.candidate
|
||||
.set_field(descriptor.id, value)
|
||||
.map_err(KcfgDecodeError::InvalidWrite)?;
|
||||
let base = value_from_bytes(
|
||||
descriptor.id,
|
||||
descriptor.kind,
|
||||
&self.expected_base_bytes[..width],
|
||||
)?;
|
||||
self.expected_base
|
||||
.set_field(descriptor.id, base)
|
||||
.map_err(KcfgDecodeError::InvalidWrite)?;
|
||||
self.current_bytes = [0; 8];
|
||||
self.expected_base_bytes = [0; 8];
|
||||
self.field_offset = 0;
|
||||
self.field_index += 1;
|
||||
if self.field_index == C::field_count() {
|
||||
@@ -809,8 +878,8 @@ fn encode_kcfg<'a, C: ConfigModel>(
|
||||
///
|
||||
/// A board-agnostic host can retain the received artifact bytes, present the
|
||||
/// borrowed metadata to its UI, and provide edited values by stable field ID.
|
||||
/// The generated metadata, schema, factory values, and enum choices are copied
|
||||
/// unchanged and the checksum is regenerated here.
|
||||
/// The expected-base scalars, metadata, schema, factory values, and enum choices
|
||||
/// are copied unchanged and the checksum is regenerated here.
|
||||
pub fn encode_edited_kcfg<'a, F>(
|
||||
source: &KcfgArtifact<'_>,
|
||||
restart_required: bool,
|
||||
@@ -848,31 +917,9 @@ where
|
||||
|
||||
#[cfg(test)]
|
||||
fn decode_kcfg_for<C: ConfigModel>(input: &[u8], factory: &C) -> Result<C, KcfgDecodeError> {
|
||||
validate_model::<C>().map_err(KcfgDecodeError::InvalidModel)?;
|
||||
let artifact = decode_kcfg(input)?;
|
||||
if artifact.schema_id != C::SCHEMA_ID {
|
||||
return Err(KcfgDecodeError::WrongSchema);
|
||||
}
|
||||
if artifact.field_count != C::field_count() {
|
||||
return Err(KcfgDecodeError::WrongFieldCount);
|
||||
}
|
||||
|
||||
let mut candidate = factory.clone();
|
||||
for (index, field) in artifact.fields().enumerate() {
|
||||
let descriptor = C::descriptor(index as u16).ok_or(KcfgDecodeError::InvalidModel(
|
||||
SnapshotModelError::MissingDescriptor(index as u16),
|
||||
))?;
|
||||
if !same_descriptor(&field, descriptor) {
|
||||
return Err(KcfgDecodeError::DescriptorMismatch(field.id));
|
||||
}
|
||||
if factory.field_value(field.id) != Some(field.factory) {
|
||||
return Err(KcfgDecodeError::FactoryMismatch(field.id));
|
||||
}
|
||||
candidate
|
||||
.set_field(field.id, field.current)
|
||||
.map_err(KcfgDecodeError::InvalidWrite)?;
|
||||
}
|
||||
Ok(candidate)
|
||||
let mut importer = KcfgImporter::new(factory).expect("valid test model");
|
||||
importer.push(factory, input)?;
|
||||
Ok(importer.finish()?.candidate)
|
||||
}
|
||||
|
||||
fn emit_body<C, F>(
|
||||
@@ -940,6 +987,10 @@ where
|
||||
if !emit_value(factory, &mut emit) {
|
||||
return Ok(());
|
||||
}
|
||||
// A fresh export establishes the exact base for subsequent host edits.
|
||||
if !emit_value(current, &mut emit) {
|
||||
return Ok(());
|
||||
}
|
||||
if let Some(minimum) = descriptor.minimum
|
||||
&& !emit_value(minimum, &mut emit)
|
||||
{
|
||||
@@ -973,16 +1024,17 @@ fn validate_descriptor(descriptor: &crate::FieldDescriptor) -> Result<(), KcfgEn
|
||||
|| descriptor.unit.contains('\0')
|
||||
|| descriptor.enum_options.len() > u8::MAX as usize
|
||||
|| (!descriptor.enum_options.is_empty() && descriptor.kind != ValueKind::U8)
|
||||
|| (descriptor.finite && !matches!(descriptor.kind, ValueKind::F32 | ValueKind::F64))
|
||||
|| (descriptor.nonzero && descriptor.kind == ValueKind::Bool)
|
||||
{
|
||||
return Err(KcfgEncodeError::InvalidMetadata(descriptor.id));
|
||||
}
|
||||
for (index, (value, label)) in descriptor.enum_options.iter().enumerate() {
|
||||
let mut seen = [0_u8; 32];
|
||||
for (value, label) in descriptor.enum_options {
|
||||
if label.is_empty()
|
||||
|| label.len() > u8::MAX as usize
|
||||
|| label.contains('\0')
|
||||
|| descriptor.enum_options[..index]
|
||||
.iter()
|
||||
.any(|previous| previous.0 == *value)
|
||||
|| mark_seen(&mut seen, usize::from(*value))
|
||||
{
|
||||
return Err(KcfgEncodeError::InvalidMetadata(descriptor.id));
|
||||
}
|
||||
@@ -995,6 +1047,8 @@ fn parse_field(
|
||||
start: usize,
|
||||
limit: usize,
|
||||
) -> Result<(KcfgField<'_>, usize), KcfgDecodeError> {
|
||||
#[cfg(test)]
|
||||
tests::FIELD_PARSES.with(|count| count.set(count.get() + 1));
|
||||
let prefix_end = start
|
||||
.checked_add(FIELD_PREFIX_LENGTH)
|
||||
.ok_or(KcfgDecodeError::Truncated)?;
|
||||
@@ -1007,7 +1061,10 @@ fn parse_field(
|
||||
let kind =
|
||||
ValueKind::try_from_code(raw_kind).ok_or(KcfgDecodeError::InvalidValueKind(raw_kind))?;
|
||||
let flags = bytes[start + 3];
|
||||
if flags & !FIELD_FLAGS != 0 {
|
||||
if flags & !FIELD_FLAGS != 0
|
||||
|| (flags & FIELD_FINITE != 0 && !matches!(kind, ValueKind::F32 | ValueKind::F64))
|
||||
|| (flags & FIELD_NONZERO != 0 && kind == ValueKind::Bool)
|
||||
{
|
||||
return Err(KcfgDecodeError::InvalidFieldFlags(id));
|
||||
}
|
||||
let key_length = usize::from(bytes[start + 4]);
|
||||
@@ -1021,6 +1078,7 @@ fn parse_field(
|
||||
let current_offset = cursor;
|
||||
let current = parse_value(bytes, &mut cursor, limit, id, kind)?;
|
||||
let factory = parse_value(bytes, &mut cursor, limit, id, kind)?;
|
||||
let expected_base = parse_value(bytes, &mut cursor, limit, id, kind)?;
|
||||
let minimum = if flags & FIELD_HAS_MINIMUM != 0 {
|
||||
Some(parse_value(bytes, &mut cursor, limit, id, kind)?)
|
||||
} else {
|
||||
@@ -1047,7 +1105,8 @@ fn parse_field(
|
||||
canonical_text(&bytes[key_end..unit_end], true).ok_or(KcfgDecodeError::InvalidText(id))?;
|
||||
cursor = unit_end;
|
||||
let options_start = cursor;
|
||||
for option_index in 0..enum_count {
|
||||
let mut seen = [0_u8; 32];
|
||||
for _ in 0..enum_count {
|
||||
if cursor.checked_add(2).is_none_or(|end| end > limit) {
|
||||
return Err(KcfgDecodeError::Truncated);
|
||||
}
|
||||
@@ -1062,11 +1121,7 @@ fn parse_field(
|
||||
if canonical_text(&bytes[cursor + 2..label_end], false).is_none() {
|
||||
return Err(KcfgDecodeError::InvalidText(id));
|
||||
}
|
||||
let mut previous = KcfgEnumOptions {
|
||||
bytes: &bytes[options_start..cursor],
|
||||
remaining: option_index,
|
||||
};
|
||||
if previous.any(|option| option.value == value) {
|
||||
if mark_seen(&mut seen, usize::from(value)) {
|
||||
return Err(KcfgDecodeError::DuplicateEnumValue(id));
|
||||
}
|
||||
cursor = label_end;
|
||||
@@ -1078,6 +1133,7 @@ fn parse_field(
|
||||
kind,
|
||||
current,
|
||||
factory,
|
||||
expected_base,
|
||||
minimum,
|
||||
maximum,
|
||||
key,
|
||||
@@ -1087,22 +1143,21 @@ fn parse_field(
|
||||
remaining: enum_count,
|
||||
},
|
||||
current_offset,
|
||||
finite: flags & FIELD_FINITE != 0,
|
||||
nonzero: flags & FIELD_NONZERO != 0,
|
||||
},
|
||||
cursor,
|
||||
))
|
||||
}
|
||||
|
||||
fn reject_duplicate_fields(artifact: &KcfgArtifact<'_>) -> Result<(), KcfgDecodeError> {
|
||||
for (index, field) in artifact.fields().enumerate() {
|
||||
if artifact
|
||||
.fields()
|
||||
.take(index)
|
||||
.any(|previous| previous.id == field.id)
|
||||
{
|
||||
return Err(KcfgDecodeError::DuplicateField(field.id));
|
||||
}
|
||||
}
|
||||
Ok(())
|
||||
fn mark_seen(bitmap: &mut [u8], value: usize) -> bool {
|
||||
#[cfg(test)]
|
||||
tests::BITMAP_PROBES.with(|count| count.set(count.get() + 1));
|
||||
let byte = &mut bitmap[value / 8];
|
||||
let bit = 1 << (value % 8);
|
||||
let duplicate = *byte & bit != 0;
|
||||
*byte |= bit;
|
||||
duplicate
|
||||
}
|
||||
|
||||
fn parse_value(
|
||||
@@ -1132,21 +1187,6 @@ fn value_from_bytes(
|
||||
ConfigValue::from_le_bytes(kind, bytes).ok_or(KcfgDecodeError::InvalidValue(id))
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
fn same_descriptor(field: &KcfgField<'_>, descriptor: &crate::FieldDescriptor) -> bool {
|
||||
field.id == descriptor.id
|
||||
&& field.kind == descriptor.kind
|
||||
&& field.minimum == descriptor.minimum
|
||||
&& field.maximum == descriptor.maximum
|
||||
&& field.key == descriptor.display_key
|
||||
&& field.unit == descriptor.unit
|
||||
&& field
|
||||
.options
|
||||
.zip(descriptor.enum_options.iter().copied())
|
||||
.all(|(actual, expected)| (actual.value, actual.label) == expected)
|
||||
&& field.options.len() == descriptor.enum_options.len()
|
||||
}
|
||||
|
||||
fn canonical_text(bytes: &[u8], allow_empty: bool) -> Option<&str> {
|
||||
let text = str::from_utf8(bytes).ok()?;
|
||||
if (!allow_empty && text.is_empty()) || text.contains('\0') {
|
||||
@@ -1166,6 +1206,7 @@ fn read_u32_be(bytes: &[u8]) -> u32 {
|
||||
|
||||
enum ExpectedFieldByte {
|
||||
Current(usize),
|
||||
ExpectedBase(usize),
|
||||
Factory(u8),
|
||||
Descriptor(u8),
|
||||
}
|
||||
@@ -1202,6 +1243,10 @@ fn expected_field_byte(
|
||||
)));
|
||||
}
|
||||
cursor += width;
|
||||
if offset < cursor + width {
|
||||
return Some(ExpectedFieldByte::ExpectedBase(offset - cursor));
|
||||
}
|
||||
cursor += width;
|
||||
if let Some(minimum) = descriptor.minimum {
|
||||
if offset < cursor + width {
|
||||
return Some(ExpectedFieldByte::Descriptor(value_byte(
|
||||
@@ -1254,7 +1299,7 @@ fn expected_field_byte(
|
||||
fn field_record_length(descriptor: &crate::FieldDescriptor) -> usize {
|
||||
let width = descriptor.kind.width();
|
||||
let value_count =
|
||||
2 + usize::from(descriptor.minimum.is_some()) + usize::from(descriptor.maximum.is_some());
|
||||
3 + usize::from(descriptor.minimum.is_some()) + usize::from(descriptor.maximum.is_some());
|
||||
FIELD_PREFIX_LENGTH
|
||||
+ width * value_count
|
||||
+ descriptor.display_key.len()
|
||||
@@ -1269,6 +1314,8 @@ fn field_record_length(descriptor: &crate::FieldDescriptor) -> usize {
|
||||
fn field_flags(descriptor: &crate::FieldDescriptor) -> u8 {
|
||||
(u8::from(descriptor.minimum.is_some()) * FIELD_HAS_MINIMUM)
|
||||
| (u8::from(descriptor.maximum.is_some()) * FIELD_HAS_MAXIMUM)
|
||||
| (u8::from(descriptor.finite) * FIELD_FINITE)
|
||||
| (u8::from(descriptor.nonzero) * FIELD_NONZERO)
|
||||
}
|
||||
|
||||
fn value_byte(value: ConfigValue, offset: usize) -> u8 {
|
||||
@@ -1321,8 +1368,78 @@ impl<'a> RangeSink<'a> {
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
extern crate std;
|
||||
|
||||
use super::*;
|
||||
use crate::{ConfigViolation, FieldDescriptor, ValidationRule};
|
||||
use std::{vec, vec::Vec};
|
||||
|
||||
std::thread_local! {
|
||||
pub(super) static FIELD_PARSES: core::cell::Cell<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 PORT: FieldId = FieldId::new(2);
|
||||
@@ -1337,6 +1454,8 @@ mod tests {
|
||||
maximum: Some(ConfigValue::U8(100)),
|
||||
enum_options: &[],
|
||||
unit: "Hz",
|
||||
finite: false,
|
||||
nonzero: false,
|
||||
},
|
||||
FieldDescriptor {
|
||||
id: PORT,
|
||||
@@ -1346,6 +1465,8 @@ mod tests {
|
||||
maximum: Some(ConfigValue::U16(u16::MAX)),
|
||||
enum_options: &[],
|
||||
unit: "id",
|
||||
finite: false,
|
||||
nonzero: false,
|
||||
},
|
||||
FieldDescriptor {
|
||||
id: MODE,
|
||||
@@ -1355,6 +1476,8 @@ mod tests {
|
||||
maximum: None,
|
||||
enum_options: MODE_OPTIONS,
|
||||
unit: "level",
|
||||
finite: false,
|
||||
nonzero: false,
|
||||
},
|
||||
];
|
||||
|
||||
@@ -1472,7 +1595,8 @@ mod tests {
|
||||
};
|
||||
let mut bytes = [0; 256];
|
||||
let encoded = encode_kcfg(¤t, &factory, true, &mut bytes).unwrap();
|
||||
let artifact = decode_kcfg(encoded).unwrap();
|
||||
let artifact = decode_kcfg(encoded, &mut [0; KCFG_FIELD_ID_SCRATCH_BYTES]).unwrap();
|
||||
assert_eq!(encoded[VERSION_OFFSET], 3);
|
||||
assert_eq!(artifact.schema_id(), Candidate::SCHEMA_ID);
|
||||
assert!(artifact.restart_required());
|
||||
let mut fields = artifact.fields();
|
||||
@@ -1489,6 +1613,20 @@ mod tests {
|
||||
}
|
||||
})
|
||||
.unwrap();
|
||||
let edited_artifact = decode_kcfg(edited, &mut [0; KCFG_FIELD_ID_SCRATCH_BYTES]).unwrap();
|
||||
for field in edited_artifact.fields() {
|
||||
assert_eq!(
|
||||
field.expected_base_value(),
|
||||
current.field_value(field.id()).unwrap()
|
||||
);
|
||||
}
|
||||
let mut importer = KcfgImporter::new(&factory).unwrap();
|
||||
for byte in edited {
|
||||
importer
|
||||
.push(&factory, core::slice::from_ref(byte))
|
||||
.unwrap();
|
||||
}
|
||||
assert_eq!(importer.finish().unwrap().expected_base, current);
|
||||
assert_eq!(
|
||||
decode_kcfg_for(edited, &factory).unwrap(),
|
||||
Candidate {
|
||||
@@ -1553,7 +1691,9 @@ mod tests {
|
||||
for chunk in contiguous[..exporter.length()].chunks(chunk_length) {
|
||||
importer.push(&factory, chunk).unwrap();
|
||||
}
|
||||
assert_eq!(importer.finish().unwrap(), current);
|
||||
let update = importer.finish().unwrap();
|
||||
assert_eq!(update.candidate, current);
|
||||
assert_eq!(update.expected_base, current);
|
||||
}
|
||||
|
||||
for truncated_length in 0..exporter.length() {
|
||||
@@ -1597,11 +1737,11 @@ mod tests {
|
||||
.unwrap()
|
||||
.len();
|
||||
|
||||
storage[FIELDS_OFFSET + FIELD_PREFIX_LENGTH + 4] ^= 1;
|
||||
storage[FIELDS_OFFSET + FIELD_PREFIX_LENGTH + 5] ^= 1;
|
||||
let checksum_offset = length - CHECKSUM_LENGTH;
|
||||
let checksum = crc32c(&storage[..checksum_offset]);
|
||||
storage[checksum_offset..length].copy_from_slice(&checksum.to_be_bytes());
|
||||
assert!(decode_kcfg(&storage[..length]).is_ok());
|
||||
assert!(decode_kcfg(&storage[..length], &mut [0; KCFG_FIELD_ID_SCRATCH_BYTES]).is_ok());
|
||||
assert!(matches!(
|
||||
decode_kcfg_for(&storage[..length], &factory),
|
||||
Err(KcfgDecodeError::DescriptorMismatch(RATE))
|
||||
@@ -1609,8 +1749,42 @@ mod tests {
|
||||
|
||||
storage[checksum_offset] ^= 1;
|
||||
assert!(matches!(
|
||||
decode_kcfg(&storage[..length]),
|
||||
decode_kcfg(&storage[..length], &mut [0; KCFG_FIELD_ID_SCRATCH_BYTES]),
|
||||
Err(KcfgDecodeError::ChecksumMismatch)
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn parser_errors_are_terminal_including_checksum_and_trailing_data() {
|
||||
let factory = factory();
|
||||
let mut bytes = [0; 256];
|
||||
let length = encode_kcfg(&factory, &factory, false, &mut bytes)
|
||||
.unwrap()
|
||||
.len();
|
||||
// Header/version, metadata, scalar validation, and checksum errors must
|
||||
// never allow a later push or finish to revive a partial update.
|
||||
for (offset, bad_byte) in [
|
||||
(0, b'X'),
|
||||
(VERSION_OFFSET, 1),
|
||||
(FIELDS_OFFSET, 42),
|
||||
(FIELDS_OFFSET + FIELD_PREFIX_LENGTH, 0),
|
||||
(FIELDS_OFFSET + FIELD_PREFIX_LENGTH + 2, 0),
|
||||
(length - 1, bytes[length - 1] ^ 1),
|
||||
] {
|
||||
let mut malformed = bytes;
|
||||
malformed[offset] = bad_byte;
|
||||
let mut importer = KcfgImporter::new(&factory).unwrap();
|
||||
let error = importer.push(&factory, &malformed[..length]).unwrap_err();
|
||||
assert_eq!(importer.push(&factory, &bytes[..length]), Err(error));
|
||||
assert_eq!(importer.push(&factory, &[]), Err(error));
|
||||
assert_eq!(importer.finish(), Err(error));
|
||||
}
|
||||
let mut importer = KcfgImporter::new(&factory).unwrap();
|
||||
importer.push(&factory, &bytes[..length]).unwrap();
|
||||
assert_eq!(
|
||||
importer.push(&factory, &[0]),
|
||||
Err(KcfgDecodeError::TrailingData)
|
||||
);
|
||||
assert_eq!(importer.finish(), Err(KcfgDecodeError::TrailingData));
|
||||
}
|
||||
}
|
||||
@@ -22,6 +22,8 @@ pub enum ValidationRule {
|
||||
UnsupportedFormat = 7,
|
||||
/// A scalar does not name a declared variant of its typed enum field.
|
||||
InvalidEnumValue = 8,
|
||||
/// An ordered range cannot contain NaN.
|
||||
Unordered = 9,
|
||||
}
|
||||
|
||||
impl ValidationRule {
|
||||
@@ -41,6 +43,7 @@ impl ValidationRule {
|
||||
6 => Self::UnsupportedCapability,
|
||||
7 => Self::UnsupportedFormat,
|
||||
8 => Self::InvalidEnumValue,
|
||||
9 => Self::Unordered,
|
||||
_ => return None,
|
||||
};
|
||||
Some(rule)
|
||||
|
||||
@@ -3,11 +3,7 @@
|
||||
use crate::{ConfigViolation, ValidationRule};
|
||||
use kaptan_err_reportable::KaptanError;
|
||||
|
||||
/// Maximum choices representable by one enum-backed field.
|
||||
///
|
||||
/// Enum values and indexed discovery use their native `u8` domain. Options
|
||||
/// are immutable flash metadata, so no smaller runtime resource limit is
|
||||
/// necessary.
|
||||
/// Maximum choice count representable by indexed `u8` discovery.
|
||||
pub(crate) const MAX_ENUM_OPTIONS_PER_FIELD: usize = u8::MAX as usize;
|
||||
/// Maximum UTF-8 bytes advertised for one enum variant name.
|
||||
pub(crate) const MAX_ENUM_OPTION_LABEL_BYTES: usize = 32;
|
||||
@@ -47,9 +43,9 @@ impl FieldId {
|
||||
|
||||
/// The scalar representation of a configuration value.
|
||||
///
|
||||
/// The variants deliberately preserve native widths: durations, for example,
|
||||
/// remain `u64` microseconds instead of being lossy display-unit conversions.
|
||||
#[derive(Clone, Copy, Debug, PartialEq)]
|
||||
/// Preserves native widths. Equality compares kind and raw bits, including
|
||||
/// signed zero and NaN payloads.
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
pub enum ConfigValue {
|
||||
/// Boolean value.
|
||||
Bool(bool),
|
||||
@@ -69,6 +65,16 @@ pub enum ConfigValue {
|
||||
F64(f64),
|
||||
}
|
||||
|
||||
/// Configuration identity is native kind plus exact scalar bits. Signed zeros
|
||||
/// differ and identical NaN payloads compare equal, including in optional values.
|
||||
impl PartialEq for ConfigValue {
|
||||
fn eq(&self, other: &Self) -> bool {
|
||||
self.kind() == other.kind() && self.raw_bits() == other.raw_bits()
|
||||
}
|
||||
}
|
||||
|
||||
impl Eq for ConfigValue {}
|
||||
|
||||
/// Failure to project a generic configuration scalar into its native type.
|
||||
#[doc(hidden)]
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
@@ -79,11 +85,7 @@ pub enum ScalarValueError {
|
||||
OutOfDomain,
|
||||
}
|
||||
|
||||
/// Native representation used by generated configuration fields.
|
||||
///
|
||||
/// Generic artifacts and private snapshots retain the native scalar kind while
|
||||
/// board and runtime structures keep the narrow or enum type that models the
|
||||
/// value.
|
||||
/// Conversion between native field types and configuration scalars.
|
||||
#[doc(hidden)]
|
||||
pub trait ConfigScalar: Copy {
|
||||
/// Generic scalar kind used at schema and transport boundaries.
|
||||
@@ -246,6 +248,10 @@ pub struct FieldDescriptor {
|
||||
pub enum_options: &'static [(u8, &'static str)],
|
||||
/// Human-readable unit for schema metadata.
|
||||
pub unit: &'static str,
|
||||
/// Reject NaN and infinities. Only meaningful for floating-point fields.
|
||||
pub finite: bool,
|
||||
/// Reject numeric zero, including either floating-point signed zero.
|
||||
pub nonzero: bool,
|
||||
}
|
||||
|
||||
/// Failure while changing one candidate field.
|
||||
|
||||
@@ -4,9 +4,12 @@
|
||||
//! independently erasable slots; the generic scalar snapshot codec supplies
|
||||
//! the payload. The previous committed slot is never erased until the complete
|
||||
//! replacement is programmed, verified, and marked committed.
|
||||
//! A persistence error can occur after that marker is durable, so it does not
|
||||
//! establish which candidate recovery will select.
|
||||
|
||||
use crate::{
|
||||
ConfigModel, ConfigStore, checksum::Crc32c, decode_scalar_snapshot, encode_scalar_snapshot,
|
||||
ConfigModel, ConfigStore, StoreRecovery, checksum::Crc32c, decode_scalar_snapshot,
|
||||
encode_scalar_snapshot,
|
||||
};
|
||||
|
||||
const MAGIC: u32 = u32::from_le_bytes(*b"KJNL");
|
||||
@@ -213,23 +216,25 @@ where
|
||||
Ok(selected.map(|record| record.candidate))
|
||||
}
|
||||
|
||||
fn load_matching<P>(&mut self, factory: &C, mut compatible: P) -> Result<Option<C>, Self::Error>
|
||||
where
|
||||
P: FnMut(&C) -> bool,
|
||||
{
|
||||
self.geometry()?;
|
||||
let left = self.record(Slot::Left, factory)?;
|
||||
let right = self.record(Slot::Right, factory)?;
|
||||
let left = left.filter(|record| compatible(&record.candidate));
|
||||
let right = right.filter(|record| compatible(&record.candidate));
|
||||
let selected = match (left, right) {
|
||||
(Some(left), Some(right)) if right.sequence > left.sequence => Some(right),
|
||||
(Some(left), Some(_)) | (Some(left), None) => Some(left),
|
||||
(None, Some(right)) => Some(right),
|
||||
(None, None) => None,
|
||||
};
|
||||
self.protected_slot = selected.as_ref().map(|record| record.slot);
|
||||
Ok(selected.map(|record| record.candidate))
|
||||
fn load_recovery(&mut self, factory: &C) -> Result<StoreRecovery<C>, Self::Error> {
|
||||
if let Some(candidate) = self.load(factory)? {
|
||||
return Ok(StoreRecovery::Committed(candidate));
|
||||
}
|
||||
// Only the no-readable-record case needs an erased-byte scan. Do not
|
||||
// infer factory state from erased headers with occupied payload bytes.
|
||||
let mut bytes = [0; VERIFY_CHUNK_LENGTH];
|
||||
for slot in 0..2 {
|
||||
for offset in (0..self.flash.slot_size()).step_by(bytes.len()) {
|
||||
let length = bytes.len().min(self.flash.slot_size() - offset);
|
||||
self.flash
|
||||
.read(slot, offset, &mut bytes[..length])
|
||||
.map_err(JournalError::Storage)?;
|
||||
if bytes[..length].iter().any(|&byte| byte != 0xff) {
|
||||
return Ok(StoreRecovery::Unreadable);
|
||||
}
|
||||
}
|
||||
}
|
||||
Ok(StoreRecovery::Empty)
|
||||
}
|
||||
|
||||
fn persist(&mut self, candidate: &C) -> Result<(), Self::Error> {
|
||||
@@ -294,6 +299,8 @@ where
|
||||
self.flash
|
||||
.program_word(target_slot.index(), COMMIT_OFFSET, COMMIT_MARKER)
|
||||
.map_err(JournalError::Storage)?;
|
||||
// The candidate is now durable even if this final read fails. Callers
|
||||
// must recover from storage rather than assume an error rolled it back.
|
||||
let Some(record) = self.record(target_slot, candidate)? else {
|
||||
return Err(JournalError::Verification);
|
||||
};
|
||||
@@ -330,7 +337,8 @@ fn read_u64(bytes: &[u8]) -> u64 {
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::{
|
||||
ConfigValue, ConfigViolation, FieldDescriptor, FieldId, FieldWriteError, ValueKind,
|
||||
ConfigManager, ConfigValue, ConfigViolation, FieldDescriptor, FieldId, FieldWriteError,
|
||||
ReplaceError, ValueKind,
|
||||
};
|
||||
|
||||
const VALUE: FieldId = FieldId::new(401);
|
||||
@@ -342,6 +350,8 @@ mod tests {
|
||||
maximum: Some(ConfigValue::U32(100)),
|
||||
enum_options: &[],
|
||||
unit: "ticks",
|
||||
finite: false,
|
||||
nonzero: false,
|
||||
};
|
||||
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
@@ -398,15 +408,26 @@ mod tests {
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
enum FakeError {
|
||||
PowerLoss,
|
||||
Read,
|
||||
Program,
|
||||
Bounds,
|
||||
ZeroToOne,
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
enum CommitFailure {
|
||||
Program,
|
||||
Read(usize),
|
||||
CorruptRead(usize),
|
||||
}
|
||||
|
||||
#[derive(Clone)]
|
||||
struct FakeNor {
|
||||
slots: [[u8; 256]; 2],
|
||||
operations: usize,
|
||||
fail_at: Option<usize>,
|
||||
commit_failure: Option<CommitFailure>,
|
||||
commit_written: bool,
|
||||
}
|
||||
|
||||
impl FakeNor {
|
||||
@@ -415,6 +436,8 @@ mod tests {
|
||||
slots: [[0xff; 256]; 2],
|
||||
operations: 0,
|
||||
fail_at: None,
|
||||
commit_failure: None,
|
||||
commit_written: false,
|
||||
}
|
||||
}
|
||||
|
||||
@@ -446,6 +469,13 @@ mod tests {
|
||||
.and_then(|slot| slot.get(offset..offset + output.len()))
|
||||
.ok_or(FakeError::Bounds)?;
|
||||
output.copy_from_slice(source);
|
||||
if self.commit_written {
|
||||
match self.commit_failure {
|
||||
Some(CommitFailure::Read(at)) if at == offset => return Err(FakeError::Read),
|
||||
Some(CommitFailure::CorruptRead(at)) if at == offset => output[0] ^= 1,
|
||||
_ => {}
|
||||
}
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
@@ -479,6 +509,12 @@ mod tests {
|
||||
return Err(FakeError::ZeroToOne);
|
||||
}
|
||||
destination.copy_from_slice(&bytes);
|
||||
if offset == COMMIT_OFFSET && word == COMMIT_MARKER {
|
||||
self.commit_written = self.commit_failure.is_some();
|
||||
if matches!(self.commit_failure, Some(CommitFailure::Program)) {
|
||||
return Err(FakeError::Program);
|
||||
}
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
@@ -486,6 +522,78 @@ mod tests {
|
||||
type Journal = DualSlotJournal<FakeNor, 64>;
|
||||
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]
|
||||
fn alternates_slots_and_recovers_the_newest_record() {
|
||||
let mut journal = Journal::new(FakeNor::erased());
|
||||
@@ -538,67 +646,4 @@ mod tests {
|
||||
let mut recovered = Journal::new(flash);
|
||||
assert_eq!(recovered.load(&FACTORY).unwrap(), Some(Candidate(30)));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn compatibility_filter_can_select_the_older_structurally_valid_slot() {
|
||||
let mut journal = Journal::new(FakeNor::erased());
|
||||
journal.persist(&Candidate(30)).unwrap();
|
||||
journal.persist(&Candidate(40)).unwrap();
|
||||
|
||||
assert_eq!(
|
||||
journal
|
||||
.load_matching(&FACTORY, |candidate| candidate.0 <= 35)
|
||||
.unwrap(),
|
||||
Some(Candidate(30))
|
||||
);
|
||||
assert_eq!(
|
||||
journal
|
||||
.load_matching(&FACTORY, |candidate| candidate.0 < 10)
|
||||
.unwrap(),
|
||||
None
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn replacing_an_older_compatible_record_preserves_it_across_every_interruption() {
|
||||
let mut initial = Journal::new(FakeNor::erased());
|
||||
initial.persist(&Candidate(30)).unwrap();
|
||||
initial.persist(&Candidate(40)).unwrap();
|
||||
let baseline = initial.into_flash();
|
||||
|
||||
let mut successful = Journal::new(baseline.clone());
|
||||
assert_eq!(
|
||||
successful
|
||||
.load_matching(&FACTORY, |candidate| candidate.0 <= 35)
|
||||
.unwrap(),
|
||||
Some(Candidate(30))
|
||||
);
|
||||
let start = successful.flash.operations;
|
||||
successful.persist(&Candidate(35)).unwrap();
|
||||
let operation_count = successful.flash.operations - start;
|
||||
|
||||
for interrupted_at in 0..operation_count {
|
||||
let mut journal = Journal::new(baseline.clone());
|
||||
assert_eq!(
|
||||
journal
|
||||
.load_matching(&FACTORY, |candidate| candidate.0 <= 35)
|
||||
.unwrap(),
|
||||
Some(Candidate(30))
|
||||
);
|
||||
journal.flash.fail_at = Some(journal.flash.operations + interrupted_at);
|
||||
assert!(matches!(
|
||||
journal.persist(&Candidate(35)),
|
||||
Err(JournalError::Storage(FakeError::PowerLoss))
|
||||
));
|
||||
|
||||
let mut recovered = Journal::new(journal.into_flash());
|
||||
assert_eq!(
|
||||
recovered
|
||||
.load_matching(&FACTORY, |candidate| candidate.0 <= 35)
|
||||
.unwrap(),
|
||||
Some(Candidate(30)),
|
||||
"mutation {interrupted_at} erased the selected compatible record"
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,11 +1,11 @@
|
||||
//! Runtime-configuration primitives shared by vehicle-specific schemas.
|
||||
//!
|
||||
//! This crate has no knowledge of MAVLink, filesystems, board routing, motors,
|
||||
//! or a particular airframe. It owns the committed-versus-active lifecycle and
|
||||
//! parameter metadata; vehicle crates provide the typed schema and complete
|
||||
//! validation. It also owns the canonical public KCFG artifact and the compact
|
||||
//! private scalar snapshot. Transport and storage framing remain separate
|
||||
//! adapters.
|
||||
//! Provides configuration lifecycle, field metadata, KCFG artifacts and storage
|
||||
//! snapshots. Vehicle crates supply the schema and validation.
|
||||
//!
|
||||
//! A persistence error makes the manager's committed view unknown and blocks
|
||||
//! ordinary saves until reboot recovery or explicit factory reset. The active
|
||||
//! and factory views remain available; an errored save may become active at boot.
|
||||
|
||||
#![no_std]
|
||||
|
||||
@@ -22,9 +22,9 @@ pub mod storage;
|
||||
pub mod validation;
|
||||
|
||||
pub use artifact::{
|
||||
KcfgArtifact, KcfgDecodeError, KcfgEncodeError, KcfgEnumOption, KcfgEnumOptions,
|
||||
KcfgExportPlan, KcfgExporter, KcfgField, KcfgFields, KcfgImporter, KcfgResultDetail,
|
||||
decode_kcfg, encode_edited_kcfg, kcfg_length,
|
||||
KCFG_FIELD_ID_SCRATCH_BYTES, KcfgArtifact, KcfgDecodeError, KcfgEncodeError, KcfgEnumOption,
|
||||
KcfgEnumOptions, KcfgExportPlan, KcfgExporter, KcfgField, KcfgFields, KcfgImporter,
|
||||
KcfgResultDetail, KcfgUpdate, decode_kcfg, encode_edited_kcfg, kcfg_length,
|
||||
};
|
||||
pub use error::{ConfigViolation, ValidationRule};
|
||||
pub use field::{
|
||||
@@ -33,10 +33,12 @@ pub use field::{
|
||||
};
|
||||
pub use journal::{DualSlotJournal, DualSlotNor, JournalError};
|
||||
pub use kaptan_config_derive::{Config, ConfigEnum};
|
||||
pub use lifecycle::{ConfigManager, ReplaceError, ReplaceOutcome, StartupError};
|
||||
pub use lifecycle::{
|
||||
ConfigManager, RecoveryDisposition, ReplaceError, ReplaceOutcome, StartupError,
|
||||
};
|
||||
pub use snapshot::{
|
||||
ScalarSnapshotDecodeError, ScalarSnapshotEncodeError, SnapshotModelError,
|
||||
decode_scalar_snapshot, encode_scalar_snapshot, scalar_snapshot_length,
|
||||
};
|
||||
pub use storage::ConfigStore;
|
||||
pub use storage::{ConfigStore, StoreRecovery};
|
||||
pub use validation::Config;
|
||||
@@ -2,7 +2,7 @@
|
||||
|
||||
use kaptan_err_reportable::KaptanError;
|
||||
|
||||
use crate::{ConfigModel, ConfigStore, ConfigViolation};
|
||||
use crate::{ConfigModel, ConfigStore, ConfigViolation, StoreRecovery};
|
||||
|
||||
/// Failure while recovering a boot configuration from a store.
|
||||
#[derive(Debug, KaptanError)]
|
||||
@@ -16,6 +16,23 @@ pub enum StartupError<E> {
|
||||
/// The board's compiled factory candidate violates its immutable context.
|
||||
#[kaptan_error(id = 1103)]
|
||||
InvalidFactory(ConfigViolation),
|
||||
/// Occupied storage contains no readable candidate for this schema.
|
||||
#[kaptan_error(id = 1104)]
|
||||
UnreadablePersisted,
|
||||
/// The explicit recovery policy supplied an invalid fallback candidate.
|
||||
#[kaptan_error(id = 1105)]
|
||||
InvalidRecovery(ConfigViolation),
|
||||
}
|
||||
|
||||
/// Immutable diagnostic describing the configuration selected at boot.
|
||||
#[derive(Clone, Copy, Debug, PartialEq)]
|
||||
pub enum RecoveryDisposition {
|
||||
/// The selected boot policy accepted a candidate without a recovery diagnostic.
|
||||
Healthy,
|
||||
/// The readable committed candidate was retained, but could not be activated.
|
||||
InvalidPersisted(ConfigViolation),
|
||||
/// Storage could not establish a readable committed candidate.
|
||||
StorageUnknown,
|
||||
}
|
||||
|
||||
/// Result of replacing the complete committed configuration.
|
||||
@@ -31,9 +48,15 @@ pub enum ReplaceError<E> {
|
||||
/// The complete candidate violates a relational or capability invariant.
|
||||
#[kaptan_error(id = 1402)]
|
||||
InvalidCandidate(ConfigViolation),
|
||||
/// The storage backend could not commit the validated candidate.
|
||||
/// The storage backend failed; the candidate may already be durable.
|
||||
#[kaptan_error(id = 1403)]
|
||||
Store(E),
|
||||
/// Durability is unknown; ordinary saving requires recovery or factory reset.
|
||||
#[kaptan_error(id = 1404)]
|
||||
DurableStateUnknown,
|
||||
/// The editor's expected base differs from the current committed values.
|
||||
#[kaptan_error(id = 1405)]
|
||||
Conflict,
|
||||
}
|
||||
|
||||
/// Owns the immutable boot-active configuration and latest durable candidate.
|
||||
@@ -41,12 +64,15 @@ pub enum ReplaceError<E> {
|
||||
/// There is deliberately no in-place activation operation: [`Self::active`]
|
||||
/// never changes after boot. A file endpoint owns its in-progress candidate;
|
||||
/// this manager receives only complete candidates for validation and commit.
|
||||
/// Any persistence error invalidates the committed view and blocks further
|
||||
/// ordinary replacements until reboot recovery or an explicit factory reset.
|
||||
pub struct ConfigManager<C: ConfigModel, S> {
|
||||
active: C::Active,
|
||||
factory: C,
|
||||
committed: C,
|
||||
committed: Option<C>,
|
||||
context: C::Context,
|
||||
store: S,
|
||||
diagnostic: RecoveryDisposition,
|
||||
}
|
||||
|
||||
impl<C, S> ConfigManager<C, S>
|
||||
@@ -55,58 +81,69 @@ where
|
||||
S: ConfigStore<C>,
|
||||
{
|
||||
/// Recovers the newest valid committed snapshot or uses the supplied
|
||||
/// factory candidate when the store is empty.
|
||||
/// factory candidate when the store is empty. Invalid or unreadable
|
||||
/// persisted state fails startup rather than selecting factory values.
|
||||
pub fn recover(
|
||||
factory: C,
|
||||
context: C::Context,
|
||||
mut store: S,
|
||||
store: S,
|
||||
) -> Result<Self, StartupError<S::Error>> {
|
||||
let factory_active = factory
|
||||
.validate(&context)
|
||||
.map_err(StartupError::InvalidFactory)?;
|
||||
let (active, committed) = match store.load(&factory).map_err(StartupError::Store)? {
|
||||
Some(committed) => {
|
||||
let active = committed
|
||||
.validate(&context)
|
||||
.map_err(StartupError::InvalidPersisted)?;
|
||||
(active, committed)
|
||||
}
|
||||
None => (factory_active, factory.clone()),
|
||||
};
|
||||
Ok(Self {
|
||||
active,
|
||||
factory,
|
||||
committed,
|
||||
context,
|
||||
store,
|
||||
})
|
||||
Self::recover_with(factory, context, store, |_, _, _| None)
|
||||
}
|
||||
|
||||
/// Recovers the newest candidate compatible with the current context,
|
||||
/// falling back to the supplied factory when no compatible record exists.
|
||||
///
|
||||
/// This is an explicit recovery policy for output-inhibited setup images.
|
||||
/// Safety-critical compositions should use [`Self::recover`] if an
|
||||
/// incompatible durable record must prevent initialization.
|
||||
pub fn recover_compatible_or_factory(
|
||||
/// Explicit setup recovery policy. On invalid or unreadable storage the
|
||||
/// callback may supply a fallback candidate, which this manager validates
|
||||
/// before activation. Returning `None` preserves strict startup failure.
|
||||
/// Readable invalid committed values remain available for repair. Neither
|
||||
/// fallback activation nor inspection writes anything to storage.
|
||||
/// The fallback supplies only the active view. It never replaces the
|
||||
/// committed candidate or turns unknown durable state into known state.
|
||||
pub fn recover_with(
|
||||
factory: C,
|
||||
context: C::Context,
|
||||
mut store: S,
|
||||
recovery: impl FnOnce(&C, &C::Context, RecoveryDisposition) -> Option<C>,
|
||||
) -> Result<Self, StartupError<S::Error>> {
|
||||
let factory_active = factory
|
||||
.validate(&context)
|
||||
.map_err(StartupError::InvalidFactory)?;
|
||||
let committed = store
|
||||
.load_matching(&factory, |candidate| candidate.validate(&context).is_ok())
|
||||
.map_err(StartupError::Store)?;
|
||||
let (active, committed) = match committed {
|
||||
Some(committed) => {
|
||||
let active = committed
|
||||
let (committed, active, diagnostic) = match store.load_recovery(&factory) {
|
||||
Ok(StoreRecovery::Committed(committed)) => {
|
||||
let active = committed.validate(&context);
|
||||
let diagnostic = match active {
|
||||
Ok(_) => RecoveryDisposition::Healthy,
|
||||
Err(issue) => RecoveryDisposition::InvalidPersisted(issue),
|
||||
};
|
||||
(
|
||||
Some(committed),
|
||||
active.map_err(StartupError::InvalidPersisted),
|
||||
diagnostic,
|
||||
)
|
||||
}
|
||||
Ok(StoreRecovery::Empty) => (
|
||||
Some(factory.clone()),
|
||||
Ok(factory_active),
|
||||
RecoveryDisposition::Healthy,
|
||||
),
|
||||
Ok(StoreRecovery::Unreadable) => (
|
||||
None,
|
||||
Err(StartupError::UnreadablePersisted),
|
||||
RecoveryDisposition::StorageUnknown,
|
||||
),
|
||||
Err(error) => (
|
||||
None,
|
||||
Err(StartupError::Store(error)),
|
||||
RecoveryDisposition::StorageUnknown,
|
||||
),
|
||||
};
|
||||
let active = match active {
|
||||
Ok(active) => active,
|
||||
Err(error) => {
|
||||
let fallback = recovery(&factory, &context, diagnostic).ok_or(error)?;
|
||||
fallback
|
||||
.validate(&context)
|
||||
.map_err(StartupError::InvalidPersisted)?;
|
||||
(active, committed)
|
||||
.map_err(StartupError::InvalidRecovery)?
|
||||
}
|
||||
None => (factory_active, factory.clone()),
|
||||
};
|
||||
Ok(Self {
|
||||
active,
|
||||
@@ -114,6 +151,7 @@ where
|
||||
committed,
|
||||
context,
|
||||
store,
|
||||
diagnostic,
|
||||
})
|
||||
}
|
||||
|
||||
@@ -122,9 +160,12 @@ where
|
||||
&self.active
|
||||
}
|
||||
|
||||
/// Returns the complete last durably committed candidate.
|
||||
pub const fn committed(&self) -> &C {
|
||||
&self.committed
|
||||
/// Returns the readable committed candidate (possibly invalid), or the
|
||||
/// empty-store factory selected at boot. Returns `None` when boot could not
|
||||
/// establish durable state or after any persistence error, since the
|
||||
/// attempted replacement may already be durable.
|
||||
pub const fn committed(&self) -> Option<&C> {
|
||||
self.committed.as_ref()
|
||||
}
|
||||
|
||||
/// Returns the complete board/image factory candidate.
|
||||
@@ -132,28 +173,76 @@ where
|
||||
&self.factory
|
||||
}
|
||||
|
||||
/// Whether committed values differ from the immutable boot-active view.
|
||||
/// Returns the boot recovery disposition; saving does not change boot state.
|
||||
pub const fn diagnostic(&self) -> RecoveryDisposition {
|
||||
self.diagnostic
|
||||
}
|
||||
|
||||
/// Whether every candidate scalar exactly matches the factory, including
|
||||
/// native floating-point bits (so positive and negative zero differ).
|
||||
pub fn is_factory(&self, candidate: &C) -> bool {
|
||||
same_values(candidate, &self.factory)
|
||||
}
|
||||
|
||||
/// Whether committed values differ from the immutable boot-active view,
|
||||
/// or reboot recovery is required because the durable state is unknown.
|
||||
pub fn restart_required(&self) -> bool {
|
||||
!active_matches_candidate::<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.
|
||||
///
|
||||
/// An unchanged candidate does not consume a flash erase/write cycle. The
|
||||
/// boot-active view remains immutable until a physical restart.
|
||||
/// After any persistence error, all replacements (including unchanged
|
||||
/// candidates) fail with [`ReplaceError::DurableStateUnknown`] until reset
|
||||
/// or reboot recovery.
|
||||
pub fn replace(&mut self, candidate: C) -> Result<ReplaceOutcome, ReplaceError<S::Error>> {
|
||||
let committed = self
|
||||
.committed
|
||||
.as_ref()
|
||||
.ok_or(ReplaceError::DurableStateUnknown)?;
|
||||
candidate
|
||||
.validate(&self.context)
|
||||
.map_err(ReplaceError::InvalidCandidate)?;
|
||||
if same_values(&candidate, &self.committed) {
|
||||
if same_values(&candidate, committed) {
|
||||
return Ok(ReplaceOutcome {
|
||||
restart_required: self.restart_required(),
|
||||
});
|
||||
}
|
||||
self.store
|
||||
.persist(&candidate)
|
||||
.map_err(ReplaceError::Store)?;
|
||||
self.committed = candidate;
|
||||
self.persist(candidate)
|
||||
}
|
||||
|
||||
/// Guarded full replacement. The expected base is compared bit-for-bit
|
||||
/// before validation, no-op detection, or any persistence attempt.
|
||||
pub fn replace_if_current(
|
||||
&mut self,
|
||||
candidate: C,
|
||||
expected_base: &C,
|
||||
) -> Result<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();
|
||||
if restart_required {
|
||||
kaptan_log::info!("configuration saved; restart required");
|
||||
@@ -175,7 +264,7 @@ fn same_values<C: ConfigModel>(left: &C, right: &C) -> bool {
|
||||
) else {
|
||||
return false;
|
||||
};
|
||||
if left.kind() != right.kind() || left.raw_bits() != right.raw_bits() {
|
||||
if left != right {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@@ -193,7 +282,7 @@ fn active_matches_candidate<C: ConfigModel>(active: &C::Active, candidate: &C) -
|
||||
) else {
|
||||
return false;
|
||||
};
|
||||
if active.kind() != candidate.kind() || active.raw_bits() != candidate.raw_bits() {
|
||||
if active != candidate {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@@ -220,6 +309,8 @@ mod tests {
|
||||
maximum: Some(ConfigValue::U8(10)),
|
||||
enum_options: &[],
|
||||
unit: "count",
|
||||
finite: false,
|
||||
nonzero: false,
|
||||
},
|
||||
FieldDescriptor {
|
||||
id: OTHER,
|
||||
@@ -229,6 +320,8 @@ mod tests {
|
||||
maximum: Some(ConfigValue::U8(10)),
|
||||
enum_options: &[],
|
||||
unit: "count",
|
||||
finite: false,
|
||||
nonzero: false,
|
||||
},
|
||||
];
|
||||
|
||||
@@ -319,9 +412,24 @@ mod tests {
|
||||
}
|
||||
}
|
||||
|
||||
struct IncompatibleStore;
|
||||
struct FailingStore<'a>(&'a Cell<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 = ();
|
||||
|
||||
fn load(&mut self, _: &Candidate) -> Result<Option<Candidate>, Self::Error> {
|
||||
@@ -329,6 +437,7 @@ mod tests {
|
||||
}
|
||||
|
||||
fn persist(&mut self, _: &Candidate) -> Result<(), Self::Error> {
|
||||
self.0.set(self.0.get() + 1);
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
@@ -362,7 +471,7 @@ mod tests {
|
||||
ValidationRule::Inconsistent,
|
||||
)))
|
||||
);
|
||||
assert_eq!(manager.committed(), &factory);
|
||||
assert_eq!(manager.committed(), Some(&factory));
|
||||
assert_eq!(writes.get(), 0);
|
||||
|
||||
assert_eq!(
|
||||
@@ -396,22 +505,147 @@ mod tests {
|
||||
restart_required: false,
|
||||
})
|
||||
);
|
||||
assert_eq!(manager.committed(), &factory);
|
||||
assert_eq!(manager.committed(), Some(&factory));
|
||||
assert_eq!(manager.factory(), &factory);
|
||||
assert_eq!(writes.get(), 2);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn explicit_recovery_policy_uses_factory_when_no_record_matches_context() {
|
||||
fn any_persistence_error_blocks_all_replacements_until_recovery() {
|
||||
use kaptan_err_reportable::ReportableError;
|
||||
|
||||
let attempts = Cell::new(0);
|
||||
let factory = Candidate { value: 2, other: 4 };
|
||||
let mut manager =
|
||||
ConfigManager::recover(factory.clone(), (), FailingStore(&attempts)).unwrap();
|
||||
assert_eq!(manager.committed(), Some(&factory));
|
||||
assert!(!manager.restart_required());
|
||||
|
||||
assert_eq!(
|
||||
manager.replace(Candidate { value: 3, other: 4 }),
|
||||
Err(ReplaceError::Store(()))
|
||||
);
|
||||
assert_eq!(manager.committed(), None);
|
||||
assert_eq!(manager.active(), &Active { value: 2, other: 4 });
|
||||
assert_eq!(manager.factory(), &factory);
|
||||
assert!(manager.restart_required());
|
||||
|
||||
for candidate in [
|
||||
factory,
|
||||
Candidate { value: 3, other: 4 },
|
||||
Candidate { value: 4, other: 4 },
|
||||
Candidate { value: 5, other: 4 },
|
||||
] {
|
||||
assert_eq!(
|
||||
manager.replace(candidate),
|
||||
Err(ReplaceError::DurableStateUnknown)
|
||||
);
|
||||
assert_eq!(manager.committed(), None);
|
||||
}
|
||||
assert_eq!(attempts.get(), 1);
|
||||
assert_eq!(
|
||||
ReplaceError::<()>::DurableStateUnknown.error_id().get(),
|
||||
1404
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn explicit_fallback_retains_invalid_committed_values_and_validates_recovery() {
|
||||
let factory = Candidate { value: 2, other: 4 };
|
||||
let invalid = Candidate { value: 9, other: 1 };
|
||||
let writes = Cell::new(0);
|
||||
assert!(matches!(
|
||||
ConfigManager::recover(factory.clone(), (), IncompatibleStore),
|
||||
ConfigManager::recover(factory.clone(), (), IncompatibleStore(&writes)),
|
||||
Err(StartupError::InvalidPersisted(_))
|
||||
));
|
||||
|
||||
let manager =
|
||||
ConfigManager::recover_compatible_or_factory(factory, (), IncompatibleStore).unwrap();
|
||||
let mut manager = ConfigManager::recover_with(
|
||||
factory.clone(),
|
||||
(),
|
||||
IncompatibleStore(&writes),
|
||||
|factory, _, disposition| {
|
||||
assert!(matches!(
|
||||
disposition,
|
||||
RecoveryDisposition::InvalidPersisted(_)
|
||||
));
|
||||
Some(factory.clone())
|
||||
},
|
||||
)
|
||||
.unwrap();
|
||||
assert_eq!(manager.committed(), Some(&invalid));
|
||||
assert_eq!(manager.active(), &Active { value: 2, other: 4 });
|
||||
assert_eq!(writes.get(), 0);
|
||||
assert_eq!(
|
||||
manager.replace_if_current(factory.clone(), &factory),
|
||||
Err(ReplaceError::Conflict)
|
||||
);
|
||||
assert_eq!(writes.get(), 0);
|
||||
manager
|
||||
.replace_if_current(factory.clone(), &invalid)
|
||||
.unwrap();
|
||||
assert_eq!(writes.get(), 1, "factory repair must persist, not no-op");
|
||||
assert_eq!(manager.committed(), Some(&factory));
|
||||
assert!(matches!(
|
||||
manager.diagnostic(),
|
||||
RecoveryDisposition::InvalidPersisted(_)
|
||||
));
|
||||
assert!(matches!(
|
||||
ConfigManager::recover_with(factory, (), IncompatibleStore(&writes), |_, _, _| {
|
||||
Some(invalid)
|
||||
}),
|
||||
Err(StartupError::InvalidRecovery(_))
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn factory_reset_is_explicit_forced_and_preserves_active_and_uncertainty_on_error() {
|
||||
struct Store<'a> {
|
||||
attempts: &'a Cell<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]
|
||||
@@ -422,7 +656,9 @@ mod tests {
|
||||
Err(StartupError::InvalidFactory(_))
|
||||
));
|
||||
assert!(matches!(
|
||||
ConfigManager::recover_compatible_or_factory(invalid_factory, (), ValidStore),
|
||||
ConfigManager::recover_with(invalid_factory, (), ValidStore, |_, _, _| {
|
||||
panic!("invalid factory must fail before the recovery callback")
|
||||
}),
|
||||
Err(StartupError::InvalidFactory(_))
|
||||
));
|
||||
}
|
||||
|
||||
@@ -12,9 +12,7 @@
|
||||
//! field_id:u16 | value_kind:u8 | value:[value_kind.width]
|
||||
//! ```
|
||||
//!
|
||||
//! Field entries are emitted and decoded in descriptor order. The schema
|
||||
//! fingerprint already fixes that order, so accepting permutations would add
|
||||
//! work without adding a compatible persistence state.
|
||||
//! Field entries follow descriptor order, as fixed by the schema fingerprint.
|
||||
|
||||
use kaptan_err_reportable::KaptanError;
|
||||
|
||||
@@ -38,12 +36,15 @@ pub(crate) const SCALAR_SNAPSHOT_ENTRY_PREFIX_LENGTH: usize = 3;
|
||||
|
||||
/// Returns the exact encoded length for a schema's field and value widths.
|
||||
///
|
||||
/// The `u16` field-count domain makes this arithmetic safe on all targets
|
||||
/// supported by this crate, including 32-bit microcontrollers.
|
||||
/// `value_bytes` must sum the native widths of all fields, each 1 through 8
|
||||
/// bytes. Panics if that total is outside the range allowed by `field_count`.
|
||||
pub const fn scalar_snapshot_length(field_count: u16, value_bytes: usize) -> usize {
|
||||
SCALAR_SNAPSHOT_HEADER_LENGTH
|
||||
+ field_count as usize * SCALAR_SNAPSHOT_ENTRY_PREFIX_LENGTH
|
||||
+ value_bytes
|
||||
let count = field_count as usize;
|
||||
assert!(
|
||||
value_bytes >= count && value_bytes <= count * ValueKind::F64.width(),
|
||||
"snapshot value bytes do not match the field-count domain"
|
||||
);
|
||||
SCALAR_SNAPSHOT_HEADER_LENGTH + count * SCALAR_SNAPSHOT_ENTRY_PREFIX_LENGTH + value_bytes
|
||||
}
|
||||
|
||||
/// A broken [`ConfigModel`] registry detected before encoding or decoding.
|
||||
@@ -225,11 +226,8 @@ pub fn encode_scalar_snapshot<'a, C: ConfigModel>(
|
||||
|
||||
/// Decodes a complete scalar snapshot into a clone of `factory`.
|
||||
///
|
||||
/// Structural validation completes before any field setter runs. Each write
|
||||
/// then passes through [`ConfigModel::set_field`], preserving the model's local
|
||||
/// validation. Whole-candidate capability validation remains the composition
|
||||
/// owner's responsibility because its context is deliberately not part of a
|
||||
/// storage codec.
|
||||
/// Checks structure before applying writes through [`ConfigModel::set_field`].
|
||||
/// The caller must validate whole-candidate relations and capabilities.
|
||||
pub fn decode_scalar_snapshot<C: ConfigModel>(
|
||||
input: &[u8],
|
||||
factory: &C,
|
||||
@@ -378,6 +376,8 @@ fn read_u16(bytes: &[u8]) -> u16 {
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
extern crate std;
|
||||
|
||||
use super::*;
|
||||
use crate::{ConfigViolation, FieldDescriptor, ValidationRule};
|
||||
|
||||
@@ -403,6 +403,15 @@ mod tests {
|
||||
descriptor(U16, ValueKind::U16),
|
||||
];
|
||||
|
||||
#[test]
|
||||
fn snapshot_length_bounds_raw_aggregate_widths() {
|
||||
assert_eq!(scalar_snapshot_length(0, 0), 23);
|
||||
assert_eq!(scalar_snapshot_length(u16::MAX, 524_280), 720_908);
|
||||
for (count, bytes) in [(0, 1), (1, 0), (1, 9), (u16::MAX, usize::MAX)] {
|
||||
assert!(std::panic::catch_unwind(|| scalar_snapshot_length(count, bytes)).is_err());
|
||||
}
|
||||
}
|
||||
|
||||
const fn descriptor(id: FieldId, kind: ValueKind) -> FieldDescriptor {
|
||||
FieldDescriptor {
|
||||
id,
|
||||
@@ -412,6 +421,8 @@ mod tests {
|
||||
maximum: None,
|
||||
enum_options: &[],
|
||||
unit: "",
|
||||
finite: false,
|
||||
nonzero: false,
|
||||
}
|
||||
}
|
||||
|
||||
@@ -682,8 +693,7 @@ mod tests {
|
||||
})
|
||||
);
|
||||
|
||||
// Every numeric kind now occupies its native width, so unlike the old
|
||||
// widened u64 representation it cannot carry out-of-width high bits.
|
||||
// Native-width encodings cannot carry out-of-width high bits.
|
||||
}
|
||||
|
||||
#[test]
|
||||
|
||||
@@ -1,5 +1,15 @@
|
||||
//! 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.
|
||||
///
|
||||
/// 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.
|
||||
fn load(&mut self, factory: &C) -> Result<Option<C>, Self::Error>;
|
||||
|
||||
/// Loads the newest structurally valid candidate accepted by `compatible`.
|
||||
///
|
||||
/// Single-record stores inherit this implementation. Journals should
|
||||
/// override it so an incompatible newest record can fall back to an older
|
||||
/// compatible slot without weakening structural integrity checks.
|
||||
fn load_matching<F>(&mut self, factory: &C, mut compatible: F) -> Result<Option<C>, Self::Error>
|
||||
where
|
||||
F: FnMut(&C) -> bool,
|
||||
{
|
||||
Ok(self
|
||||
.load(factory)?
|
||||
.filter(|candidate| compatible(candidate)))
|
||||
/// Recovers a candidate while distinguishing empty from undecodable storage.
|
||||
/// Stores whose `load(None)` can mean occupied storage must override this.
|
||||
fn load_recovery(&mut self, factory: &C) -> Result<StoreRecovery<C>, Self::Error> {
|
||||
Ok(match self.load(factory)? {
|
||||
Some(candidate) => StoreRecovery::Committed(candidate),
|
||||
None => StoreRecovery::Empty,
|
||||
})
|
||||
}
|
||||
|
||||
/// Persists one complete already-validated candidate atomically.
|
||||
///
|
||||
/// Success confirms that the candidate is durable. An error does not imply
|
||||
/// rollback: the commit marker may already be durable when a later write
|
||||
/// acknowledgement or verification read fails. Callers must treat durable
|
||||
/// state as unknown after any error until recovery reloads it from storage
|
||||
/// or an explicit factory reset successfully establishes a new record.
|
||||
fn persist(&mut self, candidate: &C) -> Result<(), Self::Error>;
|
||||
}
|
||||
@@ -14,7 +14,8 @@ pub trait Config {
|
||||
/// Exact value bytes in the flattened private snapshot.
|
||||
const SNAPSHOT_VALUE_BYTES: usize;
|
||||
|
||||
/// Fingerprint generated from the complete declared schema.
|
||||
/// Fingerprint of declared scalar metadata and ordered child fingerprints.
|
||||
/// Post-validator code and runtime capability context are not hashed.
|
||||
const SCHEMA_ID: [u8; 16];
|
||||
|
||||
/// Returns a descriptor from the flattened declaration order.
|
||||
@@ -85,19 +86,20 @@ pub fn __valid_enum_options(descriptors: &[FieldDescriptor]) -> bool {
|
||||
if descriptor.enum_options.len() > crate::field::MAX_ENUM_OPTIONS_PER_FIELD {
|
||||
return false;
|
||||
}
|
||||
for (index, option) in descriptor.enum_options.iter().enumerate() {
|
||||
let mut seen = [0_u8; 32];
|
||||
for option in descriptor.enum_options {
|
||||
if option.1.is_empty()
|
||||
|| option.1.len() > crate::field::MAX_ENUM_OPTION_LABEL_BYTES
|
||||
|| descriptor.kind != crate::ValueKind::U8
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if descriptor.enum_options[..index]
|
||||
.iter()
|
||||
.any(|previous| previous.0 == option.0)
|
||||
{
|
||||
let byte = usize::from(option.0 / 8);
|
||||
let bit = 1 << (option.0 % 8);
|
||||
if seen[byte] & bit != 0 {
|
||||
return false;
|
||||
}
|
||||
seen[byte] |= bit;
|
||||
}
|
||||
}
|
||||
true
|
||||
@@ -173,6 +175,8 @@ pub const fn __schema_id(descriptors: &[FieldDescriptor]) -> [u8; 16] {
|
||||
option += 1;
|
||||
}
|
||||
state = hash_str(state, descriptor.unit);
|
||||
state = hash_byte(state, descriptor.finite as u8);
|
||||
state = hash_byte(state, descriptor.nonzero as u8);
|
||||
index += 1;
|
||||
}
|
||||
finish_fingerprint(state)
|
||||
|
||||
@@ -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(¤t, &factory);
|
||||
assert_eq!(bytes[4], 3);
|
||||
let artifact =
|
||||
decode_kcfg(&bytes, &mut [0; kaptan_config::KCFG_FIELD_ID_SCRATCH_BYTES]).unwrap();
|
||||
assert_eq!(artifact.schema_id(), Scalars::SCHEMA_ID);
|
||||
for field in artifact.fields() {
|
||||
assert_eq!(field.finite(), field.id() == FieldId::new(12));
|
||||
assert_eq!(field.nonzero(), field.id() == FieldId::new(13));
|
||||
let expected = current.field_value(field.id()).unwrap();
|
||||
assert_exact(field.value(), expected);
|
||||
assert_exact(field.expected_base_value(), expected);
|
||||
}
|
||||
let update = import(&edit(&bytes, &factory), &factory);
|
||||
for index in 0..Scalars::field_count() {
|
||||
let id = Scalars::descriptor(index).unwrap().id;
|
||||
assert_exact(
|
||||
update.candidate.field_value(id).unwrap(),
|
||||
factory.field_value(id).unwrap(),
|
||||
);
|
||||
assert_exact(
|
||||
update.expected_base.field_value(id).unwrap(),
|
||||
current.field_value(id).unwrap(),
|
||||
);
|
||||
}
|
||||
// 24 header + 4 CRC + 70 prefix + 132 value + 87 key bytes.
|
||||
assert_eq!(bytes.len(), 317);
|
||||
|
||||
let mut importer = KcfgImporter::new(&factory).unwrap();
|
||||
assert_eq!(
|
||||
importer.push(&factory, &export(¤t, ¤t)),
|
||||
Err(KcfgDecodeError::FactoryMismatch(FieldId::new(9)))
|
||||
);
|
||||
|
||||
// Base checks, no-op checks, restart checks, and factory checks must all
|
||||
// distinguish signed zero even when every other scalar matches.
|
||||
for id in [FieldId::new(9), FieldId::new(10)] {
|
||||
let writes = Cell::new(0);
|
||||
let mut manager = ConfigManager::recover(factory.clone(), (), Store(&writes)).unwrap();
|
||||
let mut negative_zero = factory.clone();
|
||||
negative_zero
|
||||
.set_field(id, current.field_value(id).unwrap())
|
||||
.unwrap();
|
||||
assert!(!manager.is_factory(&negative_zero));
|
||||
assert_eq!(
|
||||
manager.replace_if_current(factory.clone(), &negative_zero),
|
||||
Err(ReplaceError::Conflict)
|
||||
);
|
||||
assert!(
|
||||
manager
|
||||
.replace_if_current(negative_zero.clone(), &factory)
|
||||
.unwrap()
|
||||
.restart_required
|
||||
);
|
||||
assert_eq!(writes.get(), 1);
|
||||
assert_eq!(
|
||||
manager.replace_if_current(negative_zero, &factory),
|
||||
Err(ReplaceError::Conflict)
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn nonfinite_payload_identity_survives_artifacts_cas_noops_and_factory_checks() {
|
||||
for (single, double) in [
|
||||
(0x7f80_0000, 0xfff0_0000_0000_0000), // infinities
|
||||
(0x7fc0_0042, 0xfff8_0000_0000_0042), // quiet payloads
|
||||
(0xff80_0042, 0x7ff0_0000_0000_0042), // signaling payloads
|
||||
] {
|
||||
let mut factory = factory();
|
||||
factory.f32_value = f32::from_bits(single);
|
||||
factory.f64_value = f64::from_bits(double);
|
||||
let writes = Cell::new(0);
|
||||
let mut manager = ConfigManager::recover(factory.clone(), (), Store(&writes)).unwrap();
|
||||
let bytes = export(&factory, &factory);
|
||||
let update = import(&bytes, &factory);
|
||||
assert!(manager.is_factory(&update.candidate));
|
||||
assert!(
|
||||
!manager
|
||||
.replace_if_current(update.candidate, &update.expected_base)
|
||||
.unwrap()
|
||||
.restart_required
|
||||
);
|
||||
assert_eq!(writes.get(), 0);
|
||||
|
||||
let mut other = factory.clone();
|
||||
other.f64_value = f64::from_bits(double ^ 1);
|
||||
assert!(!manager.is_factory(&other));
|
||||
assert_eq!(
|
||||
manager.replace_if_current(factory.clone(), &other),
|
||||
Err(ReplaceError::Conflict)
|
||||
);
|
||||
assert!(
|
||||
manager
|
||||
.replace_if_current(other.clone(), &factory)
|
||||
.unwrap()
|
||||
.restart_required
|
||||
);
|
||||
assert_eq!(writes.get(), 1);
|
||||
let update = import(&export(&other, &factory), &factory);
|
||||
assert_eq!(
|
||||
update.candidate.field_value(FieldId::new(10)),
|
||||
Some(ConfigValue::F64(other.f64_value))
|
||||
);
|
||||
}
|
||||
assert_ne!(ConfigValue::U32(0), ConfigValue::F32(0.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn streaming_import_checks_rule_flags_and_rejects_the_unshipped_old_format() {
|
||||
let factory = factory();
|
||||
let bytes = export(&factory, &factory);
|
||||
let mut old = bytes.clone();
|
||||
old[4] = 2;
|
||||
let mut importer = KcfgImporter::new(&factory).unwrap();
|
||||
assert_eq!(
|
||||
importer.push(&factory, &old),
|
||||
Err(KcfgDecodeError::UnsupportedVersion(2))
|
||||
);
|
||||
|
||||
let mut offset = 24;
|
||||
for index in 0..Scalars::field_count() {
|
||||
let field = Scalars::descriptor(index).unwrap();
|
||||
if field.finite || field.nonzero {
|
||||
let mut edited = bytes.clone();
|
||||
edited[offset + 3] ^= if field.finite { 4 } else { 8 };
|
||||
let mut importer = KcfgImporter::new(&factory).unwrap();
|
||||
assert_eq!(
|
||||
importer.push(&factory, &edited),
|
||||
Err(KcfgDecodeError::DescriptorMismatch(field.id))
|
||||
);
|
||||
}
|
||||
offset += 7 + field.kind.width() * 3 + field.display_key.len() + field.unit.len();
|
||||
}
|
||||
}
|
||||
@@ -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]);
|
||||
}
|
||||
File diff suppressed because it is too large.
Load diff
@@ -9,6 +9,7 @@ version.workspace = true
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
critical-section = "1.2"
|
||||
kaptan-err-reportable = { path = "../kaptan-err-reportable" }
|
||||
kaptan-mavlink = { path = "../kaptan-mavlink", default-features = false, features = [
|
||||
"sensor-telemetry",
|
||||
@@ -17,3 +18,6 @@ kaptan-sensor = { path = "../kaptan-sensor" }
|
||||
kaptan-time = { path = "../kaptan-time" }
|
||||
kaptan-tx = { path = "../kaptan-tx" }
|
||||
mavlink-core = { version = "0.18", default-features = false }
|
||||
|
||||
[target.'cfg(not(target_os = "none"))'.dev-dependencies]
|
||||
critical-section = { version = "1.2", features = ["std"] }
|
||||
@@ -8,19 +8,19 @@
|
||||
|
||||
use kaptan_err_reportable::KaptanError;
|
||||
use kaptan_mavlink::{MavlinkFrame, encoder::MavlinkEncoder, generated::kaptan::MavMessage};
|
||||
use kaptan_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot};
|
||||
use kaptan_sensor::{imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot};
|
||||
use kaptan_time::Timestamp;
|
||||
use kaptan_tx::{FrameClass, PendingFrame};
|
||||
use mavlink_core::MessageData;
|
||||
|
||||
mod mailbox;
|
||||
|
||||
pub use mailbox::{LatestImuMailbox, LatestOpticalFlowMailbox, LatestRangeMailbox};
|
||||
pub use mailbox::{
|
||||
Consumer, LatestImuMailbox, LatestMailbox, LatestOpticalFlowMailbox, LatestRangeMailbox,
|
||||
Producer,
|
||||
};
|
||||
|
||||
/// Validated publication schedule consumed by the outbound data channel.
|
||||
///
|
||||
/// This is a derived runtime input, not a persisted configuration root. Board
|
||||
/// packages own the factory values and the platform constructs this view only
|
||||
/// after validating the active user configuration against immutable hardware.
|
||||
/// Publication rates in hertz.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct DataChannelRates {
|
||||
/// IMU publication rate.
|
||||
@@ -33,15 +33,55 @@ pub struct DataChannelRates {
|
||||
pub heartbeat_hz: usize,
|
||||
}
|
||||
|
||||
/// Mailboxes read by one data-channel worker.
|
||||
#[derive(Clone, Copy)]
|
||||
/// Mailboxes read by the single telemetry collector.
|
||||
pub struct DataSources<'a> {
|
||||
/// Latest IMU observation from the acquisition worker.
|
||||
pub imu: &'a LatestImuMailbox,
|
||||
pub imu: Consumer<'a, ImuSnapshot>,
|
||||
/// Latest range observation from either supported vertical route.
|
||||
pub range: &'a LatestRangeMailbox,
|
||||
pub range: Consumer<'a, RangeSnapshot>,
|
||||
/// Latest optical-flow observation from MTF-01.
|
||||
pub optical_flow: &'a LatestOpticalFlowMailbox,
|
||||
pub optical_flow: Consumer<'a, OpticalFlowSnapshot>,
|
||||
}
|
||||
|
||||
/// Latest collected observations shared by any number of outbound links.
|
||||
#[derive(Clone, Copy, Debug, Default, Eq, PartialEq)]
|
||||
pub struct TelemetrySamples {
|
||||
/// Latest IMU observation.
|
||||
pub imu: Option<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.
|
||||
@@ -59,80 +99,70 @@ pub enum DataChannelError {
|
||||
}
|
||||
|
||||
/// Scheduled outbound MAVLink frame producer.
|
||||
pub struct DataChannel<'a, const SERVICE_HZ: usize> {
|
||||
pub struct DataChannel<const SERVICE_HZ: usize> {
|
||||
encoder: MavlinkEncoder,
|
||||
schedule: Schedule<SERVICE_HZ>,
|
||||
sources: DataSources<'a>,
|
||||
maximum_vertical_age_ticks: u8,
|
||||
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,
|
||||
maximum_vertical_age_microseconds: u64,
|
||||
last_imu_timestamp: Option<Timestamp>,
|
||||
}
|
||||
|
||||
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.
|
||||
pub fn new(
|
||||
system_id: u8,
|
||||
component_id: u8,
|
||||
rates: DataChannelRates,
|
||||
maximum_vertical_age_ticks: u8,
|
||||
sources: DataSources<'a>,
|
||||
maximum_vertical_age_microseconds: u64,
|
||||
) -> Result<Self, DataChannelError> {
|
||||
let schedule = Schedule::from_rates(rates)?;
|
||||
Ok(Self {
|
||||
encoder: MavlinkEncoder::new(system_id, component_id),
|
||||
schedule,
|
||||
sources,
|
||||
maximum_vertical_age_ticks,
|
||||
observed_imu: 0,
|
||||
observed_range: 0,
|
||||
last_range: None,
|
||||
range_age_ticks: u8::MAX,
|
||||
observed_optical_flow: 0,
|
||||
last_optical_flow: None,
|
||||
optical_flow_age_ticks: u8::MAX,
|
||||
maximum_vertical_age_microseconds,
|
||||
last_imu_timestamp: None,
|
||||
})
|
||||
}
|
||||
|
||||
/// Runs all outbound work due for one platform service tick.
|
||||
///
|
||||
/// At most one classified frame is returned for transport admission.
|
||||
pub fn on_service_tick(&mut self) -> Option<PendingFrame<MavlinkFrame>> {
|
||||
self.refresh_vertical_data();
|
||||
|
||||
/// IMU timestamps must increase; recreate the channel for a new clock epoch.
|
||||
pub fn on_service_tick(
|
||||
&mut self,
|
||||
samples: &TelemetrySamples,
|
||||
now: Timestamp,
|
||||
) -> Option<PendingFrame<MavlinkFrame>> {
|
||||
match self.schedule.next_due() {
|
||||
Some(DueData::Imu) => {
|
||||
self.sources
|
||||
.imu
|
||||
.take_newer(&mut self.observed_imu)
|
||||
.map(|sample| PendingFrame {
|
||||
let sample = samples.imu.filter(|sample| {
|
||||
self.last_imu_timestamp
|
||||
.is_none_or(|last| sample.timestamp > last)
|
||||
})?;
|
||||
self.last_imu_timestamp = Some(sample.timestamp);
|
||||
Some(PendingFrame {
|
||||
class: FrameClass::BestEffort,
|
||||
frame: self.encoder.imu_snapshot(&sample),
|
||||
})
|
||||
}
|
||||
Some(DueData::Distance) if self.range_age_ticks <= self.maximum_vertical_age_ticks => {
|
||||
self.last_range.map(|range| PendingFrame {
|
||||
Some(DueData::Distance) => samples
|
||||
.range
|
||||
.filter(|range| self.vertical_is_fresh(range.timestamp, now))
|
||||
.map(|range| PendingFrame {
|
||||
class: FrameClass::BestEffort,
|
||||
frame: self.encoder.range_snapshot(&range),
|
||||
})
|
||||
}
|
||||
Some(DueData::OpticalFlow)
|
||||
if self.optical_flow_age_ticks <= self.maximum_vertical_age_ticks =>
|
||||
{
|
||||
self.last_optical_flow.map(|flow| PendingFrame {
|
||||
}),
|
||||
Some(DueData::OpticalFlow) => samples
|
||||
.optical_flow
|
||||
.filter(|flow| self.vertical_is_fresh(flow.timestamp, now))
|
||||
.map(|flow| PendingFrame {
|
||||
class: FrameClass::BestEffort,
|
||||
frame: self.encoder.optical_flow_snapshot(&flow),
|
||||
})
|
||||
}
|
||||
}),
|
||||
Some(DueData::Heartbeat) => Some(PendingFrame {
|
||||
class: FrameClass::Vital,
|
||||
frame: self.encoder.heartbeat(),
|
||||
}),
|
||||
Some(DueData::Distance | DueData::OpticalFlow) | None => None,
|
||||
None => None,
|
||||
}
|
||||
}
|
||||
|
||||
@@ -149,23 +179,10 @@ impl<'a, const SERVICE_HZ: usize> DataChannel<'a, SERVICE_HZ> {
|
||||
}
|
||||
}
|
||||
|
||||
fn refresh_vertical_data(&mut self) {
|
||||
if let Some(range) = self.sources.range.take_newer(&mut self.observed_range) {
|
||||
self.last_range = Some(range);
|
||||
self.range_age_ticks = 0;
|
||||
} else {
|
||||
self.range_age_ticks = self.range_age_ticks.saturating_add(1);
|
||||
}
|
||||
if let Some(flow) = self
|
||||
.sources
|
||||
.optical_flow
|
||||
.take_newer(&mut self.observed_optical_flow)
|
||||
{
|
||||
self.last_optical_flow = Some(flow);
|
||||
self.optical_flow_age_ticks = 0;
|
||||
} else {
|
||||
self.optical_flow_age_ticks = self.optical_flow_age_ticks.saturating_add(1);
|
||||
}
|
||||
fn vertical_is_fresh(&self, timestamp: Timestamp, now: Timestamp) -> bool {
|
||||
now.as_microseconds()
|
||||
.checked_sub(timestamp.as_microseconds())
|
||||
.is_some_and(|age| age <= self.maximum_vertical_age_microseconds)
|
||||
}
|
||||
}
|
||||
|
||||
@@ -245,70 +262,4 @@ impl<const SERVICE_HZ: usize> Schedule<SERVICE_HZ> {
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::{
|
||||
DataChannel, DataChannelRates, DataSources, LatestImuMailbox, LatestOpticalFlowMailbox,
|
||||
LatestRangeMailbox,
|
||||
};
|
||||
use kaptan_mavlink::generated::kaptan::MavMessage;
|
||||
use kaptan_sensor::{
|
||||
imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot,
|
||||
};
|
||||
use kaptan_time::Timestamp;
|
||||
use kaptan_tx::FrameClass;
|
||||
|
||||
const RATES: DataChannelRates = DataChannelRates {
|
||||
imu_hz: 20,
|
||||
range_hz: Some(20),
|
||||
optical_flow_hz: Some(20),
|
||||
heartbeat_hz: 2,
|
||||
};
|
||||
|
||||
#[test]
|
||||
fn owns_rate_selection_encoding_and_classification() {
|
||||
let imu = LatestImuMailbox::new();
|
||||
let range = LatestRangeMailbox::new();
|
||||
let flow = LatestOpticalFlowMailbox::new();
|
||||
let mut channel: DataChannel<'_, 100> = DataChannel::new(
|
||||
1,
|
||||
1,
|
||||
RATES,
|
||||
10,
|
||||
DataSources {
|
||||
imu: &imu,
|
||||
range: &range,
|
||||
optical_flow: &flow,
|
||||
},
|
||||
)
|
||||
.unwrap();
|
||||
let mut best_effort = 0;
|
||||
let mut vital = 0;
|
||||
|
||||
for tick in 0..100 {
|
||||
imu.publish(ImuSnapshot::new(
|
||||
Timestamp::from_microseconds(tick),
|
||||
[0; 3],
|
||||
[0; 3],
|
||||
0,
|
||||
));
|
||||
range.publish(RangeSnapshot::new(Timestamp::from_microseconds(tick), 100));
|
||||
flow.publish(OpticalFlowSnapshot::new(
|
||||
Timestamp::from_microseconds(tick),
|
||||
[0; 2],
|
||||
100,
|
||||
100,
|
||||
));
|
||||
if let Some(frame) = channel.on_service_tick() {
|
||||
assert!(frame.frame.has_valid_crc::<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);
|
||||
}
|
||||
}
|
||||
mod tests;
|
||||
@@ -1,227 +1,220 @@
|
||||
//! Lock-free single-producer, single-consumer latest-value handoff.
|
||||
|
||||
use core::{
|
||||
marker::PhantomData,
|
||||
sync::atomic::{AtomicU32, Ordering},
|
||||
};
|
||||
//! Single-producer, single-consumer latest-value handoff.
|
||||
|
||||
use core::cell::Cell;
|
||||
use critical_section::Mutex;
|
||||
use kaptan_sensor::{imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot};
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
/// Converts an observation to the atomic word representation used by a
|
||||
/// [`LatestMailbox`].
|
||||
/// Fixed-size latest-value mailbox with one producer and one consumer.
|
||||
///
|
||||
/// This is a data-channel handoff contract, not a sensor-domain contract:
|
||||
/// sensor crates own their fields and units, while this crate owns their
|
||||
/// lock-free representation between an acquisition producer and consumer.
|
||||
pub trait MailboxValue<const WORDS: usize>: Copy {
|
||||
/// Converts this observation into its handoff representation.
|
||||
fn into_words(self) -> [u32; WORDS];
|
||||
|
||||
/// Reconstructs an observation from its handoff representation.
|
||||
fn from_words(words: [u32; WORDS]) -> Self;
|
||||
/// A publication replaces any unread value. Each operation acquires the platform
|
||||
/// critical section only to copy or take an `Option<T>`. There are no callbacks,
|
||||
/// allocation, retries, or waits inside that section. The platform must supply a
|
||||
/// critical-section implementation that excludes every participating context.
|
||||
/// On a host this requires the `critical-section` crate's `std` backend.
|
||||
///
|
||||
/// Storage is borrowed by the endpoints and cannot move while they are in use.
|
||||
/// Dropping or forgetting an endpoint does not leave a lock held. After both
|
||||
/// borrows end the mailbox can be split again, retaining any unread value.
|
||||
///
|
||||
/// ```
|
||||
/// use kaptan_data_channel::LatestMailbox;
|
||||
/// let mut mailbox = LatestMailbox::new();
|
||||
/// let (mut producer, mut consumer) = mailbox.split();
|
||||
/// producer.publish([1, 2, 3]);
|
||||
/// producer.publish([4, 5, 6]);
|
||||
/// assert_eq!(consumer.take(), Some([4, 5, 6]));
|
||||
/// assert_eq!(consumer.take(), None);
|
||||
/// ```
|
||||
pub struct LatestMailbox<T: Copy> {
|
||||
slot: Mutex<Cell<Option<T>>>,
|
||||
}
|
||||
|
||||
/// Lock-free latest-value SPSC mailbox.
|
||||
///
|
||||
/// Publishing a newer value intentionally replaces an unread older value: the
|
||||
/// output path is best-effort and must never delay acquisition. The sequence
|
||||
/// counter is a seqlock; odd values indicate a writer and even values identify
|
||||
/// a stable snapshot. Exactly one producer and one consumer may use an
|
||||
/// instance concurrently. Sequence operations are sequentially consistent so
|
||||
/// a reader cannot validate an old even sequence around concurrently written
|
||||
/// atomic payload words; the payload words themselves need only be relaxed.
|
||||
pub struct LatestMailbox<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.
|
||||
impl<T: Copy> LatestMailbox<T> {
|
||||
/// Creates an empty mailbox without acquiring a critical section.
|
||||
pub const fn new() -> Self {
|
||||
Self {
|
||||
sequence: AtomicU32::new(0),
|
||||
words: [const { AtomicU32::new(0) }; WORDS],
|
||||
value: PhantomData,
|
||||
slot: Mutex::new(Cell::new(None)),
|
||||
}
|
||||
}
|
||||
|
||||
/// Publishes a snapshot, replacing any unread older snapshot.
|
||||
pub fn publish(&self, value: Value) {
|
||||
let sequence = self.sequence.fetch_add(1, Ordering::SeqCst);
|
||||
debug_assert_eq!(sequence & 1, 0, "SPSC producer must not reenter");
|
||||
|
||||
for (destination, word) in self.words.iter().zip(value.into_words()) {
|
||||
destination.store(word, Ordering::Relaxed);
|
||||
}
|
||||
|
||||
self.sequence
|
||||
.store(sequence.wrapping_add(2), Ordering::SeqCst);
|
||||
}
|
||||
|
||||
/// Returns the newest stable snapshot different from `observed_sequence`.
|
||||
pub fn take_newer(&self, observed_sequence: &mut u32) -> Option<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))
|
||||
/// Borrows the mailbox as exactly one producer and one consumer.
|
||||
///
|
||||
/// A second split cannot overlap either endpoint's use:
|
||||
///
|
||||
/// ```compile_fail,E0499
|
||||
/// use kaptan_data_channel::LatestMailbox;
|
||||
/// let mut mailbox = LatestMailbox::<u32>::new();
|
||||
/// let (mut producer, _consumer) = mailbox.split();
|
||||
/// let (mut duplicate, _) = mailbox.split();
|
||||
/// producer.publish(1);
|
||||
/// duplicate.publish(2);
|
||||
/// ```
|
||||
pub fn split(&mut self) -> (Producer<'_, T>, Consumer<'_, T>) {
|
||||
(Producer { mailbox: self }, Consumer { mailbox: self })
|
||||
}
|
||||
}
|
||||
|
||||
impl<Value: MailboxValue<WORDS>, const WORDS: usize> Default for LatestMailbox<Value, WORDS> {
|
||||
impl<T: Copy> Default for LatestMailbox<T> {
|
||||
fn default() -> Self {
|
||||
Self::new()
|
||||
}
|
||||
}
|
||||
|
||||
/// Exclusive publication endpoint. Moving it transfers publication ownership.
|
||||
///
|
||||
/// ```compile_fail,E0599
|
||||
/// use kaptan_data_channel::LatestMailbox;
|
||||
/// let mut mailbox = LatestMailbox::<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.
|
||||
pub type LatestImuMailbox = LatestMailbox<ImuSnapshot, 9>;
|
||||
pub type LatestImuMailbox = LatestMailbox<ImuSnapshot>;
|
||||
/// 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.
|
||||
pub type LatestOpticalFlowMailbox = LatestMailbox<OpticalFlowSnapshot, 6>;
|
||||
|
||||
impl MailboxValue<9> for ImuSnapshot {
|
||||
fn into_words(self) -> [u32; 9] {
|
||||
let timestamp = self.timestamp.as_microseconds();
|
||||
[
|
||||
timestamp as u32,
|
||||
(timestamp >> 32) as u32,
|
||||
self.acceleration_milligravity[0] as u16 as u32,
|
||||
self.acceleration_milligravity[1] as u16 as u32,
|
||||
self.acceleration_milligravity[2] as u16 as u32,
|
||||
self.angular_rate_milliradians_per_second[0] as u16 as u32,
|
||||
self.angular_rate_milliradians_per_second[1] as u16 as u32,
|
||||
self.angular_rate_milliradians_per_second[2] as u16 as u32,
|
||||
self.temperature_centidegrees_celsius as u16 as u32,
|
||||
]
|
||||
}
|
||||
|
||||
fn from_words(words: [u32; 9]) -> Self {
|
||||
Self::new(
|
||||
timestamp_from_words(words[0], words[1]),
|
||||
[
|
||||
words[2] as u16 as i16,
|
||||
words[3] as u16 as i16,
|
||||
words[4] as u16 as i16,
|
||||
],
|
||||
[
|
||||
words[5] as u16 as i16,
|
||||
words[6] as u16 as i16,
|
||||
words[7] as u16 as i16,
|
||||
],
|
||||
words[8] as u16 as i16,
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
impl MailboxValue<3> for RangeSnapshot {
|
||||
fn into_words(self) -> [u32; 3] {
|
||||
let timestamp = self.timestamp.as_microseconds();
|
||||
[
|
||||
timestamp as u32,
|
||||
(timestamp >> 32) as u32,
|
||||
self.distance_millimetres,
|
||||
]
|
||||
}
|
||||
|
||||
fn from_words(words: [u32; 3]) -> Self {
|
||||
Self::new(timestamp_from_words(words[0], words[1]), words[2])
|
||||
}
|
||||
}
|
||||
|
||||
impl MailboxValue<6> for OpticalFlowSnapshot {
|
||||
fn into_words(self) -> [u32; 6] {
|
||||
let timestamp = self.timestamp.as_microseconds();
|
||||
[
|
||||
timestamp as u32,
|
||||
(timestamp >> 32) as u32,
|
||||
self.velocity_millimetres_per_second[0] as u32,
|
||||
self.velocity_millimetres_per_second[1] as u32,
|
||||
self.ground_distance_millimetres,
|
||||
u32::from(self.quality),
|
||||
]
|
||||
}
|
||||
|
||||
fn from_words(words: [u32; 6]) -> Self {
|
||||
Self::new(
|
||||
timestamp_from_words(words[0], words[1]),
|
||||
[words[2] as i32, words[3] as i32],
|
||||
words[4],
|
||||
words[5] as u8,
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
const fn timestamp_from_words(low: u32, high: u32) -> Timestamp {
|
||||
Timestamp::from_microseconds((low as u64) | ((high as u64) << 32))
|
||||
}
|
||||
pub type LatestOpticalFlowMailbox = LatestMailbox<OpticalFlowSnapshot>;
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::{LatestImuMailbox, LatestOpticalFlowMailbox, LatestRangeMailbox};
|
||||
use kaptan_sensor::{
|
||||
imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot,
|
||||
};
|
||||
extern crate std;
|
||||
|
||||
use super::*;
|
||||
use kaptan_time::Timestamp;
|
||||
use std::thread;
|
||||
|
||||
#[test]
|
||||
fn publishes_only_the_newest_unread_imu_sample() {
|
||||
let mailbox = LatestImuMailbox::new();
|
||||
let mut observed = 0;
|
||||
mailbox.publish(ImuSnapshot::new(
|
||||
Timestamp::from_microseconds(1),
|
||||
[1; 3],
|
||||
[2; 3],
|
||||
3,
|
||||
));
|
||||
mailbox.publish(ImuSnapshot::new(
|
||||
Timestamp::from_microseconds(2),
|
||||
[4; 3],
|
||||
[5; 3],
|
||||
6,
|
||||
));
|
||||
|
||||
let sample = mailbox.take_newer(&mut observed).unwrap();
|
||||
assert_eq!(sample.timestamp, Timestamp::from_microseconds(2));
|
||||
assert_eq!(sample.acceleration_milligravity, [4; 3]);
|
||||
assert!(mailbox.take_newer(&mut observed).is_none());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn publishes_only_the_newest_unread_range_sample() {
|
||||
let mailbox = LatestRangeMailbox::new();
|
||||
let mut observed = 0;
|
||||
mailbox.publish(RangeSnapshot::new(Timestamp::from_microseconds(1), 100));
|
||||
mailbox.publish(RangeSnapshot::new(Timestamp::from_microseconds(2), 200));
|
||||
|
||||
assert_eq!(
|
||||
mailbox.take_newer(&mut observed),
|
||||
Some(RangeSnapshot::new(Timestamp::from_microseconds(2), 200))
|
||||
fn latest_publication_overwrites_and_is_taken_once() {
|
||||
let mut mailbox = LatestImuMailbox::new();
|
||||
let (mut producer, mut consumer) = mailbox.split();
|
||||
assert_eq!(consumer.take(), None);
|
||||
let first = ImuSnapshot::new(Timestamp::from_microseconds(1), [1; 3], [2; 3], 3);
|
||||
let latest = ImuSnapshot::new(
|
||||
Timestamp::from_microseconds(u64::MAX),
|
||||
[-32_768, 0, 32_767],
|
||||
[-34_906, 0, 34_906],
|
||||
-2_501,
|
||||
);
|
||||
assert!(mailbox.take_newer(&mut observed).is_none());
|
||||
producer.publish(first);
|
||||
producer.publish(latest);
|
||||
assert_eq!(consumer.take(), Some(latest));
|
||||
assert_eq!(consumer.take(), None);
|
||||
producer.publish(latest);
|
||||
assert_eq!(consumer.take(), Some(latest));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn preserves_optical_flow_fields() {
|
||||
let mailbox = LatestOpticalFlowMailbox::new();
|
||||
let mut observed = 0;
|
||||
let sample = OpticalFlowSnapshot::new(Timestamp::from_microseconds(1), [-6, 7], 8, 9);
|
||||
mailbox.publish(sample);
|
||||
fn vertical_snapshots_preserve_all_fields() {
|
||||
let mut range = LatestRangeMailbox::new();
|
||||
let mut flow = LatestOpticalFlowMailbox::new();
|
||||
let (mut range_producer, mut range_consumer) = range.split();
|
||||
let (mut flow_producer, mut flow_consumer) = flow.split();
|
||||
let timestamp = Timestamp::from_microseconds(u64::MAX);
|
||||
let range = RangeSnapshot::new(timestamp, u32::MAX);
|
||||
let flow = OpticalFlowSnapshot::new(timestamp, [i32::MIN, i32::MAX], u32::MAX, 255);
|
||||
range_producer.publish(range);
|
||||
flow_producer.publish(flow);
|
||||
assert_eq!(range_consumer.take(), Some(range));
|
||||
assert_eq!(flow_consumer.take(), Some(flow));
|
||||
}
|
||||
|
||||
assert_eq!(mailbox.take_newer(&mut observed), Some(sample));
|
||||
#[test]
|
||||
// Forget is deliberate: endpoint safety must never depend on destruction.
|
||||
#[allow(clippy::forget_non_drop)]
|
||||
fn moving_dropping_and_forgetting_endpoints_needs_no_cleanup() {
|
||||
let mut mailbox = LatestMailbox::new();
|
||||
{
|
||||
let mut consumer = {
|
||||
let (producer, consumer) = mailbox.split();
|
||||
let mut moved = producer;
|
||||
moved.publish(1);
|
||||
consumer
|
||||
};
|
||||
// The producer left scope before its final publication is taken.
|
||||
assert_eq!(consumer.take(), Some(1));
|
||||
assert_eq!(consumer.take(), None);
|
||||
}
|
||||
{
|
||||
let (mut producer, consumer) = mailbox.split();
|
||||
core::mem::forget(consumer);
|
||||
producer.publish(2);
|
||||
producer.publish(3);
|
||||
core::mem::forget(producer);
|
||||
}
|
||||
// Neither endpoint owns a guard or controls the backing storage lifetime.
|
||||
let mut moved_mailbox = mailbox;
|
||||
let (_, mut consumer) = moved_mailbox.split();
|
||||
assert_eq!(consumer.take(), Some(3));
|
||||
assert_eq!(consumer.take(), None);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn owned_endpoints_publish_coherent_values_across_host_threads() {
|
||||
const LAST: u64 = 50_000;
|
||||
let mut mailbox = LatestMailbox::<[u64; 16]>::new();
|
||||
let (mut producer, mut consumer) = mailbox.split();
|
||||
thread::scope(|scope| {
|
||||
let writer = scope.spawn(move || {
|
||||
for value in 1..=LAST {
|
||||
producer.publish([value; 16]);
|
||||
}
|
||||
});
|
||||
let mut last = 0;
|
||||
let mut observe = |value: [u64; 16]| {
|
||||
assert_eq!(value, [value[0]; 16], "torn publication");
|
||||
assert!(value[0] > last, "duplicate or reordered publication");
|
||||
last = value[0];
|
||||
};
|
||||
while !writer.is_finished() {
|
||||
if let Some(value) = consumer.take() {
|
||||
observe(value);
|
||||
}
|
||||
thread::yield_now();
|
||||
}
|
||||
writer.join().unwrap();
|
||||
if let Some(value) = consumer.take() {
|
||||
observe(value);
|
||||
}
|
||||
assert_eq!(last, LAST);
|
||||
assert_eq!(consumer.take(), None);
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -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)
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -1,8 +1,6 @@
|
||||
//! MAVLink projection for Kaptan logs and reportable firmware errors.
|
||||
//!
|
||||
//! This adapter creates generated payload data only. A platform-owned link is
|
||||
//! solely responsible for MAVLink framing, bounded admission, DMA, and retry
|
||||
//! policy.
|
||||
//! Produces payloads. The caller handles framing and transmission.
|
||||
|
||||
#![no_std]
|
||||
|
||||
@@ -15,7 +13,7 @@ use kaptan_mavlink::generated::kaptan::{ERROR_DATA, LOG_DATA, LogFlags, LogLevel
|
||||
|
||||
const ERROR_RAISED_AT_BYTES: usize = 96;
|
||||
|
||||
/// Encodes one ordinary log record as its sole wire message.
|
||||
/// Encodes one log record.
|
||||
pub fn encode_log(record: &LogRecord) -> LOG_DATA {
|
||||
LOG_DATA {
|
||||
level: wire_level(record.level()),
|
||||
@@ -29,7 +27,7 @@ pub fn encode_log(record: &LogRecord) -> LOG_DATA {
|
||||
}
|
||||
}
|
||||
|
||||
/// Encodes one reportable firmware error as its sole wire message.
|
||||
/// Encodes one reportable firmware error.
|
||||
pub fn encode_error(record: &ErrorRecord) -> ERROR_DATA {
|
||||
let source = record.raised_at().as_bytes();
|
||||
let mut length = source.len().min(ERROR_RAISED_AT_BYTES);
|
||||
|
||||
@@ -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>;
|
||||
}
|
||||
@@ -18,4 +18,16 @@ pub enum Error<BusError> {
|
||||
/// The selected SPI route reported a terminal transfer fault.
|
||||
#[kaptan_error(id = 5104)]
|
||||
TransferFault,
|
||||
/// The bus could not safely accept and copy out the completed transfer.
|
||||
#[kaptan_error(id = 5105)]
|
||||
Completion(kaptan_spi::CompletionError),
|
||||
/// An accepted SPI transfer exceeded the 10 ms service deadline.
|
||||
#[kaptan_error(id = 5106)]
|
||||
TransferTimeout,
|
||||
/// The bus stayed busy for the pending operation's 20 ms admission budget.
|
||||
#[kaptan_error(id = 5107)]
|
||||
AdmissionTimeout,
|
||||
/// No acquisition was requested within two output periods plus 10 ms.
|
||||
#[kaptan_error(id = 5108)]
|
||||
DataReadyTimeout,
|
||||
}
|
||||
@@ -5,7 +5,7 @@ mod sample;
|
||||
pub mod service;
|
||||
|
||||
use kaptan_sensor::imu::ImuSnapshot;
|
||||
use kaptan_spi::{SpiCompletion, SpiDevice};
|
||||
use kaptan_spi::{CompletionError, SpiCompletion, SpiDevice};
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
pub use error::Error;
|
||||
@@ -27,6 +27,19 @@ const INTF_CONFIG1: u8 = 0x4d;
|
||||
const TEMP_DATA1: u8 = 0x1d;
|
||||
const SAMPLE_BYTES: usize = 14;
|
||||
const TRANSFER_CAPACITY: usize = SAMPLE_BYTES + 1;
|
||||
// TDK DS-000347 rev 1.5 §14.1 p.64: soft reset requires 1 ms. §3.3.2 p.14
|
||||
// also requires up to 1 ms before power-on register access. §14.36 p.76
|
||||
// requires 200 us without writes after OFF -> ON and at least 45 ms gyro ON
|
||||
// time. Use 45 ms as a conservative start-up allowance over §3.1/3.2's
|
||||
// TYPICAL 30/10 ms gyro/accel start-up, not a physical settling guarantee.
|
||||
const RESET_SETTLE_US: u64 = 1_000;
|
||||
const SENSOR_SETTLE_US: u64 = 45_000;
|
||||
const SAMPLE_PERIOD_US: u64 = 1_000;
|
||||
// Host service budget, not a datasheet delay. Supported SPI3 bursts take less
|
||||
// than 0.3 ms. Periodic service must continue independently of DMA and DRDY.
|
||||
const TRANSFER_TIMEOUT_US: u64 = 10_000;
|
||||
// Allows a retry on the F722's 10 ms periodic service before admission expires.
|
||||
const ADMISSION_TIMEOUT_US: u64 = 20_000;
|
||||
|
||||
// SPI mode 0, active-high push-pull, pulsed INT1. Data-ready is cleared by
|
||||
// the SPI burst read and its source is explicitly enabled on INT1.
|
||||
@@ -47,19 +60,40 @@ const CONFIGURATION: [(u8, u8); 9] = [
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
enum State {
|
||||
Dormant,
|
||||
Resetting,
|
||||
WaitingForReset,
|
||||
ReadingIdentity,
|
||||
WritingConfiguration(usize),
|
||||
WaitingForSensorStart,
|
||||
Ready,
|
||||
ReadingSample { timestamp: Timestamp },
|
||||
Pending {
|
||||
operation: Operation,
|
||||
since: Timestamp,
|
||||
},
|
||||
Active {
|
||||
operation: Operation,
|
||||
since: Timestamp,
|
||||
},
|
||||
Waiting {
|
||||
next: Operation,
|
||||
since: Timestamp,
|
||||
delay_us: u64,
|
||||
},
|
||||
Settling {
|
||||
since: Timestamp,
|
||||
},
|
||||
Ready {
|
||||
since: Timestamp,
|
||||
},
|
||||
Faulted,
|
||||
}
|
||||
|
||||
/// Observable outcome of one ICM-42688-P SPI terminal event.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
enum Operation {
|
||||
Reset,
|
||||
Identity,
|
||||
Configure(usize),
|
||||
Sample,
|
||||
}
|
||||
|
||||
/// Observable outcome of one timer or ICM-42688-P SPI terminal event.
|
||||
#[derive(Debug, Eq, PartialEq)]
|
||||
pub enum Icm42688Event<BusError> {
|
||||
/// The selected SPI interrupt had no terminal transfer event.
|
||||
/// No sample, initialization completion or terminal fault was produced.
|
||||
NotPending,
|
||||
/// One initialization transfer completed; initialization continues.
|
||||
Initializing,
|
||||
@@ -73,9 +107,8 @@ pub enum Icm42688Event<BusError> {
|
||||
|
||||
/// Configured ICM-42688-P driver.
|
||||
///
|
||||
/// The driver owns fixed transfer buffers and progresses exclusively from
|
||||
/// timer, data-ready and SPI-DMA completion events. It never waits for a
|
||||
/// peripheral status flag.
|
||||
/// The driver owns ordinary movable staging buffers and progresses from timer,
|
||||
/// data-ready and completion events. Hardware never addresses these buffers.
|
||||
pub struct Icm42688<Bus> {
|
||||
bus: Bus,
|
||||
state: State,
|
||||
@@ -96,125 +129,139 @@ impl<Bus> Icm42688<Bus> {
|
||||
}
|
||||
|
||||
impl<Bus: SpiDevice> Icm42688<Bus> {
|
||||
/// Starts the sensor reset. A subsequent periodic tick provides the
|
||||
/// datasheet-required reset-settling interval before identification.
|
||||
pub fn begin_initialization(&mut self) -> Result<(), Error<Bus::StartError>> {
|
||||
/// Accepts initialization once. Busy admission is retried by periodic
|
||||
/// service. All supplied instants must use one non-regressing boot clock.
|
||||
/// Power must be stable by this call. Register access waits another 1 ms.
|
||||
pub fn begin_initialization(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
|
||||
if self.state != State::Dormant {
|
||||
return Err(Error::NotReady);
|
||||
}
|
||||
self.prepare_write(DEVICE_CONFIG, SOFT_RESET);
|
||||
self.submit(2)?;
|
||||
self.state = State::Resetting;
|
||||
self.wait(Operation::Reset, now, RESET_SETTLE_US);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Advances reset/wake delays from the board periodic event.
|
||||
pub fn advance_initialization(&mut self) -> Result<(), Error<Bus::StartError>> {
|
||||
match self.state {
|
||||
State::WaitingForReset => {
|
||||
self.prepare_read(WHO_AM_I, 2);
|
||||
self.submit(2)?;
|
||||
self.state = State::ReadingIdentity;
|
||||
/// Services elapsed settling, admission and acquisition deadlines. Call
|
||||
/// periodically even with an active transfer. No unsolicited reads occur.
|
||||
pub fn on_periodic_tick(&mut self, now: Timestamp) -> Icm42688Event<Bus::StartError> {
|
||||
if let Some(error) = self.check_deadline(now) {
|
||||
return Icm42688Event::Fault(error);
|
||||
}
|
||||
State::WaitingForSensorStart => {
|
||||
self.start_configuration(7)?;
|
||||
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, SAMPLE_PERIOD_US) => {
|
||||
self.state = State::Ready { since: now };
|
||||
return Icm42688Event::Initialized;
|
||||
}
|
||||
_ => {}
|
||||
}
|
||||
Ok(())
|
||||
match self.submit_pending(now) {
|
||||
Ok(()) => Icm42688Event::NotPending,
|
||||
Err(error) => Icm42688Event::Fault(error),
|
||||
}
|
||||
}
|
||||
|
||||
/// Starts one temperature, accelerometer and gyroscope burst read.
|
||||
pub fn request_sample(&mut self, timestamp: Timestamp) -> Result<(), Error<Bus::StartError>> {
|
||||
if self.state != State::Ready {
|
||||
/// Accepts a burst-read request. Busy admission remains pending for periodic
|
||||
/// retry. The eventual accepted submission time becomes the snapshot
|
||||
/// timestamp, not the ISR time or a physical sampling epoch.
|
||||
pub fn request_sample(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
|
||||
if let Some(error) = self.check_deadline(now) {
|
||||
return Err(error);
|
||||
}
|
||||
if !self.is_ready() {
|
||||
return Err(Error::NotReady);
|
||||
}
|
||||
self.prepare_read(TEMP_DATA1, TRANSFER_CAPACITY);
|
||||
self.submit(TRANSFER_CAPACITY)?;
|
||||
self.state = State::ReadingSample { timestamp };
|
||||
Ok(())
|
||||
self.queue(Operation::Sample, now);
|
||||
self.submit_pending(now)
|
||||
}
|
||||
|
||||
/// Advances the state machine from an already-acknowledged SPI completion.
|
||||
pub fn on_transfer_complete(
|
||||
&mut self,
|
||||
completion: SpiCompletion,
|
||||
now: Timestamp,
|
||||
) -> Icm42688Event<Bus::StartError> {
|
||||
self.bus.complete_transfer(completion);
|
||||
match completion {
|
||||
SpiCompletion::NotPending => Icm42688Event::NotPending,
|
||||
SpiCompletion::TransferFault => {
|
||||
self.state = State::Dormant;
|
||||
if completion == SpiCompletion::NotPending {
|
||||
return Icm42688Event::NotPending;
|
||||
}
|
||||
if let Some(error) = self.check_deadline(now) {
|
||||
return Icm42688Event::Fault(error);
|
||||
}
|
||||
let State::Active { operation, since } = self.state else {
|
||||
return Icm42688Event::NotPending;
|
||||
};
|
||||
let length = if operation == Operation::Sample {
|
||||
TRANSFER_CAPACITY
|
||||
} else {
|
||||
2
|
||||
};
|
||||
match self
|
||||
.bus
|
||||
.complete_transfer(completion, &mut self.receive[..length])
|
||||
{
|
||||
Ok(()) => self.advance_after_completion(operation, since, now),
|
||||
Err(CompletionError::NotPending) => Icm42688Event::NotPending,
|
||||
Err(CompletionError::TransferFault) => {
|
||||
self.state = State::Faulted;
|
||||
Icm42688Event::Fault(Error::TransferFault)
|
||||
}
|
||||
SpiCompletion::Complete => self.advance_after_completion(),
|
||||
Err(error) => {
|
||||
self.state = State::Faulted;
|
||||
Icm42688Event::Fault(Error::Completion(error))
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Returns whether an IMU data-ready event can start a sensor transfer.
|
||||
pub fn is_ready(&self) -> bool {
|
||||
self.state == State::Ready
|
||||
matches!(self.state, State::Ready { .. })
|
||||
}
|
||||
|
||||
fn advance_after_completion(&mut self) -> Icm42688Event<Bus::StartError> {
|
||||
match self.state {
|
||||
State::Dormant => Icm42688Event::NotPending,
|
||||
State::Resetting => {
|
||||
self.state = State::WaitingForReset;
|
||||
Icm42688Event::Initializing
|
||||
}
|
||||
State::WaitingForReset | State::WaitingForSensorStart => Icm42688Event::NotPending,
|
||||
State::ReadingIdentity => {
|
||||
fn advance_after_completion(
|
||||
&mut self,
|
||||
operation: Operation,
|
||||
requested: Timestamp,
|
||||
now: Timestamp,
|
||||
) -> Icm42688Event<Bus::StartError> {
|
||||
match operation {
|
||||
Operation::Reset => self.wait(Operation::Identity, now, RESET_SETTLE_US),
|
||||
Operation::Identity => {
|
||||
let identity = self.receive[1];
|
||||
if identity != EXPECTED_WHO_AM_I {
|
||||
self.state = State::Dormant;
|
||||
self.state = State::Faulted;
|
||||
return Icm42688Event::Fault(Error::UnexpectedIdentity(identity));
|
||||
}
|
||||
self.start_configuration_event(0)
|
||||
self.queue(Operation::Configure(0), now);
|
||||
}
|
||||
State::WritingConfiguration(index) => {
|
||||
Operation::Configure(index) => {
|
||||
let next = index + 1;
|
||||
if next == 7 {
|
||||
// PWR_MGMT0 starts both sensor paths. The following
|
||||
// timer tick gives them more than the required 200 µs
|
||||
// before their output-rate configuration is written.
|
||||
self.state = State::WaitingForSensorStart;
|
||||
Icm42688Event::Initializing
|
||||
// Includes the 200 us no-write interval after PWR_MGMT0.
|
||||
self.wait(Operation::Configure(next), now, SENSOR_SETTLE_US);
|
||||
} else if next == CONFIGURATION.len() {
|
||||
self.state = State::Ready;
|
||||
Icm42688Event::Initialized
|
||||
self.state = State::Settling { since: now };
|
||||
} else {
|
||||
self.start_configuration_event(next)
|
||||
self.queue(Operation::Configure(next), now);
|
||||
}
|
||||
}
|
||||
State::Ready => Icm42688Event::NotPending,
|
||||
State::ReadingSample { timestamp } => {
|
||||
Operation::Sample => {
|
||||
let mut bytes = [0; SAMPLE_BYTES];
|
||||
bytes.copy_from_slice(&self.receive[1..]);
|
||||
self.state = State::Ready;
|
||||
Icm42688Event::Sample(sample::decode(&bytes, timestamp))
|
||||
self.state = State::Ready { since: now };
|
||||
return Icm42688Event::Sample(sample::decode(&bytes, requested));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn start_configuration_event(&mut self, index: usize) -> Icm42688Event<Bus::StartError> {
|
||||
match self.start_configuration(index) {
|
||||
match self.submit_pending(now) {
|
||||
Ok(()) => Icm42688Event::Initializing,
|
||||
Err(error) => Icm42688Event::Fault(error),
|
||||
}
|
||||
}
|
||||
|
||||
fn start_configuration(&mut self, index: usize) -> Result<(), Error<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) {
|
||||
self.transmit = [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]);
|
||||
}
|
||||
|
||||
fn submit(&mut self, length: usize) -> Result<(), Error<Bus::StartError>> {
|
||||
let (bus, transmit, receive) = (&mut self.bus, &self.transmit, &mut self.receive);
|
||||
bus.start_transfer(&transmit[..length], &mut receive[..length])
|
||||
.map_err(Error::Start)
|
||||
fn queue(&mut self, operation: Operation, now: Timestamp) {
|
||||
self.state = State::Pending {
|
||||
operation,
|
||||
since: now,
|
||||
};
|
||||
}
|
||||
|
||||
fn wait(&mut self, next: Operation, now: Timestamp, delay_us: u64) {
|
||||
self.state = State::Waiting {
|
||||
next,
|
||||
since: now,
|
||||
delay_us,
|
||||
};
|
||||
}
|
||||
|
||||
fn submit_pending(&mut self, now: Timestamp) -> Result<(), Error<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)]
|
||||
mod tests {
|
||||
use super::{Error, Icm42688, Icm42688Event, TRANSFER_CAPACITY};
|
||||
use kaptan_spi::{SpiCompletion, SpiDevice};
|
||||
use super::*;
|
||||
use crate::mpu6000::test_bus::{FakeBus, FakeError};
|
||||
use kaptan_spi::{CompletionError, SpiCompletion};
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
struct Bus {
|
||||
response: [u8; TRANSFER_CAPACITY],
|
||||
fn t(us: u64) -> Timestamp {
|
||||
Timestamp::from_microseconds(us + 1_000)
|
||||
}
|
||||
|
||||
impl SpiDevice for Bus {
|
||||
type StartError = ();
|
||||
|
||||
fn start_transfer(&mut self, _: &[u8], receive: &mut [u8]) -> Result<(), Self::StartError> {
|
||||
receive.copy_from_slice(&self.response[..receive.len()]);
|
||||
Ok(())
|
||||
fn start(driver: &mut Icm42688<FakeBus>) {
|
||||
driver
|
||||
.begin_initialization(Timestamp::from_microseconds(0))
|
||||
.unwrap();
|
||||
driver.on_periodic_tick(Timestamp::from_microseconds(999));
|
||||
assert_eq!(driver.bus.starts, 0);
|
||||
driver.on_periodic_tick(t(0));
|
||||
}
|
||||
|
||||
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]
|
||||
fn accepts_a_sample_only_after_identity_and_wake_delays() {
|
||||
let mut response = [0; TRANSFER_CAPACITY];
|
||||
response[1] = 0x47;
|
||||
let mut imu = Icm42688::new(Bus { response });
|
||||
imu.begin_initialization().unwrap();
|
||||
fn accepts_a_sample_only_after_elapsed_reset_wake_and_data_settling() {
|
||||
let mut imu = initialized();
|
||||
imu.request_sample(t(48_101)).unwrap();
|
||||
assert_eq!(imu.receive, [0; TRANSFER_CAPACITY]);
|
||||
assert_eq!(
|
||||
imu.on_transfer_complete(SpiCompletion::NotPending, t(48_102)),
|
||||
Icm42688Event::NotPending
|
||||
);
|
||||
assert_eq!(imu.receive, [0; TRANSFER_CAPACITY]);
|
||||
imu.transmit.fill(0xff);
|
||||
imu.bus.response.fill(0xff);
|
||||
let mut imu = core::mem::replace(&mut imu, Icm42688::new(FakeBus::ready()));
|
||||
assert!(matches!(
|
||||
imu.on_transfer_complete(SpiCompletion::Complete),
|
||||
Icm42688Event::Initializing
|
||||
));
|
||||
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(_)
|
||||
imu.on_transfer_complete(SpiCompletion::Complete, t(48_300)),
|
||||
Icm42688Event::Sample(sample) if sample.timestamp == t(48_101)
|
||||
));
|
||||
assert_eq!(imu.receive[1], 0x47);
|
||||
assert_eq!(
|
||||
imu.on_transfer_complete(SpiCompletion::Complete, t(48_301)),
|
||||
Icm42688Event::NotPending
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejects_a_wrong_identity() {
|
||||
let mut imu = Icm42688::new(Bus {
|
||||
response: [0; TRANSFER_CAPACITY],
|
||||
});
|
||||
imu.begin_initialization().unwrap();
|
||||
let _ = imu.on_transfer_complete(SpiCompletion::Complete);
|
||||
imu.advance_initialization().unwrap();
|
||||
let mut imu = Icm42688::new(FakeBus::ready());
|
||||
start(&mut imu);
|
||||
let _ = imu.on_transfer_complete(SpiCompletion::Complete, t(0));
|
||||
imu.on_periodic_tick(t(1_000));
|
||||
assert!(matches!(
|
||||
imu.on_transfer_complete(SpiCompletion::Complete),
|
||||
imu.on_transfer_complete(SpiCompletion::Complete, t(1_100)),
|
||||
Icm42688Event::Fault(Error::UnexpectedIdentity(0))
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn completion_errors_never_republish_a_previous_sample() {
|
||||
for (event, error) in [
|
||||
(
|
||||
SpiCompletion::Complete,
|
||||
Some(CompletionError::InvalidLength),
|
||||
),
|
||||
(SpiCompletion::Complete, Some(CompletionError::Quarantined)),
|
||||
(SpiCompletion::Complete, Some(CompletionError::NotActive)),
|
||||
(
|
||||
SpiCompletion::Complete,
|
||||
Some(CompletionError::TransferFault),
|
||||
),
|
||||
(SpiCompletion::TransferFault, None),
|
||||
] {
|
||||
let mut driver = Icm42688::new(FakeBus::ready());
|
||||
driver.state = State::Ready { since: t(0) };
|
||||
driver.request_sample(t(1)).unwrap();
|
||||
assert!(matches!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(1)),
|
||||
Icm42688Event::Sample(_)
|
||||
));
|
||||
driver.request_sample(t(2)).unwrap();
|
||||
driver.receive.fill(0xa5);
|
||||
driver.bus.fail_completion = error;
|
||||
assert!(matches!(
|
||||
driver.on_transfer_complete(event, t(2)),
|
||||
Icm42688Event::Fault(_)
|
||||
));
|
||||
assert_eq!(driver.receive, [0xa5; TRANSFER_CAPACITY]);
|
||||
assert!(!driver.is_ready());
|
||||
assert!(!matches!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(3)),
|
||||
Icm42688Event::Sample(_)
|
||||
));
|
||||
if error == Some(CompletionError::Quarantined) {
|
||||
assert_eq!(driver.begin_initialization(t(4)), Err(Error::NotReady));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejected_submission_cannot_complete_a_stale_sample() {
|
||||
let mut driver = Icm42688::new(FakeBus::ready());
|
||||
driver.state = State::Ready { since: t(0) };
|
||||
driver.receive.fill(0xa5);
|
||||
driver.bus.fail_start = true;
|
||||
assert_eq!(
|
||||
driver.request_sample(t(1)),
|
||||
Err(Error::Start(FakeError::Start))
|
||||
);
|
||||
assert_eq!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(2)),
|
||||
Icm42688Event::NotPending
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn every_phase_times_out_and_cancels_once_without_rearming() {
|
||||
for operation in [Operation::Reset, Operation::Identity, Operation::Sample]
|
||||
.into_iter()
|
||||
.chain((0..9).map(Operation::Configure))
|
||||
{
|
||||
let mut driver = Icm42688::new(FakeBus::ready());
|
||||
driver.queue(operation, t(100));
|
||||
driver.submit_pending(t(100)).unwrap();
|
||||
assert_eq!(
|
||||
driver.on_periodic_tick(t(10_099)),
|
||||
Icm42688Event::NotPending
|
||||
);
|
||||
assert_eq!(driver.bus.cancellations, 0);
|
||||
assert_eq!(
|
||||
driver.on_periodic_tick(t(10_100)),
|
||||
Icm42688Event::Fault(Error::TransferTimeout)
|
||||
);
|
||||
assert_eq!(driver.bus.cancellations, 1);
|
||||
assert_eq!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(10_101)),
|
||||
Icm42688Event::NotPending
|
||||
);
|
||||
driver.on_periodic_tick(t(1_000_000));
|
||||
assert_eq!(driver.bus.starts, 1);
|
||||
assert_eq!(
|
||||
driver.begin_initialization(t(1_000_000)),
|
||||
Err(Error::NotReady)
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn every_busy_phase_preserves_pending_operation_and_has_an_admission_deadline() {
|
||||
for operation in [Operation::Reset, Operation::Identity, Operation::Sample]
|
||||
.into_iter()
|
||||
.chain((0..9).map(Operation::Configure))
|
||||
{
|
||||
let mut driver = Icm42688::new(FakeBus::ready());
|
||||
driver.bus.busy = true;
|
||||
driver.queue(operation, t(100));
|
||||
driver.submit_pending(t(100)).unwrap();
|
||||
assert_eq!(
|
||||
driver.state,
|
||||
State::Pending {
|
||||
operation,
|
||||
since: t(100)
|
||||
}
|
||||
);
|
||||
driver.on_periodic_tick(t(200));
|
||||
driver.bus.busy = false;
|
||||
driver.on_periodic_tick(t(300));
|
||||
assert_eq!(
|
||||
driver.state,
|
||||
State::Active {
|
||||
operation,
|
||||
since: t(300)
|
||||
}
|
||||
);
|
||||
assert_eq!(driver.bus.starts, 1);
|
||||
}
|
||||
let mut driver = Icm42688::new(FakeBus::ready());
|
||||
driver.bus.busy = true;
|
||||
start(&mut driver);
|
||||
assert_eq!(
|
||||
driver.on_periodic_tick(t(20_000)),
|
||||
Icm42688Event::Fault(Error::AdmissionTimeout)
|
||||
);
|
||||
assert_eq!(driver.bus.cancellations, 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn missed_drdy_late_completion_and_failed_cancellation_never_publish() {
|
||||
let mut driver = initialized();
|
||||
driver.on_periodic_tick(t(60_099));
|
||||
assert_eq!(driver.bus.starts, 11);
|
||||
assert_eq!(
|
||||
driver.on_periodic_tick(t(60_100)),
|
||||
Icm42688Event::Fault(Error::DataReadyTimeout)
|
||||
);
|
||||
let mut driver = initialized();
|
||||
driver.request_sample(t(49_000)).unwrap();
|
||||
assert_eq!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(59_000)),
|
||||
Icm42688Event::Fault(Error::TransferTimeout)
|
||||
);
|
||||
let mut driver = initialized();
|
||||
driver.request_sample(t(49_000)).unwrap();
|
||||
driver.bus.fail_completion = Some(CompletionError::Quarantined);
|
||||
assert_eq!(
|
||||
driver.on_periodic_tick(t(59_000)),
|
||||
Icm42688Event::Fault(Error::Completion(CompletionError::Quarantined))
|
||||
);
|
||||
assert_eq!(driver.bus.start_transfer(&[1]), Err(FakeError::Quarantined));
|
||||
}
|
||||
}
|
||||
@@ -1,4 +1,9 @@
|
||||
//! ICM-42688-P burst-sample decoding.
|
||||
//!
|
||||
//! Integer division rounds toward zero before adding temperature's offset.
|
||||
//! Over the full signed raw domain, the configured ranges decode to
|
||||
//! -4,000..=3,999 mg, -8,731..=8,731 mrad/s and -22,234..=27,233 cdegC.
|
||||
//! Products fit i32 and the acceleration and temperature results fit i16.
|
||||
|
||||
use kaptan_sensor::imu::ImuSnapshot;
|
||||
use kaptan_time::Timestamp;
|
||||
@@ -32,18 +37,18 @@ const fn scale_acceleration(raw: i16) -> i16 {
|
||||
}
|
||||
|
||||
// The configured ±500°/s range has 65.5 counts/(°/s).
|
||||
const fn scale_angular_rate(raw: i16) -> i16 {
|
||||
((raw as i32 * 17_453) / 65_500) as i16
|
||||
const fn scale_angular_rate(raw: i16) -> i32 {
|
||||
(raw as i32 * 17_453) / 65_500
|
||||
}
|
||||
|
||||
// ICM-42688-P temperature is 132.48 counts/°C with a 25°C offset.
|
||||
const fn scale_temperature(raw: i16) -> i16 {
|
||||
((raw as i32 * 100) / 13_248 + 2_500) as i16
|
||||
((raw as i32 * 10_000) / 13_248 + 2_500) as i16
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::decode;
|
||||
use super::{decode, scale_temperature};
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
#[test]
|
||||
@@ -54,4 +59,61 @@ mod tests {
|
||||
assert_eq!(sample.angular_rate_milliradians_per_second[0], 8_725);
|
||||
assert_eq!(sample.temperature_centidegrees_celsius, 2_500);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn signed_raw_endpoints_fit_the_configured_ranges() {
|
||||
for (temperature_raw, temperature) in [(i16::MIN, -22_234), (i16::MAX, 27_233)] {
|
||||
let mut bytes = [0; 14];
|
||||
for (index, raw) in [
|
||||
temperature_raw,
|
||||
i16::MIN,
|
||||
0,
|
||||
i16::MAX,
|
||||
i16::MIN,
|
||||
0,
|
||||
i16::MAX,
|
||||
]
|
||||
.into_iter()
|
||||
.enumerate()
|
||||
{
|
||||
bytes[index * 2..index * 2 + 2].copy_from_slice(&raw.to_be_bytes());
|
||||
}
|
||||
let timestamp = Timestamp::from_microseconds(123);
|
||||
let sample = decode(&bytes, timestamp);
|
||||
assert_eq!(sample.timestamp, timestamp);
|
||||
assert_eq!(sample.acceleration_milligravity, [-4_000, 0, 3_999]);
|
||||
assert_eq!(
|
||||
sample.angular_rate_milliradians_per_second,
|
||||
[-8_731, 0, 8_731]
|
||||
);
|
||||
assert_eq!(sample.temperature_centidegrees_celsius, temperature);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn temperature_matches_datasheet_scale_and_rounding() {
|
||||
// ±13,248 counts is ±100°C relative to the 25°C offset.
|
||||
for (raw, expected) in [
|
||||
(-13_248, -7_500),
|
||||
(-132, 2_401),
|
||||
(-1, 2_500),
|
||||
(0, 2_500),
|
||||
(1, 2_500),
|
||||
(132, 2_599),
|
||||
(13_248, 12_500),
|
||||
] {
|
||||
assert_eq!(scale_temperature(raw), expected);
|
||||
}
|
||||
|
||||
// Independent floating-point datasheet reference over all 65,536 raw
|
||||
// codes. Truncation may lose less than one centidegree, never wrap.
|
||||
let mut previous = i16::MIN;
|
||||
for raw in i16::MIN..=i16::MAX {
|
||||
let actual = scale_temperature(raw);
|
||||
let reference = (f64::from(raw) / 132.48 + 25.0) * 100.0;
|
||||
assert!((f64::from(actual) - reference).abs() < 1.0);
|
||||
assert!(actual >= previous);
|
||||
previous = actual;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -36,27 +36,18 @@ where
|
||||
Clock: MonotonicClock,
|
||||
Bus: SpiDevice,
|
||||
{
|
||||
/// Submits the asynchronous reset transaction.
|
||||
/// Accepts initialization and starts the power-on register-access delay.
|
||||
pub fn begin(&mut self) -> Result<(), Error<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> {
|
||||
if self.imu.is_ready() {
|
||||
return match self.imu.request_sample(self.clock.now()) {
|
||||
Ok(()) | Err(Error::NotReady) => Icm42688ServiceEvent::Idle,
|
||||
Err(error) => Icm42688ServiceEvent::Fault(error),
|
||||
};
|
||||
}
|
||||
let _ = self.clock.now();
|
||||
match self.imu.advance_initialization() {
|
||||
Ok(()) => Icm42688ServiceEvent::Idle,
|
||||
Err(error) => Icm42688ServiceEvent::Fault(error),
|
||||
}
|
||||
service_event(self.imu.on_periodic_tick(self.clock.now()))
|
||||
}
|
||||
|
||||
/// Handles one coalesced data-ready event.
|
||||
/// Handles one coalesced data-ready notification. Time is captured here in
|
||||
/// the worker, at read submission, not at IRQ entry or physical sampling.
|
||||
pub fn on_data_ready(&mut self) -> Icm42688ServiceEvent<Bus::StartError> {
|
||||
if !self.imu.is_ready() {
|
||||
return Icm42688ServiceEvent::Idle;
|
||||
@@ -72,11 +63,110 @@ where
|
||||
&mut self,
|
||||
completion: SpiCompletion,
|
||||
) -> 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::Initialized => Icm42688ServiceEvent::Initialized,
|
||||
Icm42688Event::Sample(sample) => Icm42688ServiceEvent::Sample(sample),
|
||||
Icm42688Event::Fault(error) => Icm42688ServiceEvent::Fault(error),
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::mpu6000::test_bus::FakeBus;
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
struct Clock(u64);
|
||||
impl MonotonicClock for Clock {
|
||||
fn now(&mut self) -> Timestamp {
|
||||
Timestamp::from_microseconds(self.0)
|
||||
}
|
||||
}
|
||||
|
||||
fn initialized() -> Icm42688Service<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);
|
||||
}
|
||||
}
|
||||
@@ -1,8 +1,4 @@
|
||||
//! Hardware-independent chip protocol drivers.
|
||||
//!
|
||||
//! Drivers express device register protocols and data conversion through small
|
||||
//! bus traits. MCU, pin, DMA, interrupt, and RTIC mechanics belong below this
|
||||
//! layer in a HAL route implementation and its platform composition.
|
||||
|
||||
#![no_std]
|
||||
|
||||
|
||||
@@ -29,6 +29,9 @@ pub enum Error<BusError> {
|
||||
/// The device reported a documented no-signal sentinel distance of zero or one centimetre.
|
||||
#[kaptan_error(id = 5208)]
|
||||
NoSignal,
|
||||
/// The controller rejected completion or could not prove physical STOP.
|
||||
#[kaptan_error(id = 5209)]
|
||||
Completion(kaptan_i2c::CompletionError),
|
||||
}
|
||||
|
||||
/// LIDAR-Lite v3 startup lifecycle failure.
|
||||
|
||||
@@ -4,7 +4,7 @@ mod error;
|
||||
mod registers;
|
||||
pub mod service;
|
||||
|
||||
use kaptan_i2c::{I2cBus, I2cCompletion};
|
||||
use kaptan_i2c::{CompletionError, I2cBus, I2cCompletion};
|
||||
use kaptan_sensor::range::RangeSnapshot;
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
@@ -24,6 +24,8 @@ const DISTANCE_LENGTH: usize = 2;
|
||||
///
|
||||
/// The extra margin preserves liveness if the optical engine remains busy
|
||||
/// while still accepting a documented slow acquisition.
|
||||
/// Completion at exactly this elapsed time is accepted. Later completion is
|
||||
/// discarded, regardless of whether the periodic timeout handler has run.
|
||||
const MEASUREMENT_TIMEOUT_MICROSECONDS: u64 = 200_000;
|
||||
|
||||
/// Receiver-bias correction selection for one acquisition.
|
||||
@@ -50,6 +52,7 @@ impl AcquisitionMode {
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
enum State {
|
||||
Settling,
|
||||
Faulted,
|
||||
Idle,
|
||||
Starting { timestamp: Timestamp },
|
||||
Waiting { timestamp: Timestamp },
|
||||
@@ -174,6 +177,7 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
|
||||
timestamp: Timestamp,
|
||||
mode: AcquisitionMode,
|
||||
) -> Result<(), Error<Bus::StartError>> {
|
||||
self.service_bus(timestamp)?;
|
||||
if self.state != State::Idle {
|
||||
return Err(Error::NotReady);
|
||||
}
|
||||
@@ -192,15 +196,16 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
|
||||
/// therefore only records phase completion; this periodic path begins the
|
||||
/// following transaction after the bus has been released.
|
||||
pub fn poll_measurement(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
|
||||
self.service_bus(now)?;
|
||||
let timestamp = match self.state {
|
||||
State::Waiting { timestamp }
|
||||
| State::StatusReadPending { timestamp }
|
||||
| State::DistanceRegisterPending { timestamp }
|
||||
| State::DistanceReadPending { timestamp } => timestamp,
|
||||
State::Settling | State::Idle => return Err(Error::NotReady),
|
||||
State::Settling | State::Idle | State::Faulted => return Err(Error::NotReady),
|
||||
_ => {
|
||||
if self.measurement_timed_out(now) {
|
||||
self.bus.cancel_transaction();
|
||||
self.bus.cancel_transaction(now);
|
||||
self.state = State::Idle;
|
||||
return Err(Error::MeasurementTimeout);
|
||||
}
|
||||
@@ -208,7 +213,7 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
|
||||
}
|
||||
};
|
||||
if self.measurement_timed_out(now) {
|
||||
self.bus.cancel_transaction();
|
||||
self.bus.cancel_transaction(now);
|
||||
self.state = State::Idle;
|
||||
return Err(Error::MeasurementTimeout);
|
||||
}
|
||||
@@ -222,17 +227,36 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
|
||||
}
|
||||
|
||||
/// Advances the state machine from an already-acknowledged I²C terminal event.
|
||||
/// `now` is the actual completion-handler time in the acquisition clock domain.
|
||||
pub fn on_transaction_complete(
|
||||
&mut self,
|
||||
completion: I2cCompletion,
|
||||
now: Timestamp,
|
||||
) -> LidarLiteV3Event<Bus::StartError> {
|
||||
if completion == I2cCompletion::NotPending || !self.has_active_transaction() {
|
||||
return LidarLiteV3Event::NotPending;
|
||||
}
|
||||
|
||||
if self.measurement_timed_out(now) {
|
||||
// Discard a controller-owned completed read without copying it out.
|
||||
// If STOP is still pending, cancellation keeps admission closed and
|
||||
// periodic bus service must finish STOP or report StopUnproven.
|
||||
self.bus.cancel_transaction(now);
|
||||
self.state = State::Idle;
|
||||
return LidarLiteV3Event::Fault(Error::MeasurementTimeout);
|
||||
}
|
||||
|
||||
match completion {
|
||||
I2cCompletion::TransferFault => {
|
||||
self.bus.complete_transaction(completion, None);
|
||||
let result = self.bus.complete_transaction(completion, None);
|
||||
match result {
|
||||
Err(CompletionError::NotPending) => return LidarLiteV3Event::NotPending,
|
||||
Err(CompletionError::TransferFault) | Ok(()) => {}
|
||||
Err(error) => {
|
||||
self.state = State::Faulted;
|
||||
return LidarLiteV3Event::Fault(Error::Completion(error));
|
||||
}
|
||||
}
|
||||
self.state = State::Idle;
|
||||
LidarLiteV3Event::Fault(Error::TransferFault)
|
||||
}
|
||||
@@ -242,7 +266,13 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
|
||||
State::ReadingDistance { .. } => Some(&mut self.receive[..DISTANCE_LENGTH]),
|
||||
_ => None,
|
||||
};
|
||||
self.bus.complete_transaction(completion, read);
|
||||
if let Err(error) = self.bus.complete_transaction(completion, read) {
|
||||
if error == CompletionError::NotPending {
|
||||
return LidarLiteV3Event::NotPending;
|
||||
}
|
||||
self.state = State::Faulted;
|
||||
return LidarLiteV3Event::Fault(Error::Completion(error));
|
||||
}
|
||||
self.advance_after_completion()
|
||||
}
|
||||
I2cCompletion::NotPending => unreachable!(),
|
||||
@@ -254,6 +284,14 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
|
||||
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 {
|
||||
matches!(
|
||||
self.state,
|
||||
@@ -276,16 +314,16 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
|
||||
| State::WritingDistanceRegister { timestamp }
|
||||
| State::DistanceReadPending { timestamp }
|
||||
| State::ReadingDistance { timestamp } => timestamp,
|
||||
State::Settling | State::Idle => return false,
|
||||
State::Settling | State::Idle | State::Faulted => return false,
|
||||
};
|
||||
now.as_microseconds()
|
||||
.saturating_sub(timestamp.as_microseconds())
|
||||
>= MEASUREMENT_TIMEOUT_MICROSECONDS
|
||||
> MEASUREMENT_TIMEOUT_MICROSECONDS
|
||||
}
|
||||
|
||||
fn advance_after_completion(&mut self) -> LidarLiteV3Event<Bus::StartError> {
|
||||
match self.state {
|
||||
State::Settling => LidarLiteV3Event::NotPending,
|
||||
State::Settling | State::Faulted => LidarLiteV3Event::NotPending,
|
||||
State::Idle => LidarLiteV3Event::NotPending,
|
||||
State::Starting { timestamp } => {
|
||||
self.state = State::Waiting { timestamp };
|
||||
@@ -380,7 +418,7 @@ mod tests {
|
||||
AcquisitionMode, Error, LidarLiteV3, LidarLiteV3Address, LidarLiteV3Event,
|
||||
MEASUREMENT_TIMEOUT_MICROSECONDS, StartupError,
|
||||
};
|
||||
use kaptan_i2c::{I2cBus, I2cCompletion};
|
||||
use kaptan_i2c::{CompletionError, I2cBus, I2cCompletion};
|
||||
use kaptan_sensor::range::RangeSnapshot;
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
@@ -408,7 +446,11 @@ mod tests {
|
||||
response: [u8; 2],
|
||||
fail_start: bool,
|
||||
completions: usize,
|
||||
read_copies: usize,
|
||||
cancellations: usize,
|
||||
completion_error: Option<CompletionError>,
|
||||
service_error: Option<CompletionError>,
|
||||
cancelled_at: Option<Timestamp>,
|
||||
}
|
||||
|
||||
impl FakeBus {
|
||||
@@ -418,7 +460,11 @@ mod tests {
|
||||
response: [0; 2],
|
||||
fail_start: false,
|
||||
completions: 0,
|
||||
read_copies: 0,
|
||||
cancellations: 0,
|
||||
completion_error: None,
|
||||
service_error: None,
|
||||
cancelled_at: None,
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -454,17 +500,36 @@ mod tests {
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn complete_transaction(&mut self, completion: I2cCompletion, read: Option<&mut [u8]>) {
|
||||
fn complete_transaction(
|
||||
&mut self,
|
||||
completion: I2cCompletion,
|
||||
read: Option<&mut [u8]>,
|
||||
) -> Result<(), CompletionError> {
|
||||
if completion == I2cCompletion::NotPending {
|
||||
return Err(CompletionError::NotPending);
|
||||
}
|
||||
if let Some(error) = self.completion_error.take() {
|
||||
return Err(error);
|
||||
}
|
||||
if completion == I2cCompletion::Complete
|
||||
&& let Some(read) = read
|
||||
{
|
||||
read.copy_from_slice(&self.response[..read.len()]);
|
||||
self.read_copies += 1;
|
||||
}
|
||||
self.completions += 1;
|
||||
self.transaction = Transaction::None;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn cancel_transaction(&mut self) {
|
||||
fn cancel_transaction(&mut self, now: Timestamp) {
|
||||
self.cancellations += 1;
|
||||
self.cancelled_at = Some(now);
|
||||
self.transaction = Transaction::None;
|
||||
}
|
||||
|
||||
fn service(&mut self, _: Timestamp) -> Result<(), CompletionError> {
|
||||
self.service_error.take().map_or(Ok(()), Err)
|
||||
}
|
||||
}
|
||||
|
||||
@@ -473,12 +538,18 @@ mod tests {
|
||||
let mut driver = LidarLiteV3::new(FakeBus::ready());
|
||||
assert!(!driver.is_ready());
|
||||
assert_eq!(
|
||||
driver.on_transaction_complete(I2cCompletion::NotPending),
|
||||
driver.on_transaction_complete(
|
||||
I2cCompletion::NotPending,
|
||||
Timestamp::from_microseconds(0)
|
||||
),
|
||||
LidarLiteV3Event::NotPending
|
||||
);
|
||||
assert_eq!(driver.bus.completions, 0);
|
||||
assert_eq!(
|
||||
driver.on_transaction_complete(I2cCompletion::TransferFault),
|
||||
driver.on_transaction_complete(
|
||||
I2cCompletion::TransferFault,
|
||||
Timestamp::from_microseconds(0)
|
||||
),
|
||||
LidarLiteV3Event::NotPending
|
||||
);
|
||||
assert_eq!(driver.bus.completions, 0);
|
||||
@@ -514,7 +585,7 @@ mod tests {
|
||||
}
|
||||
);
|
||||
assert_eq!(
|
||||
driver.on_transaction_complete(I2cCompletion::Complete),
|
||||
driver.on_transaction_complete(I2cCompletion::Complete, timestamp),
|
||||
LidarLiteV3Event::MeasurementStarted
|
||||
);
|
||||
driver.poll_measurement(timestamp).unwrap();
|
||||
@@ -528,7 +599,7 @@ mod tests {
|
||||
);
|
||||
driver.bus.response = [STATUS_HEALTHY, 0];
|
||||
assert_eq!(
|
||||
driver.on_transaction_complete(I2cCompletion::Complete),
|
||||
driver.on_transaction_complete(I2cCompletion::Complete, timestamp),
|
||||
LidarLiteV3Event::ReadingStatus
|
||||
);
|
||||
driver.poll_measurement(timestamp).unwrap();
|
||||
@@ -540,7 +611,7 @@ mod tests {
|
||||
}
|
||||
);
|
||||
assert_eq!(
|
||||
driver.on_transaction_complete(I2cCompletion::Complete),
|
||||
driver.on_transaction_complete(I2cCompletion::Complete, timestamp),
|
||||
LidarLiteV3Event::SelectingDistanceRegister
|
||||
);
|
||||
driver.poll_measurement(timestamp).unwrap();
|
||||
@@ -553,7 +624,7 @@ mod tests {
|
||||
}
|
||||
);
|
||||
assert_eq!(
|
||||
driver.on_transaction_complete(I2cCompletion::Complete),
|
||||
driver.on_transaction_complete(I2cCompletion::Complete, timestamp),
|
||||
LidarLiteV3Event::ReadingMeasurement
|
||||
);
|
||||
driver.bus.response = 291_u16.to_be_bytes();
|
||||
@@ -566,7 +637,7 @@ mod tests {
|
||||
}
|
||||
);
|
||||
assert_eq!(
|
||||
driver.on_transaction_complete(I2cCompletion::Complete),
|
||||
driver.on_transaction_complete(I2cCompletion::Complete, timestamp),
|
||||
LidarLiteV3Event::Measurement(RangeSnapshot::new(timestamp, 2_910))
|
||||
);
|
||||
assert!(driver.is_ready());
|
||||
@@ -581,17 +652,20 @@ mod tests {
|
||||
AcquisitionMode::WithoutReceiverBiasCorrection,
|
||||
)
|
||||
.unwrap();
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
driver
|
||||
.poll_measurement(Timestamp::from_microseconds(0))
|
||||
.unwrap();
|
||||
driver.bus.response = [STATUS_HEALTHY | 1, 0];
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
driver
|
||||
.poll_measurement(Timestamp::from_microseconds(0))
|
||||
.unwrap();
|
||||
assert_eq!(
|
||||
driver.on_transaction_complete(I2cCompletion::Complete),
|
||||
driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
|
||||
LidarLiteV3Event::MeasurementBusy
|
||||
);
|
||||
assert!(!driver.is_ready());
|
||||
@@ -611,11 +685,12 @@ mod tests {
|
||||
AcquisitionMode::WithReceiverBiasCorrection,
|
||||
)
|
||||
.unwrap();
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
|
||||
assert_eq!(
|
||||
driver.poll_measurement(Timestamp::from_microseconds(
|
||||
MEASUREMENT_TIMEOUT_MICROSECONDS,
|
||||
MEASUREMENT_TIMEOUT_MICROSECONDS + 1,
|
||||
)),
|
||||
Err(Error::MeasurementTimeout)
|
||||
);
|
||||
@@ -632,17 +707,20 @@ mod tests {
|
||||
AcquisitionMode::WithReceiverBiasCorrection,
|
||||
)
|
||||
.unwrap();
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
driver
|
||||
.poll_measurement(Timestamp::from_microseconds(0))
|
||||
.unwrap();
|
||||
driver.bus.response = [STATUS_HEALTHY | STATUS_PROCESS_ERROR, 0];
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
driver
|
||||
.poll_measurement(Timestamp::from_microseconds(0))
|
||||
.unwrap();
|
||||
assert_eq!(
|
||||
driver.on_transaction_complete(I2cCompletion::Complete),
|
||||
driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
|
||||
LidarLiteV3Event::Fault(Error::ProcessError)
|
||||
);
|
||||
|
||||
@@ -652,17 +730,20 @@ mod tests {
|
||||
AcquisitionMode::WithReceiverBiasCorrection,
|
||||
)
|
||||
.unwrap();
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
driver
|
||||
.poll_measurement(Timestamp::from_microseconds(0))
|
||||
.unwrap();
|
||||
driver.bus.response = [0, 0];
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
driver
|
||||
.poll_measurement(Timestamp::from_microseconds(0))
|
||||
.unwrap();
|
||||
assert_eq!(
|
||||
driver.on_transaction_complete(I2cCompletion::Complete),
|
||||
driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
|
||||
LidarLiteV3Event::Fault(Error::Unhealthy)
|
||||
);
|
||||
|
||||
@@ -672,17 +753,20 @@ mod tests {
|
||||
AcquisitionMode::WithReceiverBiasCorrection,
|
||||
)
|
||||
.unwrap();
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
driver
|
||||
.poll_measurement(Timestamp::from_microseconds(0))
|
||||
.unwrap();
|
||||
driver.bus.response = [STATUS_HEALTHY | STATUS_INVALID_SIGNAL, 0];
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
driver
|
||||
.poll_measurement(Timestamp::from_microseconds(0))
|
||||
.unwrap();
|
||||
assert_eq!(
|
||||
driver.on_transaction_complete(I2cCompletion::Complete),
|
||||
driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
|
||||
LidarLiteV3Event::Fault(Error::InvalidSignal)
|
||||
);
|
||||
}
|
||||
@@ -696,20 +780,25 @@ mod tests {
|
||||
AcquisitionMode::WithReceiverBiasCorrection,
|
||||
)
|
||||
.unwrap();
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
driver
|
||||
.poll_measurement(Timestamp::from_microseconds(0))
|
||||
.unwrap();
|
||||
driver.bus.response = [STATUS_HEALTHY, 0];
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
driver.bus.response = 1_u16.to_be_bytes();
|
||||
let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
|
||||
assert_eq!(
|
||||
driver.on_transaction_complete(I2cCompletion::Complete),
|
||||
driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
|
||||
LidarLiteV3Event::Fault(Error::NoSignal)
|
||||
);
|
||||
|
||||
@@ -719,20 +808,25 @@ mod tests {
|
||||
AcquisitionMode::WithReceiverBiasCorrection,
|
||||
)
|
||||
.unwrap();
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
driver
|
||||
.poll_measurement(Timestamp::from_microseconds(0))
|
||||
.unwrap();
|
||||
driver.bus.response = [STATUS_HEALTHY, 0];
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
driver.bus.response = 0_u16.to_be_bytes();
|
||||
let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
|
||||
assert_eq!(
|
||||
driver.on_transaction_complete(I2cCompletion::Complete),
|
||||
driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
|
||||
LidarLiteV3Event::Fault(Error::NoSignal)
|
||||
);
|
||||
|
||||
@@ -742,20 +836,25 @@ mod tests {
|
||||
AcquisitionMode::WithReceiverBiasCorrection,
|
||||
)
|
||||
.unwrap();
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
driver
|
||||
.poll_measurement(Timestamp::from_microseconds(0))
|
||||
.unwrap();
|
||||
driver.bus.response = [STATUS_HEALTHY, 0];
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
|
||||
let _ = driver.on_transaction_complete(I2cCompletion::Complete);
|
||||
let _ = driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
|
||||
driver.bus.response = u16::MAX.to_be_bytes();
|
||||
let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
|
||||
assert_eq!(
|
||||
driver.on_transaction_complete(I2cCompletion::Complete),
|
||||
driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
|
||||
LidarLiteV3Event::Measurement(RangeSnapshot::new(
|
||||
Timestamp::from_microseconds(0),
|
||||
655_350,
|
||||
@@ -773,9 +872,147 @@ mod tests {
|
||||
)
|
||||
.unwrap();
|
||||
assert_eq!(
|
||||
driver.on_transaction_complete(I2cCompletion::TransferFault),
|
||||
driver.on_transaction_complete(
|
||||
I2cCompletion::TransferFault,
|
||||
Timestamp::from_microseconds(0)
|
||||
),
|
||||
LidarLiteV3Event::Fault(Error::TransferFault)
|
||||
);
|
||||
assert!(driver.is_ready());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn cancellation_is_serviced_after_acquisition_timeout_and_stop_failure_latches_fault() {
|
||||
let mut driver = initialized();
|
||||
driver
|
||||
.request_measurement(
|
||||
Timestamp::from_microseconds(123),
|
||||
AcquisitionMode::WithReceiverBiasCorrection,
|
||||
)
|
||||
.unwrap();
|
||||
let timeout = Timestamp::from_microseconds(200_124);
|
||||
assert_eq!(
|
||||
driver.poll_measurement(timeout),
|
||||
Err(Error::MeasurementTimeout)
|
||||
);
|
||||
assert_eq!(driver.bus.cancelled_at, Some(timeout));
|
||||
driver.bus.service_error = Some(CompletionError::StopUnproven);
|
||||
assert_eq!(
|
||||
driver.request_measurement(
|
||||
Timestamp::from_microseconds(210_123),
|
||||
AcquisitionMode::WithReceiverBiasCorrection
|
||||
),
|
||||
Err(Error::Completion(CompletionError::StopUnproven))
|
||||
);
|
||||
assert!(!driver.is_ready());
|
||||
assert_eq!(
|
||||
driver.on_transaction_complete(
|
||||
I2cCompletion::Complete,
|
||||
Timestamp::from_microseconds(210_123)
|
||||
),
|
||||
LidarLiteV3Event::NotPending
|
||||
);
|
||||
assert_eq!(
|
||||
driver.poll_measurement(Timestamp::from_microseconds(220_123)),
|
||||
Err(Error::NotReady)
|
||||
);
|
||||
assert_eq!(driver.bus.cancellations, 1);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn unproven_completion_cannot_advance_a_phase_or_publish_stale_bytes() {
|
||||
let mut driver = initialized();
|
||||
driver
|
||||
.request_measurement(
|
||||
Timestamp::from_microseconds(0),
|
||||
AcquisitionMode::WithReceiverBiasCorrection,
|
||||
)
|
||||
.unwrap();
|
||||
driver.bus.completion_error = Some(CompletionError::NotPending);
|
||||
assert_eq!(
|
||||
driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
|
||||
LidarLiteV3Event::NotPending
|
||||
);
|
||||
assert_eq!(
|
||||
driver.state,
|
||||
super::State::Starting {
|
||||
timestamp: Timestamp::from_microseconds(0)
|
||||
}
|
||||
);
|
||||
driver.bus.completion_error = Some(CompletionError::StopUnproven);
|
||||
assert_eq!(
|
||||
driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
|
||||
LidarLiteV3Event::Fault(Error::Completion(CompletionError::StopUnproven))
|
||||
);
|
||||
assert!(!driver.is_ready());
|
||||
assert_eq!(
|
||||
driver
|
||||
.on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
|
||||
LidarLiteV3Event::NotPending
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn late_terminal_events_discard_every_active_phase_without_copying() {
|
||||
for phase in 0..5 {
|
||||
for completion in [I2cCompletion::Complete, I2cCompletion::TransferFault] {
|
||||
let mut driver = initialized();
|
||||
let start = Timestamp::from_microseconds(123);
|
||||
driver
|
||||
.request_measurement(start, AcquisitionMode::WithReceiverBiasCorrection)
|
||||
.unwrap();
|
||||
driver.bus.response = [STATUS_HEALTHY, 0];
|
||||
for step in 0..phase {
|
||||
let now = 124 + step * 2;
|
||||
driver.on_transaction_complete(
|
||||
I2cCompletion::Complete,
|
||||
Timestamp::from_microseconds(now),
|
||||
);
|
||||
driver
|
||||
.poll_measurement(Timestamp::from_microseconds(now + 1))
|
||||
.unwrap();
|
||||
}
|
||||
let before = driver.state;
|
||||
let copies = driver.bus.read_copies;
|
||||
let completions = driver.bus.completions;
|
||||
driver.receive = [0xa5; 2];
|
||||
let late = Timestamp::from_microseconds(200_124);
|
||||
|
||||
// A nonterminal notification is not permission to cancel, even
|
||||
// when the timer has been delayed past the acquisition limit.
|
||||
assert_eq!(
|
||||
driver.on_transaction_complete(I2cCompletion::NotPending, late),
|
||||
LidarLiteV3Event::NotPending
|
||||
);
|
||||
assert_eq!(driver.state, before);
|
||||
assert_eq!(driver.bus.cancellations, 0);
|
||||
assert_eq!(driver.bus.completions, completions);
|
||||
assert_eq!(driver.receive, [0xa5; 2]);
|
||||
|
||||
assert_eq!(
|
||||
driver.on_transaction_complete(completion, late),
|
||||
LidarLiteV3Event::Fault(Error::MeasurementTimeout)
|
||||
);
|
||||
assert_eq!(driver.bus.cancelled_at, Some(late));
|
||||
assert_eq!(driver.bus.cancellations, 1);
|
||||
assert_eq!(driver.bus.transaction, Transaction::None);
|
||||
assert_eq!(driver.bus.read_copies, copies);
|
||||
assert_eq!(driver.receive, [0xa5; 2]);
|
||||
assert_eq!(
|
||||
driver.on_transaction_complete(completion, late),
|
||||
LidarLiteV3Event::NotPending
|
||||
);
|
||||
|
||||
// Cancellation is still serviced if physical STOP was unproven.
|
||||
driver.bus.service_error = Some(CompletionError::StopUnproven);
|
||||
assert_eq!(
|
||||
driver.poll_measurement(Timestamp::from_microseconds(210_124)),
|
||||
Err(Error::Completion(CompletionError::StopUnproven))
|
||||
);
|
||||
assert!(!driver.is_ready());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -3,16 +3,16 @@
|
||||
use super::{AcquisitionMode, Error, LidarLiteV3, LidarLiteV3Event, StartupError};
|
||||
use kaptan_i2c::{I2cBus, I2cCompletion};
|
||||
use kaptan_sensor::range::RangeSnapshot;
|
||||
use kaptan_time::MonotonicClock;
|
||||
use kaptan_time::{MonotonicClock, Timestamp};
|
||||
|
||||
/// One observable result from a LIDAR-Lite v3 acquisition service.
|
||||
#[derive(Debug)]
|
||||
pub enum LidarLiteV3ServiceEvent<BusError> {
|
||||
/// No observation or terminal fault was produced.
|
||||
/// No observation or reportable error was produced.
|
||||
Idle,
|
||||
/// One coherent range observation is ready for publication.
|
||||
Measurement(RangeSnapshot),
|
||||
/// The LIDAR-Lite v3 path entered a terminal fault state.
|
||||
/// Acquisition or controller failed.
|
||||
Fault(Error<BusError>),
|
||||
}
|
||||
|
||||
@@ -55,22 +55,21 @@ impl<Clock: MonotonicClock, Bus: I2cBus> LidarLiteV3Service<Clock, Bus> {
|
||||
&mut self,
|
||||
completion: I2cCompletion,
|
||||
) -> 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.
|
||||
///
|
||||
/// The closure keeps controller-specific interrupt handling at the board
|
||||
/// boundary while preserving this service as the sole owner of LIDAR state.
|
||||
/// Acknowledges a controller interrupt and advances the acquisition.
|
||||
pub fn on_controller_interrupt<F>(
|
||||
&mut self,
|
||||
acknowledge: F,
|
||||
) -> LidarLiteV3ServiceEvent<Bus::StartError>
|
||||
where
|
||||
F: FnOnce(&mut Bus) -> I2cCompletion,
|
||||
F: FnOnce(&mut Bus, Timestamp) -> I2cCompletion,
|
||||
{
|
||||
let completion = self.lidar.acknowledge_with(acknowledge);
|
||||
self.on_transaction_complete(completion)
|
||||
let now = self.clock.now();
|
||||
let completion = self.lidar.acknowledge_with(|bus| acknowledge(bus, now));
|
||||
service_event(self.lidar.on_transaction_complete(completion, now))
|
||||
}
|
||||
}
|
||||
|
||||
@@ -89,19 +88,22 @@ fn service_event<BusError>(event: LidarLiteV3Event<BusError>) -> LidarLiteV3Serv
|
||||
mod tests {
|
||||
use super::{LidarLiteV3Service, LidarLiteV3ServiceEvent, service_event};
|
||||
use crate::lidar_lite_v3::{AcquisitionMode, Error, LidarLiteV3, LidarLiteV3Event};
|
||||
use kaptan_i2c::{I2cBus, I2cCompletion};
|
||||
use kaptan_i2c::{CompletionError, I2cBus, I2cCompletion};
|
||||
use kaptan_time::{MonotonicClock, Timestamp};
|
||||
|
||||
struct Clock;
|
||||
struct Clock(u64);
|
||||
|
||||
impl MonotonicClock for Clock {
|
||||
fn now(&mut self) -> Timestamp {
|
||||
Timestamp::from_microseconds(123)
|
||||
Timestamp::from_microseconds(self.0)
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Default)]
|
||||
struct Bus {
|
||||
response: [u8; 2],
|
||||
cancelled_at: Option<Timestamp>,
|
||||
read_copies: usize,
|
||||
}
|
||||
|
||||
impl I2cBus for Bus {
|
||||
@@ -115,22 +117,37 @@ mod tests {
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn complete_transaction(&mut self, completion: I2cCompletion, read: Option<&mut [u8]>) {
|
||||
fn complete_transaction(
|
||||
&mut self,
|
||||
completion: I2cCompletion,
|
||||
read: Option<&mut [u8]>,
|
||||
) -> Result<(), CompletionError> {
|
||||
if completion == I2cCompletion::NotPending {
|
||||
return Err(CompletionError::NotPending);
|
||||
}
|
||||
if completion == I2cCompletion::Complete
|
||||
&& let Some(read) = read
|
||||
{
|
||||
read.copy_from_slice(&self.response[..read.len()]);
|
||||
self.read_copies += 1;
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn cancel_transaction(&mut self) {}
|
||||
fn cancel_transaction(&mut self, now: Timestamp) {
|
||||
self.cancelled_at = Some(now);
|
||||
}
|
||||
|
||||
fn service(&mut self, _: Timestamp) -> Result<(), CompletionError> {
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn produces_a_measurement_after_stop_separated_i2c_transactions() {
|
||||
let mut service = LidarLiteV3Service::new(
|
||||
Clock,
|
||||
LidarLiteV3::new(Bus { response: [0; 2] }),
|
||||
Clock(123),
|
||||
LidarLiteV3::new(Bus::default()),
|
||||
AcquisitionMode::WithReceiverBiasCorrection,
|
||||
);
|
||||
service.finish_startup().unwrap();
|
||||
@@ -148,7 +165,7 @@ mod tests {
|
||||
LidarLiteV3ServiceEvent::Idle
|
||||
));
|
||||
assert!(matches!(
|
||||
service.on_controller_interrupt(|bus| {
|
||||
service.on_controller_interrupt(|bus, _| {
|
||||
bus.response = [1 << 5, 0];
|
||||
I2cCompletion::Complete
|
||||
}),
|
||||
@@ -159,7 +176,7 @@ mod tests {
|
||||
LidarLiteV3ServiceEvent::Idle
|
||||
));
|
||||
assert!(matches!(
|
||||
service.on_controller_interrupt(|_| I2cCompletion::Complete),
|
||||
service.on_controller_interrupt(|_, _| I2cCompletion::Complete),
|
||||
LidarLiteV3ServiceEvent::Idle
|
||||
));
|
||||
assert!(matches!(
|
||||
@@ -167,7 +184,7 @@ mod tests {
|
||||
LidarLiteV3ServiceEvent::Idle
|
||||
));
|
||||
assert!(matches!(
|
||||
service.on_controller_interrupt(|bus| {
|
||||
service.on_controller_interrupt(|bus, _| {
|
||||
bus.response = [0x01, 0x23];
|
||||
I2cCompletion::Complete
|
||||
}),
|
||||
@@ -178,7 +195,7 @@ mod tests {
|
||||
LidarLiteV3ServiceEvent::Idle
|
||||
));
|
||||
assert!(matches!(
|
||||
service.on_controller_interrupt(|_| I2cCompletion::Complete),
|
||||
service.on_controller_interrupt(|_, _| I2cCompletion::Complete),
|
||||
LidarLiteV3ServiceEvent::Measurement(sample)
|
||||
if sample.timestamp == Timestamp::from_microseconds(123)
|
||||
&& sample.distance_millimetres == 2_910
|
||||
@@ -200,4 +217,102 @@ mod tests {
|
||||
LidarLiteV3ServiceEvent::Fault(Error::Unhealthy)
|
||||
));
|
||||
}
|
||||
|
||||
fn distance_pending() -> LidarLiteV3Service<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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -18,4 +18,16 @@ pub enum Error<BusError> {
|
||||
/// The selected SPI route reported a terminal transfer fault.
|
||||
#[kaptan_error(id = 5004)]
|
||||
TransferFault,
|
||||
/// The bus could not safely accept and copy out the completed transfer.
|
||||
#[kaptan_error(id = 5005)]
|
||||
Completion(kaptan_spi::CompletionError),
|
||||
/// An accepted SPI transfer exceeded the 10 ms service deadline.
|
||||
#[kaptan_error(id = 5006)]
|
||||
TransferTimeout,
|
||||
/// The bus stayed busy for the pending operation's 20 ms admission budget.
|
||||
#[kaptan_error(id = 5007)]
|
||||
AdmissionTimeout,
|
||||
/// No acquisition was requested within two output periods plus 10 ms.
|
||||
#[kaptan_error(id = 5008)]
|
||||
DataReadyTimeout,
|
||||
}
|
||||
@@ -6,8 +6,11 @@ mod registers;
|
||||
mod sample;
|
||||
pub mod service;
|
||||
|
||||
#[cfg(test)]
|
||||
pub(crate) mod test_bus;
|
||||
|
||||
use kaptan_sensor::imu::ImuSnapshot;
|
||||
use kaptan_spi::{SpiCompletion, SpiDevice};
|
||||
use kaptan_spi::{CompletionError, SpiCompletion, SpiDevice};
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
pub use config::{AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000Config};
|
||||
@@ -22,6 +25,21 @@ use registers::{
|
||||
};
|
||||
|
||||
const TRANSFER_CAPACITY: usize = SAMPLE_BYTES + 1;
|
||||
// RM-MPU-6000A-00 rev 4.2, §4.28 p.41 (bundled datasheet.pdf):
|
||||
// DEVICE_RESET, 100 ms, SIGNAL_PATH_RESET=7, 100 ms.
|
||||
const RESET_SETTLE_US: u64 = 100_000;
|
||||
const SIGNAL_PATH_RESET: u8 = 0x68;
|
||||
// PS-MPU-6000A-00 rev 3.1 §6.3 p.14: register access needs up to 100 ms
|
||||
// after power-up. §6.1 p.12 lists 30 ms TYPICAL gyro start-up and §6.6 p.17
|
||||
// gives a 10 ms maximum PLL settling time. Use a conservative 100 ms wake
|
||||
// allowance, not a guaranteed physical settling bound.
|
||||
const WAKE_SETTLE_US: u64 = 100_000;
|
||||
// Host service budgets, not sensor timing specifications. A 15-byte burst on
|
||||
// the supported F722 SPI route takes less than 0.3 ms. Periodic service must
|
||||
// continue even if no DMA/DRDY interrupt arrives.
|
||||
const TRANSFER_TIMEOUT_US: u64 = 10_000;
|
||||
// Allows a retry on the F722's 10 ms periodic service before admission expires.
|
||||
const ADMISSION_TIMEOUT_US: u64 = 20_000;
|
||||
const CONFIGURATION: [(u8, u8); 9] = [
|
||||
(PWR_MGMT_1, CLOCK_SOURCE_PLL_X_GYRO),
|
||||
(USER_CTRL, I2C_INTERFACE_DISABLE),
|
||||
@@ -37,18 +55,41 @@ const CONFIGURATION: [(u8, u8); 9] = [
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
enum State {
|
||||
Dormant,
|
||||
Resetting,
|
||||
WaitingForReset,
|
||||
ReadingIdentity,
|
||||
WritingConfiguration(usize),
|
||||
Ready,
|
||||
ReadingSample { timestamp: Timestamp },
|
||||
Pending {
|
||||
operation: Operation,
|
||||
since: Timestamp,
|
||||
},
|
||||
Active {
|
||||
operation: Operation,
|
||||
since: Timestamp,
|
||||
},
|
||||
Waiting {
|
||||
next: Operation,
|
||||
since: Timestamp,
|
||||
delay_us: u64,
|
||||
},
|
||||
Settling {
|
||||
since: Timestamp,
|
||||
},
|
||||
Ready {
|
||||
since: Timestamp,
|
||||
},
|
||||
Faulted,
|
||||
}
|
||||
|
||||
/// Observable outcome of one SPI terminal event.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
enum Operation {
|
||||
Reset,
|
||||
ResetSignalPaths,
|
||||
Identity,
|
||||
Configure(usize),
|
||||
Sample,
|
||||
}
|
||||
|
||||
/// Observable outcome of one timer or SPI terminal event.
|
||||
#[derive(Debug, Eq, PartialEq)]
|
||||
pub enum Mpu6000Event<BusError> {
|
||||
/// The selected SPI interrupt had no terminal transfer event.
|
||||
/// No sample, initialization completion or terminal fault was produced.
|
||||
NotPending,
|
||||
/// One initialization transfer completed; initialization continues.
|
||||
Initializing,
|
||||
@@ -62,8 +103,8 @@ pub enum Mpu6000Event<BusError> {
|
||||
|
||||
/// Configured MPU6000 driver.
|
||||
///
|
||||
/// The driver owns fixed transfer buffers and advances only from external
|
||||
/// interrupt events. It never waits for SPI status flags.
|
||||
/// The driver owns ordinary movable staging buffers and advances only from
|
||||
/// external events. The bus copies these bytes; hardware never addresses them.
|
||||
pub struct Mpu6000<Bus> {
|
||||
bus: Bus,
|
||||
config: Mpu6000Config,
|
||||
@@ -86,109 +127,144 @@ impl<Bus> Mpu6000<Bus> {
|
||||
}
|
||||
|
||||
impl<Bus: SpiDevice> Mpu6000<Bus> {
|
||||
/// Starts a device reset without waiting for SPI.
|
||||
///
|
||||
/// The caller must subsequently call [`Self::advance_initialization`] only
|
||||
/// after the platform-selected reset-settling interval has elapsed.
|
||||
pub fn begin_initialization(&mut self) -> Result<(), Error<Bus::StartError>> {
|
||||
/// Accepts initialization once. Busy admission is retried by periodic
|
||||
/// service. All supplied instants must use one non-regressing boot clock.
|
||||
/// Power must be stable by this call. Register access waits another 100 ms.
|
||||
pub fn begin_initialization(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
|
||||
if self.state != State::Dormant {
|
||||
return Err(Error::NotReady);
|
||||
}
|
||||
|
||||
self.prepare_write(PWR_MGMT_1, DEVICE_RESET);
|
||||
self.submit(2)?;
|
||||
self.state = State::Resetting;
|
||||
self.wait(Operation::Reset, now, RESET_SETTLE_US);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Starts identity verification after the required reset-settling interval.
|
||||
///
|
||||
/// This operation only submits the next asynchronous SPI transfer; it
|
||||
/// never waits for a peripheral or sensor status flag.
|
||||
pub fn advance_initialization(&mut self) -> Result<(), Error<Bus::StartError>> {
|
||||
if self.state != State::WaitingForReset {
|
||||
return Ok(());
|
||||
/// Services elapsed settling, admission and acquisition deadlines. This must
|
||||
/// be called periodically throughout the driver's lifetime, including while
|
||||
/// a transfer is active. It never starts an unsolicited sample read.
|
||||
pub fn on_periodic_tick(&mut self, now: Timestamp) -> Mpu6000Event<Bus::StartError> {
|
||||
if let Some(error) = self.check_deadline(now) {
|
||||
return Mpu6000Event::Fault(error);
|
||||
}
|
||||
match self.state {
|
||||
State::Waiting {
|
||||
next,
|
||||
since,
|
||||
delay_us,
|
||||
} if elapsed(now, since, delay_us) => {
|
||||
self.queue(next, now);
|
||||
}
|
||||
State::Settling { since } if elapsed(now, since, self.sample_period_us()) => {
|
||||
self.state = State::Ready { since: now };
|
||||
return Mpu6000Event::Initialized;
|
||||
}
|
||||
_ => {}
|
||||
}
|
||||
match self.submit_pending(now) {
|
||||
Ok(()) => Mpu6000Event::NotPending,
|
||||
Err(error) => Mpu6000Event::Fault(error),
|
||||
}
|
||||
}
|
||||
|
||||
self.prepare_read(WHO_AM_I, 2);
|
||||
if let Err(error) = self.submit(2) {
|
||||
self.state = State::Dormant;
|
||||
/// Accepts a burst-read request. Busy admission remains pending for periodic
|
||||
/// retry. The eventual accepted submission time becomes the snapshot
|
||||
/// timestamp, not the ISR time or a physical sampling epoch.
|
||||
pub fn request_sample(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
|
||||
if let Some(error) = self.check_deadline(now) {
|
||||
return Err(error);
|
||||
}
|
||||
self.state = State::ReadingIdentity;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Starts one fourteen-byte sensor read when initialization is complete.
|
||||
pub fn request_sample(&mut self, timestamp: Timestamp) -> Result<(), Error<Bus::StartError>> {
|
||||
if self.state != State::Ready {
|
||||
if !self.is_ready() {
|
||||
return Err(Error::NotReady);
|
||||
}
|
||||
|
||||
self.prepare_read(ACCEL_XOUT_H, TRANSFER_CAPACITY);
|
||||
self.submit(TRANSFER_CAPACITY)?;
|
||||
self.state = State::ReadingSample { timestamp };
|
||||
Ok(())
|
||||
self.queue(Operation::Sample, now);
|
||||
self.submit_pending(now)
|
||||
}
|
||||
|
||||
/// Advances the state machine from an already-acknowledged DMA terminal event.
|
||||
pub fn on_transfer_complete(
|
||||
&mut self,
|
||||
completion: SpiCompletion,
|
||||
now: Timestamp,
|
||||
) -> Mpu6000Event<Bus::StartError> {
|
||||
self.bus.complete_transfer(completion);
|
||||
match completion {
|
||||
SpiCompletion::NotPending => Mpu6000Event::NotPending,
|
||||
SpiCompletion::TransferFault => {
|
||||
self.state = State::Dormant;
|
||||
if completion == SpiCompletion::NotPending {
|
||||
return Mpu6000Event::NotPending;
|
||||
}
|
||||
if let Some(error) = self.check_deadline(now) {
|
||||
return Mpu6000Event::Fault(error);
|
||||
}
|
||||
let State::Active { operation, since } = self.state else {
|
||||
return Mpu6000Event::NotPending;
|
||||
};
|
||||
let length = if operation == Operation::Sample {
|
||||
TRANSFER_CAPACITY
|
||||
} else {
|
||||
2
|
||||
};
|
||||
match self
|
||||
.bus
|
||||
.complete_transfer(completion, &mut self.receive[..length])
|
||||
{
|
||||
Ok(()) => self.advance_after_completion(operation, since, now),
|
||||
Err(CompletionError::NotPending) => Mpu6000Event::NotPending,
|
||||
Err(CompletionError::TransferFault) => {
|
||||
self.state = State::Faulted;
|
||||
Mpu6000Event::Fault(Error::TransferFault)
|
||||
}
|
||||
SpiCompletion::Complete => self.advance_after_completion(),
|
||||
Err(error) => {
|
||||
self.state = State::Faulted;
|
||||
Mpu6000Event::Fault(Error::Completion(error))
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Returns whether an IMU data-ready event can start a sensor transfer.
|
||||
pub fn is_ready(&self) -> bool {
|
||||
self.state == State::Ready
|
||||
matches!(self.state, State::Ready { .. })
|
||||
}
|
||||
|
||||
fn advance_after_completion(&mut self) -> Mpu6000Event<Bus::StartError> {
|
||||
match self.state {
|
||||
State::Dormant => Mpu6000Event::NotPending,
|
||||
State::Resetting => {
|
||||
self.state = State::WaitingForReset;
|
||||
Mpu6000Event::Initializing
|
||||
}
|
||||
State::WaitingForReset => Mpu6000Event::NotPending,
|
||||
State::ReadingIdentity => {
|
||||
fn advance_after_completion(
|
||||
&mut self,
|
||||
operation: Operation,
|
||||
requested: Timestamp,
|
||||
now: Timestamp,
|
||||
) -> Mpu6000Event<Bus::StartError> {
|
||||
match operation {
|
||||
Operation::Reset => self.wait(Operation::ResetSignalPaths, now, RESET_SETTLE_US),
|
||||
Operation::ResetSignalPaths => self.wait(Operation::Identity, now, RESET_SETTLE_US),
|
||||
Operation::Identity => {
|
||||
let identity = self.receive[1];
|
||||
if identity != EXPECTED_WHO_AM_I {
|
||||
self.state = State::Dormant;
|
||||
self.state = State::Faulted;
|
||||
return Mpu6000Event::Fault(Error::UnexpectedIdentity(identity));
|
||||
}
|
||||
|
||||
self.start_configuration(0)
|
||||
self.queue(Operation::Configure(0), now);
|
||||
}
|
||||
State::WritingConfiguration(index) => {
|
||||
Operation::Configure(index) => {
|
||||
let next = index + 1;
|
||||
if next == CONFIGURATION.len() {
|
||||
self.state = State::Ready;
|
||||
Mpu6000Event::Initialized
|
||||
if index == 0 {
|
||||
self.wait(Operation::Configure(next), now, WAKE_SETTLE_US);
|
||||
} else if next == CONFIGURATION.len() {
|
||||
// Allow a complete output period after the last write before
|
||||
// accepting DRDY. RM §4.2 defines the configured sample rate.
|
||||
self.state = State::Settling { since: now };
|
||||
} else {
|
||||
self.start_configuration(next)
|
||||
self.queue(Operation::Configure(next), now);
|
||||
}
|
||||
}
|
||||
State::Ready => Mpu6000Event::NotPending,
|
||||
State::ReadingSample { timestamp } => {
|
||||
Operation::Sample => {
|
||||
let mut bytes = [0; SAMPLE_BYTES];
|
||||
bytes.copy_from_slice(&self.receive[1..]);
|
||||
self.state = State::Ready;
|
||||
Mpu6000Event::Sample(sample::decode(&bytes, timestamp, self.config))
|
||||
self.state = State::Ready { since: now };
|
||||
return Mpu6000Event::Sample(sample::decode(&bytes, requested, self.config));
|
||||
}
|
||||
}
|
||||
match self.submit_pending(now) {
|
||||
Ok(()) => Mpu6000Event::Initializing,
|
||||
Err(error) => Mpu6000Event::Fault(error),
|
||||
}
|
||||
}
|
||||
|
||||
fn start_configuration(&mut self, index: usize) -> Mpu6000Event<Bus::StartError> {
|
||||
fn prepare_configuration(&mut self, index: usize) {
|
||||
let (register, value) = CONFIGURATION[index];
|
||||
let value = match register {
|
||||
CONFIG => self.config.digital_low_pass_filter.register_value(),
|
||||
@@ -198,16 +274,6 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
|
||||
_ => value,
|
||||
};
|
||||
self.prepare_write(register, value);
|
||||
match self.submit(2) {
|
||||
Ok(()) => {
|
||||
self.state = State::WritingConfiguration(index);
|
||||
Mpu6000Event::Initializing
|
||||
}
|
||||
Err(error) => {
|
||||
self.state = State::Dormant;
|
||||
Mpu6000Event::Fault(error)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn prepare_read(&mut self, register: u8, length: usize) {
|
||||
@@ -223,21 +289,106 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
|
||||
self.transmit[..2].copy_from_slice(&[register, value]);
|
||||
}
|
||||
|
||||
fn submit(&mut self, length: usize) -> Result<(), Error<Bus::StartError>> {
|
||||
let (bus, transmit, receive) = (&mut self.bus, &self.transmit, &mut self.receive);
|
||||
bus.start_transfer(&transmit[..length], &mut receive[..length])
|
||||
.map_err(Error::Start)
|
||||
fn queue(&mut self, operation: Operation, now: Timestamp) {
|
||||
self.state = State::Pending {
|
||||
operation,
|
||||
since: now,
|
||||
};
|
||||
}
|
||||
|
||||
fn wait(&mut self, next: Operation, now: Timestamp, delay_us: u64) {
|
||||
self.state = State::Waiting {
|
||||
next,
|
||||
since: now,
|
||||
delay_us,
|
||||
};
|
||||
}
|
||||
|
||||
fn submit_pending(&mut self, now: Timestamp) -> Result<(), Error<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)]
|
||||
mod tests {
|
||||
use super::{
|
||||
AccelerometerRange, DigitalLowPassFilter, Error, GyroscopeRange, Mpu6000, Mpu6000Config,
|
||||
Mpu6000Event, TRANSFER_CAPACITY,
|
||||
};
|
||||
use kaptan_spi::{SpiCompletion, SpiDevice};
|
||||
use kaptan_time::Timestamp;
|
||||
use super::test_bus::{FakeBus, FakeError};
|
||||
use super::*;
|
||||
|
||||
const TEST_CONFIG: Mpu6000Config = Mpu6000Config {
|
||||
accelerometer: AccelerometerRange::PlusMinus4G,
|
||||
@@ -246,79 +397,87 @@ mod tests {
|
||||
sample_rate_divider: 9,
|
||||
};
|
||||
|
||||
#[derive(Debug, Eq, PartialEq)]
|
||||
enum FakeError {
|
||||
Start,
|
||||
fn t(us: u64) -> Timestamp {
|
||||
Timestamp::from_microseconds(us + 100_000)
|
||||
}
|
||||
|
||||
struct FakeBus {
|
||||
receive: [u8; TRANSFER_CAPACITY],
|
||||
length: usize,
|
||||
fail_start: bool,
|
||||
fn start(driver: &mut Mpu6000<FakeBus>) {
|
||||
driver
|
||||
.begin_initialization(Timestamp::from_microseconds(0))
|
||||
.unwrap();
|
||||
driver.on_periodic_tick(Timestamp::from_microseconds(99_999));
|
||||
assert_eq!(driver.bus.starts, 0);
|
||||
driver.on_periodic_tick(t(0));
|
||||
}
|
||||
|
||||
impl FakeBus {
|
||||
fn ready() -> Self {
|
||||
Self {
|
||||
receive: [0; TRANSFER_CAPACITY],
|
||||
length: 0,
|
||||
fail_start: false,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl SpiDevice for FakeBus {
|
||||
type StartError = FakeError;
|
||||
|
||||
fn start_transfer(&mut self, _: &[u8], receive: &mut [u8]) -> Result<(), Self::StartError> {
|
||||
if self.fail_start {
|
||||
return Err(FakeError::Start);
|
||||
}
|
||||
|
||||
self.length = receive.len();
|
||||
receive.copy_from_slice(&self.receive[..self.length]);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn complete_transfer(&mut self, _: SpiCompletion) {}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn configures_a_verified_device_without_waiting() {
|
||||
fn initialized() -> Mpu6000<FakeBus> {
|
||||
let mut bus = FakeBus::ready();
|
||||
bus.receive[1] = 0x68;
|
||||
bus.response[1] = 0x68;
|
||||
let mut driver = Mpu6000::new(bus, TEST_CONFIG);
|
||||
|
||||
driver.begin_initialization().unwrap();
|
||||
assert_eq!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete),
|
||||
Mpu6000Event::Initializing
|
||||
);
|
||||
driver.advance_initialization().unwrap();
|
||||
for _ in 0..9 {
|
||||
assert_eq!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete),
|
||||
Mpu6000Event::Initializing
|
||||
);
|
||||
start(&mut driver);
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(100));
|
||||
driver.on_periodic_tick(t(100_100));
|
||||
assert_eq!(&driver.bus.transmitted[..2], &[0x68, 7]);
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(100_200));
|
||||
driver.on_periodic_tick(t(200_200));
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(200_300));
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(200_400));
|
||||
driver.on_periodic_tick(t(300_400));
|
||||
for index in 1..9 {
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(300_400 + index * 100));
|
||||
}
|
||||
assert_eq!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete),
|
||||
driver.on_periodic_tick(t(311_200)),
|
||||
Mpu6000Event::Initialized
|
||||
);
|
||||
assert!(driver.is_ready());
|
||||
driver
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reset_signal_reset_and_wake_wait_elapsed_time_from_completion() {
|
||||
let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
|
||||
driver.bus.response[1] = 0x68;
|
||||
start(&mut driver);
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(100));
|
||||
for now in [100, 100, 50_000, 100_099] {
|
||||
driver.on_periodic_tick(t(now));
|
||||
assert_eq!(driver.bus.starts, 1);
|
||||
assert!(!driver.is_ready());
|
||||
}
|
||||
driver.on_periodic_tick(t(100_100));
|
||||
assert_eq!(driver.bus.starts, 2);
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(100_200));
|
||||
driver.on_periodic_tick(t(200_199));
|
||||
assert_eq!(driver.bus.starts, 2);
|
||||
driver.on_periodic_tick(t(200_200));
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(200_300));
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(200_400));
|
||||
driver.on_periodic_tick(t(300_399));
|
||||
assert_eq!(driver.bus.starts, 4);
|
||||
driver.on_periodic_tick(t(300_400));
|
||||
for index in 1..9 {
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(300_400 + index * 100));
|
||||
}
|
||||
driver.on_periodic_tick(t(311_199));
|
||||
assert!(!driver.is_ready());
|
||||
assert_eq!(driver.request_sample(t(311_199)), Err(Error::NotReady));
|
||||
assert_eq!(
|
||||
driver.on_periodic_tick(t(311_200)),
|
||||
Mpu6000Event::Initialized
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejects_an_unexpected_identity() {
|
||||
let mut bus = FakeBus::ready();
|
||||
bus.receive[1] = 0x70;
|
||||
bus.response[1] = 0x70;
|
||||
let mut driver = Mpu6000::new(bus, TEST_CONFIG);
|
||||
|
||||
driver.begin_initialization().unwrap();
|
||||
let _ = driver.on_transfer_complete(SpiCompletion::Complete);
|
||||
driver.advance_initialization().unwrap();
|
||||
driver.queue(Operation::Identity, t(0));
|
||||
driver.submit_pending(t(0)).unwrap();
|
||||
assert!(matches!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete),
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(100)),
|
||||
Mpu6000Event::Fault(Error::UnexpectedIdentity(0x70))
|
||||
));
|
||||
}
|
||||
@@ -330,22 +489,23 @@ mod tests {
|
||||
0x00, 0x00, // 36.53 C
|
||||
0x40, 0x10, 0xBF, 0xF0, 0x00, 0x00, // gyro: 500, -500, 0 dps
|
||||
];
|
||||
let mut bus = FakeBus::ready();
|
||||
bus.receive[1] = 0x68;
|
||||
let mut driver = Mpu6000::new(bus, TEST_CONFIG);
|
||||
|
||||
driver.begin_initialization().unwrap();
|
||||
let _ = driver.on_transfer_complete(SpiCompletion::Complete);
|
||||
driver.advance_initialization().unwrap();
|
||||
for _ in 0..9 {
|
||||
let _ = driver.on_transfer_complete(SpiCompletion::Complete);
|
||||
}
|
||||
let _ = driver.on_transfer_complete(SpiCompletion::Complete);
|
||||
driver.bus.receive[1..].copy_from_slice(&burst);
|
||||
driver
|
||||
.request_sample(Timestamp::from_microseconds(42))
|
||||
.unwrap();
|
||||
let Mpu6000Event::Sample(sample) = driver.on_transfer_complete(SpiCompletion::Complete)
|
||||
let mut driver = initialized();
|
||||
driver.bus.response[1..].copy_from_slice(&burst);
|
||||
driver.request_sample(t(312_000)).unwrap();
|
||||
assert_eq!(driver.receive, [0; TRANSFER_CAPACITY]);
|
||||
assert_eq!(
|
||||
driver.on_transfer_complete(SpiCompletion::NotPending, t(312_100)),
|
||||
Mpu6000Event::NotPending
|
||||
);
|
||||
assert_eq!(driver.receive, [0; TRANSFER_CAPACITY]);
|
||||
// Overwrite staging bytes and move the whole driver while work is pending.
|
||||
// The fake bus, like the DMA route, owns the pending response itself.
|
||||
driver.transmit.fill(0xff);
|
||||
driver.bus.response.fill(0xff);
|
||||
let mut driver =
|
||||
core::mem::replace(&mut driver, Mpu6000::new(FakeBus::ready(), TEST_CONFIG));
|
||||
let Mpu6000Event::Sample(sample) =
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(312_200))
|
||||
else {
|
||||
panic!("sample transfer must complete");
|
||||
};
|
||||
@@ -356,18 +516,184 @@ mod tests {
|
||||
[8_726, -8_726, 0]
|
||||
);
|
||||
assert_eq!(sample.temperature_centidegrees_celsius, 3_653);
|
||||
assert_eq!(sample.timestamp, t(312_000));
|
||||
assert_eq!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(312_300)),
|
||||
Mpu6000Event::NotPending
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reports_a_route_transfer_fault() {
|
||||
let mut bus = FakeBus::ready();
|
||||
bus.receive[1] = 0x68;
|
||||
bus.response[1] = 0x68;
|
||||
let mut driver = Mpu6000::new(bus, TEST_CONFIG);
|
||||
|
||||
driver.begin_initialization().unwrap();
|
||||
start(&mut driver);
|
||||
assert!(matches!(
|
||||
driver.on_transfer_complete(SpiCompletion::TransferFault),
|
||||
driver.on_transfer_complete(SpiCompletion::TransferFault, t(100)),
|
||||
Mpu6000Event::Fault(Error::TransferFault)
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn completion_errors_never_republish_a_previous_sample() {
|
||||
for (event, error) in [
|
||||
(
|
||||
SpiCompletion::Complete,
|
||||
Some(CompletionError::InvalidLength),
|
||||
),
|
||||
(SpiCompletion::Complete, Some(CompletionError::Quarantined)),
|
||||
(SpiCompletion::Complete, Some(CompletionError::NotActive)),
|
||||
(
|
||||
SpiCompletion::Complete,
|
||||
Some(CompletionError::TransferFault),
|
||||
),
|
||||
(SpiCompletion::TransferFault, None),
|
||||
] {
|
||||
let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
|
||||
driver.state = State::Ready { since: t(0) };
|
||||
driver.request_sample(t(1)).unwrap();
|
||||
assert!(matches!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(1)),
|
||||
Mpu6000Event::Sample(_)
|
||||
));
|
||||
driver.request_sample(t(2)).unwrap();
|
||||
driver.receive.fill(0xa5);
|
||||
driver.bus.fail_completion = error;
|
||||
assert!(matches!(
|
||||
driver.on_transfer_complete(event, t(2)),
|
||||
Mpu6000Event::Fault(_)
|
||||
));
|
||||
assert_eq!(driver.receive, [0xa5; TRANSFER_CAPACITY]);
|
||||
assert!(!driver.is_ready());
|
||||
assert!(!matches!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(3)),
|
||||
Mpu6000Event::Sample(_)
|
||||
));
|
||||
if error == Some(CompletionError::Quarantined) {
|
||||
assert_eq!(driver.begin_initialization(t(4)), Err(Error::NotReady));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejected_submission_cannot_complete_a_stale_sample() {
|
||||
let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
|
||||
driver.state = State::Ready { since: t(0) };
|
||||
driver.receive.fill(0xa5);
|
||||
driver.bus.fail_start = true;
|
||||
assert_eq!(
|
||||
driver.request_sample(t(1)),
|
||||
Err(Error::Start(FakeError::Start))
|
||||
);
|
||||
assert_eq!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(2)),
|
||||
Mpu6000Event::NotPending
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn every_transfer_phase_times_out_and_cancels_once_without_rearming() {
|
||||
let operations = [
|
||||
Operation::Reset,
|
||||
Operation::ResetSignalPaths,
|
||||
Operation::Identity,
|
||||
Operation::Sample,
|
||||
];
|
||||
for operation in operations
|
||||
.into_iter()
|
||||
.chain((0..9).map(Operation::Configure))
|
||||
{
|
||||
let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
|
||||
driver.queue(operation, t(100));
|
||||
driver.submit_pending(t(100)).unwrap();
|
||||
assert_eq!(driver.on_periodic_tick(t(10_099)), Mpu6000Event::NotPending);
|
||||
assert_eq!(driver.bus.cancellations, 0);
|
||||
assert_eq!(
|
||||
driver.on_periodic_tick(t(10_100)),
|
||||
Mpu6000Event::Fault(Error::TransferTimeout)
|
||||
);
|
||||
assert_eq!(driver.bus.cancellations, 1);
|
||||
assert_eq!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(10_101)),
|
||||
Mpu6000Event::NotPending
|
||||
);
|
||||
driver.on_periodic_tick(t(1_000_000));
|
||||
assert_eq!(driver.bus.starts, 1);
|
||||
assert_eq!(
|
||||
driver.begin_initialization(t(1_000_000)),
|
||||
Err(Error::NotReady)
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn every_busy_phase_preserves_pending_operation_and_has_an_admission_deadline() {
|
||||
for operation in [
|
||||
Operation::Reset,
|
||||
Operation::ResetSignalPaths,
|
||||
Operation::Identity,
|
||||
Operation::Sample,
|
||||
]
|
||||
.into_iter()
|
||||
.chain((0..9).map(Operation::Configure))
|
||||
{
|
||||
let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
|
||||
driver.bus.busy = true;
|
||||
driver.queue(operation, t(100));
|
||||
driver.submit_pending(t(100)).unwrap();
|
||||
assert_eq!(
|
||||
driver.state,
|
||||
State::Pending {
|
||||
operation,
|
||||
since: t(100)
|
||||
}
|
||||
);
|
||||
driver.on_periodic_tick(t(200));
|
||||
driver.bus.busy = false;
|
||||
driver.on_periodic_tick(t(300));
|
||||
assert_eq!(
|
||||
driver.state,
|
||||
State::Active {
|
||||
operation,
|
||||
since: t(300)
|
||||
}
|
||||
);
|
||||
assert_eq!(driver.bus.starts, 1);
|
||||
}
|
||||
let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
|
||||
driver.bus.busy = true;
|
||||
start(&mut driver);
|
||||
assert_eq!(
|
||||
driver.on_periodic_tick(t(20_000)),
|
||||
Mpu6000Event::Fault(Error::AdmissionTimeout)
|
||||
);
|
||||
assert_eq!(driver.bus.cancellations, 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn missed_drdy_and_late_completions_fault_instead_of_polling_or_publishing() {
|
||||
let mut driver = initialized();
|
||||
driver.on_periodic_tick(t(341_199));
|
||||
assert_eq!(driver.bus.starts, 12);
|
||||
assert_eq!(
|
||||
driver.on_periodic_tick(t(341_200)),
|
||||
Mpu6000Event::Fault(Error::DataReadyTimeout)
|
||||
);
|
||||
let mut driver = initialized();
|
||||
driver.request_sample(t(312_000)).unwrap();
|
||||
assert_eq!(
|
||||
driver.on_transfer_complete(SpiCompletion::Complete, t(322_000)),
|
||||
Mpu6000Event::Fault(Error::TransferTimeout)
|
||||
);
|
||||
let mut driver = initialized();
|
||||
driver.request_sample(t(312_000)).unwrap();
|
||||
driver.bus.fail_completion = Some(CompletionError::Quarantined);
|
||||
assert_eq!(
|
||||
driver.on_periodic_tick(t(322_000)),
|
||||
Mpu6000Event::Fault(Error::Completion(CompletionError::Quarantined))
|
||||
);
|
||||
assert_eq!(driver.bus.start_transfer(&[1]), Err(FakeError::Quarantined));
|
||||
}
|
||||
}
|
||||
@@ -1,4 +1,10 @@
|
||||
//! MPU6000 burst-sample decoding.
|
||||
//!
|
||||
//! Integer division rounds toward zero before adding temperature's offset.
|
||||
//! For the full signed raw domain, acceleration stays in -16,000..=15,999 mg
|
||||
//! across all ranges and temperature in -5,984..=13,290 cdegC. Gyro conversion
|
||||
//! uses 0.017453 rad/degree and reaches -34,871..=34,870 mrad/s at ±2000°/s.
|
||||
//! Its largest intermediate magnitude is 571,899,904, which fits i32.
|
||||
|
||||
use kaptan_sensor::imu::ImuSnapshot;
|
||||
use kaptan_time::Timestamp;
|
||||
@@ -34,10 +40,98 @@ const fn scale_acceleration(raw: i16, config: Mpu6000Config) -> i16 {
|
||||
((raw as i32 * 1_000) / config.accelerometer.counts_per_g()) as i16
|
||||
}
|
||||
|
||||
const fn scale_angular_rate(raw: i16, config: Mpu6000Config) -> i16 {
|
||||
((raw as i32 * 17_453) / config.gyroscope.milliradians_per_second_divisor()) as i16
|
||||
const fn scale_angular_rate(raw: i16, config: Mpu6000Config) -> i32 {
|
||||
(raw as i32 * 17_453) / config.gyroscope.milliradians_per_second_divisor()
|
||||
}
|
||||
|
||||
const fn scale_temperature(raw: i16) -> i16 {
|
||||
((raw as i32 * 100) / 340 + 3_653) as i16
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::{decode, scale_temperature};
|
||||
use crate::mpu6000::config::{
|
||||
AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000Config,
|
||||
};
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
#[test]
|
||||
fn signed_raw_endpoints_decode_in_every_full_scale_configuration() {
|
||||
// Known answers from the datasheet sensitivities and the documented
|
||||
// 0.017453 rad/degree conversion, truncated toward zero.
|
||||
let accelerometers = [
|
||||
(AccelerometerRange::PlusMinus2G, [-2_000, 0, 1_999]),
|
||||
(AccelerometerRange::PlusMinus4G, [-4_000, 0, 3_999]),
|
||||
(AccelerometerRange::PlusMinus8G, [-8_000, 0, 7_999]),
|
||||
(AccelerometerRange::PlusMinus16G, [-16_000, 0, 15_999]),
|
||||
];
|
||||
let gyroscopes = [
|
||||
(
|
||||
GyroscopeRange::PlusMinus250DegreesPerSecond,
|
||||
[-4_365, 0, 4_365],
|
||||
),
|
||||
(
|
||||
GyroscopeRange::PlusMinus500DegreesPerSecond,
|
||||
[-8_731, 0, 8_731],
|
||||
),
|
||||
(
|
||||
GyroscopeRange::PlusMinus1000DegreesPerSecond,
|
||||
[-17_435, 0, 17_435],
|
||||
),
|
||||
(
|
||||
GyroscopeRange::PlusMinus2000DegreesPerSecond,
|
||||
[-34_871, 0, 34_870],
|
||||
),
|
||||
];
|
||||
let timestamp = Timestamp::from_microseconds(123);
|
||||
for (accelerometer, acceleration) in accelerometers {
|
||||
for (gyroscope, angular_rate) in gyroscopes {
|
||||
for (temperature_raw, temperature) in [(i16::MIN, -5_984), (i16::MAX, 13_290)] {
|
||||
let mut bytes = [0; 14];
|
||||
for (index, raw) in [
|
||||
i16::MIN,
|
||||
0,
|
||||
i16::MAX,
|
||||
temperature_raw,
|
||||
i16::MIN,
|
||||
0,
|
||||
i16::MAX,
|
||||
]
|
||||
.into_iter()
|
||||
.enumerate()
|
||||
{
|
||||
bytes[index * 2..index * 2 + 2].copy_from_slice(&raw.to_be_bytes());
|
||||
}
|
||||
let sample = decode(
|
||||
&bytes,
|
||||
timestamp,
|
||||
Mpu6000Config {
|
||||
accelerometer,
|
||||
gyroscope,
|
||||
digital_low_pass_filter: DigitalLowPassFilter::Bandwidth184Hz,
|
||||
sample_rate_divider: 0,
|
||||
},
|
||||
);
|
||||
assert_eq!(sample.timestamp, timestamp);
|
||||
assert_eq!(sample.acceleration_milligravity, acceleration);
|
||||
assert_eq!(sample.angular_rate_milliradians_per_second, angular_rate);
|
||||
assert_eq!(sample.temperature_centidegrees_celsius, temperature);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn temperature_rounds_toward_zero_before_adding_offset() {
|
||||
for (raw, expected) in [
|
||||
(-340, 3_553),
|
||||
(-1, 3_653),
|
||||
(0, 3_653),
|
||||
(1, 3_653),
|
||||
(340, 3_753),
|
||||
] {
|
||||
assert_eq!(scale_temperature(raw), expected);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -39,28 +39,18 @@ where
|
||||
Clock: MonotonicClock,
|
||||
Bus: SpiDevice,
|
||||
{
|
||||
/// Submits the asynchronous sensor reset transaction.
|
||||
/// Accepts initialization and starts the power-on register-access delay.
|
||||
pub fn begin(&mut self) -> Result<(), Error<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> {
|
||||
if self.imu.is_ready() {
|
||||
return match self.imu.request_sample(self.clock.now()) {
|
||||
Ok(()) | Err(Error::NotReady) => Mpu6000ServiceEvent::Idle,
|
||||
Err(error) => Mpu6000ServiceEvent::Fault(error),
|
||||
};
|
||||
service_event(self.imu.on_periodic_tick(self.clock.now()))
|
||||
}
|
||||
|
||||
let _ = self.clock.now();
|
||||
match self.imu.advance_initialization() {
|
||||
Ok(()) => Mpu6000ServiceEvent::Idle,
|
||||
Err(error) => Mpu6000ServiceEvent::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) -> Mpu6000ServiceEvent<Bus::StartError> {
|
||||
if !self.imu.is_ready() {
|
||||
return Mpu6000ServiceEvent::Idle;
|
||||
@@ -77,22 +67,27 @@ where
|
||||
&mut self,
|
||||
completion: SpiCompletion,
|
||||
) -> 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::Initialized => Mpu6000ServiceEvent::Initialized,
|
||||
Mpu6000Event::Sample(sample) => Mpu6000ServiceEvent::Sample(sample),
|
||||
Mpu6000Event::Fault(error) => Mpu6000ServiceEvent::Fault(error),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::{Mpu6000Service, Mpu6000ServiceEvent};
|
||||
use crate::mpu6000::test_bus::FakeBus;
|
||||
use crate::mpu6000::{
|
||||
AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000, Mpu6000Config,
|
||||
};
|
||||
use kaptan_spi::{SpiCompletion, SpiDevice};
|
||||
use kaptan_spi::SpiCompletion;
|
||||
use kaptan_time::{MonotonicClock, Timestamp};
|
||||
|
||||
const TEST_CONFIG: Mpu6000Config = Mpu6000Config {
|
||||
@@ -102,62 +97,80 @@ mod tests {
|
||||
sample_rate_divider: 9,
|
||||
};
|
||||
|
||||
struct Clock;
|
||||
struct Clock(u64);
|
||||
|
||||
impl MonotonicClock for Clock {
|
||||
fn now(&mut self) -> Timestamp {
|
||||
Timestamp::from_microseconds(42)
|
||||
Timestamp::from_microseconds(self.0)
|
||||
}
|
||||
}
|
||||
|
||||
struct Bus {
|
||||
response: [u8; 15],
|
||||
}
|
||||
|
||||
impl SpiDevice for Bus {
|
||||
type StartError = ();
|
||||
|
||||
fn start_transfer(&mut self, _: &[u8], receive: &mut [u8]) -> Result<(), Self::StartError> {
|
||||
receive.copy_from_slice(&self.response[..receive.len()]);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn complete_transfer(&mut self, _: SpiCompletion) {}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn accepts_a_sample_only_after_initialization() {
|
||||
let mut response = [0; 15];
|
||||
response[1] = 0x68;
|
||||
let mut service = Mpu6000Service::new(Clock, Mpu6000::new(Bus { response }, TEST_CONFIG));
|
||||
let mut bus = FakeBus::ready();
|
||||
bus.response[1] = 0x68;
|
||||
let mut service = Mpu6000Service::new(Clock(0), Mpu6000::new(bus, TEST_CONFIG));
|
||||
|
||||
service.begin().unwrap();
|
||||
assert!(matches!(service.on_data_ready(), Mpu6000ServiceEvent::Idle));
|
||||
service.clock.0 = 99_999;
|
||||
service.on_periodic_tick();
|
||||
assert_eq!(service.imu.bus.starts, 0);
|
||||
service.clock.0 = 100_000;
|
||||
service.on_periodic_tick();
|
||||
assert!(matches!(
|
||||
service.on_transfer_complete(SpiCompletion::Complete),
|
||||
Mpu6000ServiceEvent::Idle
|
||||
));
|
||||
assert!(matches!(
|
||||
service.on_periodic_tick(),
|
||||
Mpu6000ServiceEvent::Idle
|
||||
));
|
||||
for _ in 0..9 {
|
||||
service.clock.0 = 200_000;
|
||||
service.on_periodic_tick();
|
||||
service.on_transfer_complete(SpiCompletion::Complete);
|
||||
service.clock.0 = 300_000;
|
||||
service.on_periodic_tick();
|
||||
service.on_transfer_complete(SpiCompletion::Complete);
|
||||
service.on_transfer_complete(SpiCompletion::Complete);
|
||||
service.clock.0 = 400_000;
|
||||
service.on_periodic_tick();
|
||||
for _ in 1..9 {
|
||||
assert!(matches!(
|
||||
service.on_transfer_complete(SpiCompletion::Complete),
|
||||
Mpu6000ServiceEvent::Idle
|
||||
));
|
||||
}
|
||||
service.clock.0 = 410_000;
|
||||
assert!(matches!(
|
||||
service.on_transfer_complete(SpiCompletion::Complete),
|
||||
service.on_periodic_tick(),
|
||||
Mpu6000ServiceEvent::Initialized
|
||||
));
|
||||
service.clock.0 = 410_042;
|
||||
assert!(matches!(service.on_data_ready(), Mpu6000ServiceEvent::Idle));
|
||||
service.clock.0 = 410_123;
|
||||
assert!(matches!(
|
||||
service.on_transfer_complete(SpiCompletion::Complete),
|
||||
Mpu6000ServiceEvent::Sample(sample) if sample.timestamp == Timestamp::from_microseconds(410_042)
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn fake_clock_expires_active_work_without_any_irq() {
|
||||
let mut service =
|
||||
Mpu6000Service::new(Clock(50), Mpu6000::new(FakeBus::ready(), TEST_CONFIG));
|
||||
service.begin().unwrap();
|
||||
service.clock.0 = 100_050;
|
||||
service.on_periodic_tick();
|
||||
service.clock.0 = 110_049;
|
||||
assert!(matches!(
|
||||
service.on_periodic_tick(),
|
||||
Mpu6000ServiceEvent::Idle
|
||||
));
|
||||
service.clock.0 = 110_050;
|
||||
assert!(matches!(
|
||||
service.on_periodic_tick(),
|
||||
Mpu6000ServiceEvent::Fault(super::Error::TransferTimeout)
|
||||
));
|
||||
assert!(matches!(
|
||||
service.on_transfer_complete(SpiCompletion::Complete),
|
||||
Mpu6000ServiceEvent::Sample(_)
|
||||
Mpu6000ServiceEvent::Idle
|
||||
));
|
||||
}
|
||||
}
|
||||
@@ -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(())
|
||||
}
|
||||
}
|
||||
@@ -1,14 +1,7 @@
|
||||
//! MicoAir MTF-01 Microlink UART frame decoder.
|
||||
//!
|
||||
//! Device documentation belongs beside this module.
|
||||
//!
|
||||
//! The MTF-01 continuously transmits fixed-size Microlink range-sensor frames.
|
||||
//! Feed each received UART byte to [`mtf01::Mtf01::receive_byte`]. The parser is
|
||||
//! allocation-free, does not use packed structs, and resynchronizes on a new
|
||||
//! frame header after malformed input.
|
||||
|
||||
use kaptan_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot};
|
||||
use kaptan_time::{MonotonicClock, Timestamp};
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
const FRAME_HEADER: u8 = 0xEF;
|
||||
const RANGE_SENSOR_MESSAGE_ID: u8 = 0x51;
|
||||
@@ -26,11 +19,7 @@ enum State {
|
||||
Checksum,
|
||||
}
|
||||
|
||||
/// One complete MTF-01 range and optical-flow report.
|
||||
///
|
||||
/// This preserves the MTF-01's device-specific fields. Use
|
||||
/// [`Self::range_snapshot`] or [`Self::optical_flow_snapshot`] to obtain a
|
||||
/// device-independent observation for the rest of the flight stack.
|
||||
/// MTF-01 range and optical-flow report in device units.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct Mtf01Report {
|
||||
/// Flight-controller monotonic time at frame reception.
|
||||
@@ -58,34 +47,6 @@ pub struct Mtf01Report {
|
||||
}
|
||||
|
||||
impl Mtf01Report {
|
||||
/// Creates an MTF-01 observation from one validated Microlink frame.
|
||||
#[allow(clippy::too_many_arguments)]
|
||||
pub const fn new(
|
||||
timestamp: Timestamp,
|
||||
sensor_time_milliseconds: u32,
|
||||
distance_millimetres: u32,
|
||||
distance_strength: u8,
|
||||
distance_precision: u8,
|
||||
distance_valid: bool,
|
||||
flow_velocity_x_centimetres_per_second_at_one_metre: i16,
|
||||
flow_velocity_y_centimetres_per_second_at_one_metre: i16,
|
||||
flow_quality: u8,
|
||||
flow_valid: bool,
|
||||
) -> Self {
|
||||
Self {
|
||||
timestamp,
|
||||
sensor_time_milliseconds,
|
||||
distance_millimetres,
|
||||
distance_strength,
|
||||
distance_precision,
|
||||
distance_valid,
|
||||
flow_velocity_x_centimetres_per_second_at_one_metre,
|
||||
flow_velocity_y_centimetres_per_second_at_one_metre,
|
||||
flow_quality,
|
||||
flow_valid,
|
||||
}
|
||||
}
|
||||
|
||||
/// Returns the valid range portion of this report.
|
||||
pub const fn range_snapshot(self) -> Option<RangeSnapshot> {
|
||||
if self.distance_valid && self.distance_millimetres != 0 {
|
||||
@@ -129,13 +90,10 @@ impl Mtf01Report {
|
||||
}
|
||||
}
|
||||
|
||||
/// MTF-01 Microlink decoder.
|
||||
/// Microlink decoder for one UART receive stream.
|
||||
///
|
||||
/// One decoder is owned by one UART receive path. It accepts the MTF-01's
|
||||
/// documented range-sensor message ID and 20-byte payload length. Device and
|
||||
/// system IDs are deliberately accepted as transmitted, matching MicoAir's
|
||||
/// reference decoder and allowing sensor IDs configured outside factory
|
||||
/// defaults.
|
||||
/// Accepts message ID 0x51 with a 20-byte payload. Device and system IDs are
|
||||
/// unrestricted, matching MicoAir's reference decoder.
|
||||
pub struct Mtf01 {
|
||||
state: State,
|
||||
checksum: u8,
|
||||
@@ -155,9 +113,8 @@ impl Mtf01 {
|
||||
/// Accepts one UART byte and returns a complete, checksum-validated MTF-01
|
||||
/// report when available.
|
||||
///
|
||||
/// `timestamp` should be the controller monotonic time at receipt of this
|
||||
/// byte. It is used only for a completed frame, so it records the arrival
|
||||
/// time of the report rather than an arbitrary intermediate byte.
|
||||
/// `timestamp` is the controller's monotonic time at byte receipt.
|
||||
/// The report retains the checksum byte's timestamp.
|
||||
pub fn receive_byte(&mut self, timestamp: Timestamp, byte: u8) -> Option<Mtf01Report> {
|
||||
match self.state {
|
||||
State::Header => {
|
||||
@@ -198,10 +155,7 @@ impl Mtf01 {
|
||||
}
|
||||
State::Checksum => {
|
||||
let valid = byte == self.checksum;
|
||||
// The checksum terminates the frame even when its value is
|
||||
// also the Microlink header. The reference parser returns to
|
||||
// its initial state here rather than treating checksum data as
|
||||
// the start of the next frame.
|
||||
// A checksum of 0xEF ends the frame and is not a new header.
|
||||
self.reset();
|
||||
if valid {
|
||||
return Some(self.snapshot(timestamp));
|
||||
@@ -233,18 +187,22 @@ impl Mtf01 {
|
||||
}
|
||||
|
||||
fn snapshot(&self, timestamp: Timestamp) -> Mtf01Report {
|
||||
Mtf01Report::new(
|
||||
Mtf01Report {
|
||||
timestamp,
|
||||
u32::from_le_bytes(self.payload[0..4].try_into().unwrap()),
|
||||
u32::from_le_bytes(self.payload[4..8].try_into().unwrap()),
|
||||
self.payload[8],
|
||||
self.payload[9],
|
||||
self.payload[10] == 1,
|
||||
i16::from_le_bytes(self.payload[12..14].try_into().unwrap()),
|
||||
i16::from_le_bytes(self.payload[14..16].try_into().unwrap()),
|
||||
self.payload[16],
|
||||
self.payload[17] == 1,
|
||||
)
|
||||
sensor_time_milliseconds: u32::from_le_bytes(self.payload[0..4].try_into().unwrap()),
|
||||
distance_millimetres: u32::from_le_bytes(self.payload[4..8].try_into().unwrap()),
|
||||
distance_strength: self.payload[8],
|
||||
distance_precision: self.payload[9],
|
||||
distance_valid: self.payload[10] == 1,
|
||||
flow_velocity_x_centimetres_per_second_at_one_metre: i16::from_le_bytes(
|
||||
self.payload[12..14].try_into().unwrap(),
|
||||
),
|
||||
flow_velocity_y_centimetres_per_second_at_one_metre: i16::from_le_bytes(
|
||||
self.payload[14..16].try_into().unwrap(),
|
||||
),
|
||||
flow_quality: self.payload[16],
|
||||
flow_valid: self.payload[17] == 1,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -254,42 +212,10 @@ impl Default for Mtf01 {
|
||||
}
|
||||
}
|
||||
|
||||
/// Timestamping MTF-01 UART receive service.
|
||||
///
|
||||
/// Feed this service from the selected UART receive interrupt or DMA consumer.
|
||||
/// The board retains ownership of pin muxing, buffering, and interrupt
|
||||
/// acknowledgement.
|
||||
pub struct Mtf01Service<Clock> {
|
||||
clock: 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)]
|
||||
mod tests {
|
||||
use super::{Mtf01, Mtf01Report, Mtf01Service};
|
||||
use kaptan_time::{MonotonicClock, Timestamp};
|
||||
use super::{Mtf01, Mtf01Report};
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
fn frame(payload: [u8; 20]) -> [u8; 27] {
|
||||
let mut frame = [0_u8; 27];
|
||||
@@ -323,18 +249,18 @@ mod tests {
|
||||
|
||||
assert_eq!(
|
||||
report,
|
||||
Some(Mtf01Report::new(
|
||||
Some(Mtf01Report {
|
||||
timestamp,
|
||||
0x1234_5678,
|
||||
1_234,
|
||||
200,
|
||||
7,
|
||||
true,
|
||||
-1_234,
|
||||
1_234,
|
||||
250,
|
||||
true,
|
||||
))
|
||||
sensor_time_milliseconds: 0x1234_5678,
|
||||
distance_millimetres: 1_234,
|
||||
distance_strength: 200,
|
||||
distance_precision: 7,
|
||||
distance_valid: true,
|
||||
flow_velocity_x_centimetres_per_second_at_one_metre: -1_234,
|
||||
flow_velocity_y_centimetres_per_second_at_one_metre: 1_234,
|
||||
flow_quality: 250,
|
||||
flow_valid: true,
|
||||
})
|
||||
);
|
||||
}
|
||||
|
||||
@@ -423,18 +349,18 @@ mod tests {
|
||||
|
||||
#[test]
|
||||
fn exposes_only_valid_generic_observations() {
|
||||
let mut report = Mtf01Report::new(
|
||||
Timestamp::from_microseconds(123),
|
||||
7,
|
||||
2_000,
|
||||
8,
|
||||
9,
|
||||
true,
|
||||
50,
|
||||
-25,
|
||||
12,
|
||||
true,
|
||||
);
|
||||
let mut report = Mtf01Report {
|
||||
timestamp: Timestamp::from_microseconds(123),
|
||||
sensor_time_milliseconds: 7,
|
||||
distance_millimetres: 2_000,
|
||||
distance_strength: 8,
|
||||
distance_precision: 9,
|
||||
distance_valid: true,
|
||||
flow_velocity_x_centimetres_per_second_at_one_metre: 50,
|
||||
flow_velocity_y_centimetres_per_second_at_one_metre: -25,
|
||||
flow_quality: 12,
|
||||
flow_valid: true,
|
||||
};
|
||||
assert_eq!(report.range_snapshot().unwrap().distance_millimetres, 2_000);
|
||||
assert_eq!(
|
||||
report
|
||||
@@ -449,21 +375,15 @@ mod tests {
|
||||
assert_eq!(report.optical_flow_snapshot(), None);
|
||||
}
|
||||
|
||||
struct Clock(u64);
|
||||
|
||||
impl MonotonicClock for Clock {
|
||||
fn now(&mut self) -> Timestamp {
|
||||
self.0 += 1;
|
||||
Timestamp::from_microseconds(self.0)
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn timestamps_a_report_with_its_last_received_byte() {
|
||||
let mut service = Mtf01Service::new(Clock(100));
|
||||
let mut decoder = Mtf01::new();
|
||||
let report = sample_frame()
|
||||
.into_iter()
|
||||
.find_map(|byte| service.on_received_byte(byte))
|
||||
.enumerate()
|
||||
.find_map(|(index, byte)| {
|
||||
decoder.receive_byte(Timestamp::from_microseconds(101 + index as u64), byte)
|
||||
})
|
||||
.unwrap();
|
||||
|
||||
assert_eq!(report.timestamp, Timestamp::from_microseconds(127));
|
||||
|
||||
@@ -18,7 +18,7 @@ pub use protocol::{
|
||||
Content, ControlRequest, ControlResponse, DataRequest, DataResponse, FileFailure, FileMetadata,
|
||||
FileOperation, FilePath, FileRequest, FileResponse, FileStatus, Principal, Target,
|
||||
};
|
||||
pub use service::{FileProvider, FileService, crc32c};
|
||||
pub use service::{DispatchResult, FileProvider, FileService, IngressId, TransferPeer, crc32c};
|
||||
|
||||
/// Bytes transported by one `FILE_DATA` record.
|
||||
pub const DATA_BYTES: usize = 246;
|
||||
|
||||
@@ -36,7 +36,7 @@ impl Target {
|
||||
}
|
||||
}
|
||||
|
||||
/// Exact MAVLink source that owns an open transfer.
|
||||
/// Claimed MAVLink source identity; these wire fields are not authentication.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct Principal {
|
||||
system: u8,
|
||||
@@ -104,9 +104,9 @@ pub enum FileOperation {
|
||||
pub enum FileStatus {
|
||||
/// The operation succeeded.
|
||||
Ok = 1,
|
||||
/// Another principal owns the single transfer slot.
|
||||
/// Another peer owns the single transfer slot.
|
||||
Busy = 2,
|
||||
/// The transfer identity is absent, stale, or owned by another principal.
|
||||
/// The transfer identity is absent, stale, or owned by another peer.
|
||||
InvalidTransfer = 3,
|
||||
/// The operation is not valid for the transfer's current mode or state.
|
||||
InvalidState = 4,
|
||||
|
||||
@@ -6,6 +6,46 @@ use crate::{
|
||||
Principal, Target,
|
||||
};
|
||||
|
||||
/// Caller-assigned ingress identity, supplied out of band rather than on the wire.
|
||||
///
|
||||
/// Distinct receive paths must use distinct, stable IDs while a lease is retained.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct IngressId(pub u8);
|
||||
|
||||
/// Transfer owner, including the receive path of the claimed MAVLink identity.
|
||||
///
|
||||
/// This isolates ingresses; it does not authenticate the principal on an ingress.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct TransferPeer {
|
||||
pub principal: Principal,
|
||||
pub ingress: IngressId,
|
||||
}
|
||||
|
||||
/// Response and the service's authoritative lease-renewal decision.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct DispatchResult {
|
||||
pub response: FileResponse,
|
||||
/// Accepted owner activity or an exact accepted retry, including a cached
|
||||
/// terminal failure. Rejections never renew the lease, regardless of status.
|
||||
pub refresh_lease: bool,
|
||||
}
|
||||
|
||||
impl DispatchResult {
|
||||
fn control(response: ControlResponse, refresh_lease: bool) -> Self {
|
||||
Self {
|
||||
response: FileResponse::Control(response),
|
||||
refresh_lease,
|
||||
}
|
||||
}
|
||||
|
||||
fn data(response: DataResponse) -> Self {
|
||||
Self {
|
||||
response: FileResponse::Data(response),
|
||||
refresh_lease: true,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Format-agnostic owner of the one file currently pinned or staged.
|
||||
///
|
||||
/// The service calls `abort` after any provider failure, explicit abort, lease
|
||||
@@ -69,7 +109,7 @@ struct LastChunk {
|
||||
}
|
||||
|
||||
struct Session {
|
||||
owner: Principal,
|
||||
owner: TransferPeer,
|
||||
transfer_id: u16,
|
||||
path: FilePath,
|
||||
mode: Mode,
|
||||
@@ -81,7 +121,7 @@ struct Session {
|
||||
|
||||
#[derive(Clone, Copy)]
|
||||
struct Completed {
|
||||
owner: Principal,
|
||||
owner: TransferPeer,
|
||||
transfer_id: u16,
|
||||
request: FileRequest,
|
||||
response: ControlResponse,
|
||||
@@ -108,17 +148,19 @@ impl FileService {
|
||||
}
|
||||
}
|
||||
|
||||
/// Handles one already integrity-checked request.
|
||||
/// Handles one already integrity-checked request from an explicit ingress.
|
||||
///
|
||||
/// Untargeted requests are silently discarded. The caller may retry a
|
||||
/// response whose transmission was not admitted: every state-changing
|
||||
/// request has an exact bounded replay path.
|
||||
/// request has an exact bounded replay path. Renew the open/replay lease only
|
||||
/// when `refresh_lease` is true, independently of response delivery. The peer
|
||||
/// must come from the received frame's identity and caller-known ingress.
|
||||
pub fn dispatch<P: FileProvider>(
|
||||
&mut self,
|
||||
provider: &mut P,
|
||||
principal: Principal,
|
||||
peer: TransferPeer,
|
||||
request: FileRequest,
|
||||
) -> Option<FileResponse> {
|
||||
) -> Option<DispatchResult> {
|
||||
let addressed = match request {
|
||||
FileRequest::Control(request) => request.target,
|
||||
FileRequest::Data(request) => request.target,
|
||||
@@ -128,16 +170,15 @@ impl FileService {
|
||||
}
|
||||
Some(match request {
|
||||
FileRequest::Control(request) if request.operation == FileOperation::Read => {
|
||||
self.read(provider, principal, request)
|
||||
self.read(provider, peer, request)
|
||||
}
|
||||
FileRequest::Control(request) => {
|
||||
FileResponse::Control(self.control(provider, principal, request))
|
||||
}
|
||||
FileRequest::Data(request) => self.data(provider, principal, request),
|
||||
FileRequest::Control(request) => self.control(provider, peer, request),
|
||||
FileRequest::Data(request) => self.data(provider, peer, request),
|
||||
})
|
||||
}
|
||||
|
||||
/// Aborts the open transfer, if any, and clears replay state.
|
||||
/// Aborts the open transfer, if any, and clears owner and replay state.
|
||||
/// Returns whether an open provider transfer was aborted.
|
||||
pub fn expire<P: FileProvider>(&mut self, provider: &mut P) -> bool {
|
||||
let expired = matches!(self.state, TransferState::Open(_));
|
||||
if expired {
|
||||
@@ -150,28 +191,32 @@ impl FileService {
|
||||
fn control<P: FileProvider>(
|
||||
&mut self,
|
||||
provider: &mut P,
|
||||
principal: Principal,
|
||||
peer: TransferPeer,
|
||||
request: ControlRequest,
|
||||
) -> ControlResponse {
|
||||
) -> DispatchResult {
|
||||
match request.operation {
|
||||
FileOperation::OpenRead => self.open(provider, principal, request, Mode::Read),
|
||||
FileOperation::OpenWrite => self.open(provider, principal, request, Mode::Write),
|
||||
FileOperation::OpenRead => self.open(provider, peer, request, Mode::Read),
|
||||
FileOperation::OpenWrite => self.open(provider, peer, request, Mode::Write),
|
||||
FileOperation::Read => unreachable!("read requests are dispatched as data responses"),
|
||||
FileOperation::Close => self.close(provider, principal, request),
|
||||
FileOperation::Abort => self.abort(provider, principal, request),
|
||||
FileOperation::WriteAck => {
|
||||
self.failure(principal, request, FileStatus::InvalidState, Content::EMPTY)
|
||||
}
|
||||
FileOperation::Close => self.close(provider, peer, request),
|
||||
FileOperation::Abort => self.abort(provider, peer, request),
|
||||
FileOperation::WriteAck => self.failure(
|
||||
peer.principal,
|
||||
request,
|
||||
FileStatus::InvalidState,
|
||||
Content::EMPTY,
|
||||
),
|
||||
}
|
||||
}
|
||||
|
||||
fn open<P: FileProvider>(
|
||||
&mut self,
|
||||
provider: &mut P,
|
||||
principal: Principal,
|
||||
peer: TransferPeer,
|
||||
request: ControlRequest,
|
||||
mode: Mode,
|
||||
) -> ControlResponse {
|
||||
) -> DispatchResult {
|
||||
let principal = peer.principal;
|
||||
if request.transfer_id == 0 || request.position != 0 {
|
||||
return self.failure(
|
||||
principal,
|
||||
@@ -188,7 +233,7 @@ impl FileService {
|
||||
};
|
||||
|
||||
let replace_open = if let TransferState::Open(session) = &self.state {
|
||||
if session.owner != principal {
|
||||
if session.owner != peer {
|
||||
return self.failure(principal, request, FileStatus::Busy, Content::EMPTY);
|
||||
}
|
||||
let same = session.transfer_id == request.transfer_id
|
||||
@@ -229,7 +274,7 @@ impl FileService {
|
||||
self.state = TransferState::Idle;
|
||||
}
|
||||
if let TransferState::Completed(completed) = &self.state {
|
||||
if completed.owner != principal {
|
||||
if completed.owner != peer {
|
||||
return self.failure(principal, request, FileStatus::Busy, Content::EMPTY);
|
||||
}
|
||||
if completed.transfer_id == request.transfer_id {
|
||||
@@ -258,7 +303,7 @@ impl FileService {
|
||||
}
|
||||
};
|
||||
self.state = TransferState::Open(Session {
|
||||
owner: principal,
|
||||
owner: peer,
|
||||
transfer_id: request.transfer_id,
|
||||
path,
|
||||
mode,
|
||||
@@ -276,36 +321,27 @@ impl FileService {
|
||||
fn read<P: FileProvider>(
|
||||
&mut self,
|
||||
provider: &mut P,
|
||||
principal: Principal,
|
||||
peer: TransferPeer,
|
||||
request: ControlRequest,
|
||||
) -> FileResponse {
|
||||
) -> DispatchResult {
|
||||
let principal = peer.principal;
|
||||
if !canonical_non_open(&request) {
|
||||
return FileResponse::Control(self.failure(
|
||||
principal,
|
||||
request,
|
||||
FileStatus::InvalidState,
|
||||
Content::EMPTY,
|
||||
));
|
||||
return self.failure(principal, request, FileStatus::InvalidState, Content::EMPTY);
|
||||
}
|
||||
let TransferState::Open(session) = &mut self.state else {
|
||||
return FileResponse::Control(self.replay_or_failure(principal, request));
|
||||
return self.replay_or_failure(peer, request);
|
||||
};
|
||||
if session.owner != principal || session.transfer_id != request.transfer_id {
|
||||
return FileResponse::Control(response(
|
||||
if session.owner != peer || session.transfer_id != request.transfer_id {
|
||||
return self.failure(
|
||||
principal,
|
||||
request.operation,
|
||||
request.transfer_id,
|
||||
request.position,
|
||||
FileMetadata {
|
||||
size: request.file_size,
|
||||
checksum_crc32c: request.checksum_crc32c,
|
||||
},
|
||||
request,
|
||||
FileStatus::InvalidTransfer,
|
||||
Content::EMPTY,
|
||||
));
|
||||
);
|
||||
}
|
||||
if session.mode != Mode::Read {
|
||||
return FileResponse::Control(response(
|
||||
return DispatchResult::control(
|
||||
response(
|
||||
principal,
|
||||
request.operation,
|
||||
request.transfer_id,
|
||||
@@ -313,7 +349,9 @@ impl FileService {
|
||||
session.metadata,
|
||||
FileStatus::InvalidState,
|
||||
Content::EMPTY,
|
||||
));
|
||||
),
|
||||
false,
|
||||
);
|
||||
}
|
||||
if request.position != session.position {
|
||||
if let Some(last) = session
|
||||
@@ -321,14 +359,15 @@ impl FileService {
|
||||
.as_ref()
|
||||
.filter(|last| last.position == request.position)
|
||||
{
|
||||
return FileResponse::Data(DataResponse {
|
||||
return DispatchResult::data(DataResponse {
|
||||
target: principal.target(),
|
||||
position: request.position,
|
||||
transfer_id: request.transfer_id,
|
||||
data: last.data,
|
||||
});
|
||||
}
|
||||
return FileResponse::Control(response(
|
||||
return DispatchResult::control(
|
||||
response(
|
||||
principal,
|
||||
request.operation,
|
||||
request.transfer_id,
|
||||
@@ -336,10 +375,13 @@ impl FileService {
|
||||
session.metadata,
|
||||
FileStatus::InvalidPosition,
|
||||
Content::EMPTY,
|
||||
));
|
||||
),
|
||||
false,
|
||||
);
|
||||
}
|
||||
let Some(logical) = chunk_length(session.metadata.size, request.position) else {
|
||||
return FileResponse::Control(response(
|
||||
return DispatchResult::control(
|
||||
response(
|
||||
principal,
|
||||
request.operation,
|
||||
request.transfer_id,
|
||||
@@ -347,10 +389,13 @@ impl FileService {
|
||||
session.metadata,
|
||||
FileStatus::InvalidPosition,
|
||||
Content::EMPTY,
|
||||
));
|
||||
),
|
||||
false,
|
||||
);
|
||||
};
|
||||
if logical == 0 {
|
||||
return FileResponse::Control(response(
|
||||
return DispatchResult::control(
|
||||
response(
|
||||
principal,
|
||||
request.operation,
|
||||
request.transfer_id,
|
||||
@@ -358,7 +403,9 @@ impl FileService {
|
||||
session.metadata,
|
||||
FileStatus::InvalidPosition,
|
||||
Content::EMPTY,
|
||||
));
|
||||
),
|
||||
false,
|
||||
);
|
||||
}
|
||||
let mut data = [0_u8; DATA_BYTES];
|
||||
if let Err(error) = provider.read(request.position, &mut data[..logical]) {
|
||||
@@ -373,15 +420,14 @@ impl FileService {
|
||||
error.status(),
|
||||
error.content(),
|
||||
);
|
||||
self.remember(principal, FileRequest::Control(request), response);
|
||||
return FileResponse::Control(response);
|
||||
return self.remember(peer, FileRequest::Control(request), response);
|
||||
}
|
||||
session.last_chunk = Some(LastChunk {
|
||||
position: request.position,
|
||||
data,
|
||||
});
|
||||
session.position += logical as u32;
|
||||
FileResponse::Data(DataResponse {
|
||||
DispatchResult::data(DataResponse {
|
||||
target: principal.target(),
|
||||
position: request.position,
|
||||
transfer_id: request.transfer_id,
|
||||
@@ -392,66 +438,64 @@ impl FileService {
|
||||
fn data<P: FileProvider>(
|
||||
&mut self,
|
||||
provider: &mut P,
|
||||
principal: Principal,
|
||||
peer: TransferPeer,
|
||||
request: DataRequest,
|
||||
) -> FileResponse {
|
||||
) -> DispatchResult {
|
||||
let principal = peer.principal;
|
||||
let TransferState::Open(session) = &mut self.state else {
|
||||
return FileResponse::Control(self.replay_data_or_failure(principal, request));
|
||||
return self.replay_data_or_failure(peer, request);
|
||||
};
|
||||
if session.owner != principal || session.transfer_id != request.transfer_id {
|
||||
return FileResponse::Control(data_failure(
|
||||
principal,
|
||||
request,
|
||||
FileStatus::InvalidTransfer,
|
||||
));
|
||||
if session.owner != peer || session.transfer_id != request.transfer_id {
|
||||
return DispatchResult::control(
|
||||
data_failure(principal, request, FileStatus::InvalidTransfer),
|
||||
false,
|
||||
);
|
||||
}
|
||||
if session.mode != Mode::Write {
|
||||
return FileResponse::Control(data_failure(
|
||||
principal,
|
||||
request,
|
||||
FileStatus::InvalidState,
|
||||
));
|
||||
return DispatchResult::control(
|
||||
data_failure(principal, request, FileStatus::InvalidState),
|
||||
false,
|
||||
);
|
||||
}
|
||||
let logical = match chunk_length(session.metadata.size, request.position) {
|
||||
Some(length) if length != 0 => length,
|
||||
_ => {
|
||||
return FileResponse::Control(data_failure(
|
||||
principal,
|
||||
request,
|
||||
FileStatus::InvalidPosition,
|
||||
));
|
||||
return DispatchResult::control(
|
||||
data_failure(principal, request, FileStatus::InvalidPosition),
|
||||
false,
|
||||
);
|
||||
}
|
||||
};
|
||||
if request.data[logical..].iter().any(|byte| *byte != 0) {
|
||||
return FileResponse::Control(data_failure(
|
||||
principal,
|
||||
request,
|
||||
FileStatus::RequestConflict,
|
||||
));
|
||||
return DispatchResult::control(
|
||||
data_failure(principal, request, FileStatus::RequestConflict),
|
||||
false,
|
||||
);
|
||||
}
|
||||
if request.position < session.position {
|
||||
let replayed = session
|
||||
.last_chunk
|
||||
.as_ref()
|
||||
.is_some_and(|last| last.position == request.position && last.data == request.data);
|
||||
return FileResponse::Control(if replayed {
|
||||
return DispatchResult::control(
|
||||
if replayed {
|
||||
write_ack(principal, session)
|
||||
} else {
|
||||
data_failure(principal, request, FileStatus::RequestConflict)
|
||||
});
|
||||
},
|
||||
replayed,
|
||||
);
|
||||
}
|
||||
if request.position != session.position {
|
||||
return FileResponse::Control(data_failure(
|
||||
principal,
|
||||
request,
|
||||
FileStatus::InvalidPosition,
|
||||
));
|
||||
return DispatchResult::control(
|
||||
data_failure(principal, request, FileStatus::InvalidPosition),
|
||||
false,
|
||||
);
|
||||
}
|
||||
if let Err(error) = provider.write(request.position, &request.data[..logical]) {
|
||||
provider.abort();
|
||||
let response = data_provider_failure(principal, request, error);
|
||||
self.remember(principal, FileRequest::Data(request), response);
|
||||
return FileResponse::Control(response);
|
||||
return self.remember(peer, FileRequest::Data(request), response);
|
||||
}
|
||||
session.checksum.update(&request.data[..logical]);
|
||||
session.position += logical as u32;
|
||||
@@ -459,22 +503,23 @@ impl FileService {
|
||||
position: request.position,
|
||||
data: request.data,
|
||||
});
|
||||
FileResponse::Control(write_ack(principal, session))
|
||||
DispatchResult::control(write_ack(principal, session), true)
|
||||
}
|
||||
|
||||
fn close<P: FileProvider>(
|
||||
&mut self,
|
||||
provider: &mut P,
|
||||
principal: Principal,
|
||||
peer: TransferPeer,
|
||||
request: ControlRequest,
|
||||
) -> ControlResponse {
|
||||
) -> DispatchResult {
|
||||
let principal = peer.principal;
|
||||
if !canonical_non_open(&request) {
|
||||
return self.failure(principal, request, FileStatus::InvalidState, Content::EMPTY);
|
||||
}
|
||||
let TransferState::Open(session) = &self.state else {
|
||||
return self.replay_or_failure(principal, request);
|
||||
return self.replay_or_failure(peer, request);
|
||||
};
|
||||
if session.owner != principal || session.transfer_id != request.transfer_id {
|
||||
if session.owner != peer || session.transfer_id != request.transfer_id {
|
||||
return self.failure(
|
||||
principal,
|
||||
request,
|
||||
@@ -494,14 +539,19 @@ impl FileService {
|
||||
&& session.checksum.checksum() != session.metadata.checksum_crc32c
|
||||
{
|
||||
provider.abort();
|
||||
let response = self.failure(
|
||||
let response = response(
|
||||
principal,
|
||||
request,
|
||||
request.operation,
|
||||
request.transfer_id,
|
||||
request.position,
|
||||
FileMetadata {
|
||||
size: request.file_size,
|
||||
checksum_crc32c: request.checksum_crc32c,
|
||||
},
|
||||
FileStatus::ChecksumMismatch,
|
||||
Content::EMPTY,
|
||||
);
|
||||
self.remember(principal, FileRequest::Control(request), response);
|
||||
return response;
|
||||
return self.remember(peer, FileRequest::Control(request), response);
|
||||
}
|
||||
let completion = provider.close();
|
||||
let response = match completion {
|
||||
@@ -527,23 +577,23 @@ impl FileService {
|
||||
)
|
||||
}
|
||||
};
|
||||
self.remember(principal, FileRequest::Control(request), response);
|
||||
response
|
||||
self.remember(peer, FileRequest::Control(request), response)
|
||||
}
|
||||
|
||||
fn abort<P: FileProvider>(
|
||||
&mut self,
|
||||
provider: &mut P,
|
||||
principal: Principal,
|
||||
peer: TransferPeer,
|
||||
request: ControlRequest,
|
||||
) -> ControlResponse {
|
||||
) -> DispatchResult {
|
||||
let principal = peer.principal;
|
||||
if !canonical_non_open(&request) {
|
||||
return self.failure(principal, request, FileStatus::InvalidState, Content::EMPTY);
|
||||
}
|
||||
let TransferState::Open(session) = &self.state else {
|
||||
return self.replay_or_failure(principal, request);
|
||||
return self.replay_or_failure(peer, request);
|
||||
};
|
||||
if session.owner != principal || session.transfer_id != request.transfer_id {
|
||||
if session.owner != peer || session.transfer_id != request.transfer_id {
|
||||
return self.failure(
|
||||
principal,
|
||||
request,
|
||||
@@ -562,8 +612,7 @@ impl FileService {
|
||||
FileStatus::Ok,
|
||||
Content::EMPTY,
|
||||
);
|
||||
self.remember(principal, FileRequest::Control(request), response);
|
||||
response
|
||||
self.remember(peer, FileRequest::Control(request), response)
|
||||
}
|
||||
|
||||
fn open_response(
|
||||
@@ -571,7 +620,8 @@ impl FileService {
|
||||
principal: Principal,
|
||||
operation: FileOperation,
|
||||
session: &Session,
|
||||
) -> ControlResponse {
|
||||
) -> DispatchResult {
|
||||
DispatchResult::control(
|
||||
response(
|
||||
principal,
|
||||
operation,
|
||||
@@ -580,6 +630,8 @@ impl FileService {
|
||||
session.metadata,
|
||||
FileStatus::Ok,
|
||||
Content::EMPTY,
|
||||
),
|
||||
true,
|
||||
)
|
||||
}
|
||||
|
||||
@@ -589,7 +641,8 @@ impl FileService {
|
||||
request: ControlRequest,
|
||||
status: FileStatus,
|
||||
content: Content,
|
||||
) -> ControlResponse {
|
||||
) -> DispatchResult {
|
||||
DispatchResult::control(
|
||||
response(
|
||||
principal,
|
||||
request.operation,
|
||||
@@ -601,20 +654,23 @@ impl FileService {
|
||||
},
|
||||
status,
|
||||
content,
|
||||
),
|
||||
false,
|
||||
)
|
||||
}
|
||||
|
||||
fn replay_or_failure(&self, principal: Principal, request: ControlRequest) -> ControlResponse {
|
||||
fn replay_or_failure(&self, peer: TransferPeer, request: ControlRequest) -> DispatchResult {
|
||||
let principal = peer.principal;
|
||||
match &self.state {
|
||||
TransferState::Completed(completed)
|
||||
if completed.owner == principal
|
||||
if completed.owner == peer
|
||||
&& completed.transfer_id == request.transfer_id
|
||||
&& completed.request == FileRequest::Control(request) =>
|
||||
{
|
||||
completed.response
|
||||
DispatchResult::control(completed.response, true)
|
||||
}
|
||||
TransferState::Completed(completed)
|
||||
if completed.owner == principal && completed.transfer_id == request.transfer_id =>
|
||||
if completed.owner == peer && completed.transfer_id == request.transfer_id =>
|
||||
{
|
||||
self.failure(
|
||||
principal,
|
||||
@@ -633,31 +689,39 @@ impl FileService {
|
||||
}
|
||||
}
|
||||
|
||||
fn replay_data_or_failure(
|
||||
&self,
|
||||
principal: Principal,
|
||||
request: DataRequest,
|
||||
) -> ControlResponse {
|
||||
fn replay_data_or_failure(&self, peer: TransferPeer, request: DataRequest) -> DispatchResult {
|
||||
let principal = peer.principal;
|
||||
match &self.state {
|
||||
TransferState::Completed(completed)
|
||||
if completed.owner == principal
|
||||
if completed.owner == peer
|
||||
&& completed.transfer_id == request.transfer_id
|
||||
&& completed.request == FileRequest::Data(request) =>
|
||||
{
|
||||
completed.response
|
||||
DispatchResult::control(completed.response, true)
|
||||
}
|
||||
TransferState::Completed(completed)
|
||||
if completed.owner == principal && completed.transfer_id == request.transfer_id =>
|
||||
if completed.owner == peer && completed.transfer_id == request.transfer_id =>
|
||||
{
|
||||
data_failure(principal, request, FileStatus::RequestConflict)
|
||||
DispatchResult::control(
|
||||
data_failure(principal, request, FileStatus::RequestConflict),
|
||||
false,
|
||||
)
|
||||
}
|
||||
TransferState::Idle | TransferState::Open(_) | TransferState::Completed(_) => {
|
||||
data_failure(principal, request, FileStatus::InvalidTransfer)
|
||||
DispatchResult::control(
|
||||
data_failure(principal, request, FileStatus::InvalidTransfer),
|
||||
false,
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn remember(&mut self, owner: Principal, request: FileRequest, response: ControlResponse) {
|
||||
fn remember(
|
||||
&mut self,
|
||||
owner: TransferPeer,
|
||||
request: FileRequest,
|
||||
response: ControlResponse,
|
||||
) -> DispatchResult {
|
||||
let transfer_id = match request {
|
||||
FileRequest::Control(request) => request.transfer_id,
|
||||
FileRequest::Data(request) => request.transfer_id,
|
||||
@@ -668,6 +732,7 @@ impl FileService {
|
||||
request,
|
||||
response,
|
||||
});
|
||||
DispatchResult::control(response, true)
|
||||
}
|
||||
}
|
||||
|
||||
@@ -753,7 +818,10 @@ mod tests {
|
||||
use super::*;
|
||||
|
||||
const SERVICE: Target = Target::new(1, 1);
|
||||
const CLIENT: Principal = Principal::new(42, 7);
|
||||
const CLIENT: TransferPeer = TransferPeer {
|
||||
principal: Principal::new(42, 7),
|
||||
ingress: IngressId(0),
|
||||
};
|
||||
const PATH: &[u8] = b"/test/data.bin";
|
||||
|
||||
#[derive(Clone, Copy, Eq, PartialEq)]
|
||||
@@ -903,8 +971,12 @@ mod tests {
|
||||
})
|
||||
}
|
||||
|
||||
fn control_response(response: Option<FileResponse>) -> ControlResponse {
|
||||
let Some(FileResponse::Control(response)) = response else {
|
||||
fn control_response(response: Option<DispatchResult>) -> ControlResponse {
|
||||
let Some(DispatchResult {
|
||||
response: FileResponse::Control(response),
|
||||
..
|
||||
}) = response
|
||||
else {
|
||||
panic!("expected control response");
|
||||
};
|
||||
response
|
||||
@@ -960,20 +1032,28 @@ mod tests {
|
||||
assert_eq!(opened.file_size, 300);
|
||||
|
||||
let first = service.dispatch(&mut provider, CLIENT, control(FileOperation::Read, 9, 0));
|
||||
let Some(FileResponse::Data(first)) = first else {
|
||||
let Some(DispatchResult {
|
||||
response: FileResponse::Data(first),
|
||||
refresh_lease: true,
|
||||
}) = first
|
||||
else {
|
||||
panic!("expected data");
|
||||
};
|
||||
assert_eq!(&first.data, &[0x5a; DATA_BYTES]);
|
||||
|
||||
let retry = service.dispatch(&mut provider, CLIENT, control(FileOperation::Read, 9, 0));
|
||||
assert_eq!(retry, Some(FileResponse::Data(first)));
|
||||
assert_eq!(retry, Some(DispatchResult::data(first)));
|
||||
assert_eq!(provider.reads, 1);
|
||||
|
||||
let Some(FileResponse::Data(last)) = service.dispatch(
|
||||
let Some(DispatchResult {
|
||||
response: FileResponse::Data(last),
|
||||
refresh_lease: true,
|
||||
}) = service.dispatch(
|
||||
&mut provider,
|
||||
CLIENT,
|
||||
control(FileOperation::Read, 9, DATA_BYTES as u32),
|
||||
) else {
|
||||
)
|
||||
else {
|
||||
panic!("expected final data");
|
||||
};
|
||||
assert_eq!(&last.data[..54], &source[DATA_BYTES..]);
|
||||
@@ -1001,7 +1081,10 @@ mod tests {
|
||||
);
|
||||
assert!(matches!(
|
||||
service.dispatch(&mut provider, CLIENT, control(FileOperation::Read, 1, 0),),
|
||||
Some(FileResponse::Data(_))
|
||||
Some(DispatchResult {
|
||||
response: FileResponse::Data(_),
|
||||
refresh_lease: true
|
||||
})
|
||||
));
|
||||
assert_eq!(
|
||||
control_response(service.dispatch(
|
||||
@@ -1035,7 +1118,10 @@ mod tests {
|
||||
|
||||
assert!(matches!(
|
||||
service.dispatch(&mut provider, CLIENT, control(FileOperation::Read, 2, 0)),
|
||||
Some(FileResponse::Data(_))
|
||||
Some(DispatchResult {
|
||||
response: FileResponse::Data(_),
|
||||
refresh_lease: true
|
||||
})
|
||||
));
|
||||
assert_eq!(
|
||||
control_response(service.dispatch(
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -10,6 +10,8 @@ workspace = true
|
||||
|
||||
[dependencies]
|
||||
kaptan-config = { path = "../kaptan-config" }
|
||||
kaptan-err-reportable = { path = "../kaptan-err-reportable" }
|
||||
kaptan-log = { path = "../kaptan-log" }
|
||||
minicbor = { version = "0.25", features = ["derive"] }
|
||||
kaptan-control = { path = "../kaptan-control" }
|
||||
kaptan-navigation = { path = "../kaptan-navigation" }
|
||||
|
||||
[dev-dependencies]
|
||||
kaptan-math = { path = "../kaptan-math" }
|
||||
@@ -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)
|
||||
}
|
||||
}
|
||||
@@ -1,17 +1,19 @@
|
||||
//! Vehicle configuration schema and CBOR snapshot codec.
|
||||
//! Validated configuration for the portable flight runtime.
|
||||
|
||||
#![no_std]
|
||||
|
||||
pub mod cbor;
|
||||
pub mod profile;
|
||||
|
||||
pub use cbor::{
|
||||
CborEncodeError, CborError, MAXIMUM_CANDIDATE_BYTES, decode_candidate, encode_candidate,
|
||||
};
|
||||
pub use profile::{CandidateProfile, FORMAT_VERSION, VehicleCapabilities, VehicleProfile};
|
||||
pub use profile::{CandidateProfile, VehicleCapabilities, VehicleProfile};
|
||||
|
||||
/// Generated fingerprint of the complete vehicle configuration schema.
|
||||
pub const SCHEMA_ID: [u8; 16] = <CandidateProfile as kaptan_config::Config>::SCHEMA_ID;
|
||||
|
||||
#[cfg(test)]
|
||||
extern crate std;
|
||||
|
||||
#[cfg(test)]
|
||||
extern crate self as kaptan_flight_config;
|
||||
#[cfg(test)]
|
||||
#[path = "../tests/support/mod.rs"]
|
||||
mod test_support;
|
||||
@@ -1,30 +1,28 @@
|
||||
//! Immutable configuration projections consumed by runtime owners.
|
||||
|
||||
/// Immutable compiled contract for the physical vehicle/board image.
|
||||
///
|
||||
/// A runtime candidate can tune only values inside this contract. It cannot
|
||||
/// create pins, buses, sensor drivers, or actuator channels.
|
||||
use kaptan_config::{Config, ConfigValue, ConfigViolation, FieldId, ValidationRule};
|
||||
use kaptan_navigation::{
|
||||
ContactGeometry, NavigationConfig, NavigationInstallation, NoiseDensities,
|
||||
};
|
||||
|
||||
use super::{
|
||||
ActuatorLimits, CandidateProfile, ControlSettings, PidSettings, SafetySettings, ThrustSettings,
|
||||
TimingSettings,
|
||||
};
|
||||
|
||||
/// Compiled vehicle capabilities.
|
||||
#[derive(Clone, Copy, Debug, PartialEq)]
|
||||
pub struct VehicleCapabilities {
|
||||
/// Number of board-declared logical actuator channels available to the
|
||||
/// selected vehicle model.
|
||||
actuator_count: u8,
|
||||
/// Reviewed physical allocation for this board/airframe image.
|
||||
///
|
||||
/// This is capability data rather than a persisted setting: profile edits
|
||||
/// must never alter motor order, thrust signs, or control authority.
|
||||
/// Fixed allocation rows. Profile edits cannot change motor order or thrust signs.
|
||||
mixer: [[f32; 4]; 4],
|
||||
installation: NavigationInstallation,
|
||||
}
|
||||
|
||||
impl VehicleCapabilities {
|
||||
/// Returns the reviewed four-actuator Quad-X allocation supported by P1.
|
||||
///
|
||||
/// New allocation layouts require a dedicated board-bound airframe review
|
||||
/// and constructor, rather than accepting an arbitrary coefficient array
|
||||
/// through a configuration or capability initializer.
|
||||
pub const fn quad_x() -> Self {
|
||||
/// Four-actuator Quad-X allocation with an explicit navigation installation.
|
||||
pub const fn quad_x(installation: NavigationInstallation) -> Self {
|
||||
Self {
|
||||
actuator_count: 4,
|
||||
installation,
|
||||
mixer: [
|
||||
[-1.0, 1.0, 1.0, 1.0],
|
||||
[-1.0, -1.0, -1.0, 1.0],
|
||||
@@ -33,205 +31,91 @@ impl VehicleCapabilities {
|
||||
],
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) const fn actuator_count(self) -> u8 {
|
||||
self.actuator_count
|
||||
}
|
||||
|
||||
pub(crate) const fn mixer(self) -> [[f32; 4]; 4] {
|
||||
self.mixer
|
||||
}
|
||||
}
|
||||
|
||||
/// Active prearm timing policy.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct SafetySettings {
|
||||
pub(crate) prearm_dwell_microseconds: u64,
|
||||
}
|
||||
|
||||
impl SafetySettings {
|
||||
pub const fn prearm_dwell_microseconds(self) -> u64 {
|
||||
self.prearm_dwell_microseconds
|
||||
}
|
||||
}
|
||||
|
||||
/// Active stream timing policy.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct TimingSettings {
|
||||
pub(crate) maximum_imu_interval_microseconds: u64,
|
||||
pub(crate) imu_freshness_microseconds: u64,
|
||||
pub(crate) local_state_freshness_microseconds: u64,
|
||||
pub(crate) command_link_freshness_microseconds: u64,
|
||||
}
|
||||
|
||||
impl TimingSettings {
|
||||
pub const fn maximum_imu_interval_microseconds(self) -> u64 {
|
||||
self.maximum_imu_interval_microseconds
|
||||
}
|
||||
pub const fn imu_freshness_microseconds(self) -> u64 {
|
||||
self.imu_freshness_microseconds
|
||||
}
|
||||
pub const fn local_state_freshness_microseconds(self) -> u64 {
|
||||
self.local_state_freshness_microseconds
|
||||
}
|
||||
pub const fn command_link_freshness_microseconds(self) -> u64 {
|
||||
self.command_link_freshness_microseconds
|
||||
}
|
||||
}
|
||||
|
||||
/// Active local-aiding uncertainty policy.
|
||||
#[derive(Clone, Copy, Debug, PartialEq)]
|
||||
pub struct AidingSettings {
|
||||
pub(crate) position_variance_m2: f32,
|
||||
pub(crate) velocity_variance_m2_per_second_squared: f32,
|
||||
}
|
||||
|
||||
impl AidingSettings {
|
||||
pub const fn position_variance_m2(self) -> f32 {
|
||||
self.position_variance_m2
|
||||
}
|
||||
pub const fn velocity_variance_m2_per_second_squared(self) -> f32 {
|
||||
self.velocity_variance_m2_per_second_squared
|
||||
}
|
||||
}
|
||||
|
||||
/// Active outer-loop controller policy.
|
||||
#[derive(Clone, Copy, Debug, PartialEq)]
|
||||
pub struct ControlSettings {
|
||||
pub(crate) position_gain_per_second_squared: f32,
|
||||
pub(crate) velocity_gain_per_second: f32,
|
||||
pub(crate) maximum_horizontal_acceleration_mps2: f32,
|
||||
pub(crate) maximum_upward_acceleration_mps2: f32,
|
||||
pub(crate) maximum_downward_acceleration_mps2: f32,
|
||||
}
|
||||
|
||||
impl ControlSettings {
|
||||
pub const fn position_gain_per_second_squared(self) -> f32 {
|
||||
self.position_gain_per_second_squared
|
||||
}
|
||||
pub const fn velocity_gain_per_second(self) -> f32 {
|
||||
self.velocity_gain_per_second
|
||||
}
|
||||
pub const fn maximum_horizontal_acceleration_mps2(self) -> f32 {
|
||||
self.maximum_horizontal_acceleration_mps2
|
||||
}
|
||||
pub const fn maximum_upward_acceleration_mps2(self) -> f32 {
|
||||
self.maximum_upward_acceleration_mps2
|
||||
}
|
||||
pub const fn maximum_downward_acceleration_mps2(self) -> f32 {
|
||||
self.maximum_downward_acceleration_mps2
|
||||
}
|
||||
}
|
||||
|
||||
/// Active physical thrust envelope.
|
||||
#[derive(Clone, Copy, Debug, PartialEq)]
|
||||
pub struct ThrustSettings {
|
||||
pub(crate) mass_kg: f32,
|
||||
pub(crate) maximum_collective_thrust_newtons: f32,
|
||||
pub(crate) maximum_tilt_tangent: f32,
|
||||
}
|
||||
|
||||
impl ThrustSettings {
|
||||
pub const fn mass_kg(self) -> f32 {
|
||||
self.mass_kg
|
||||
}
|
||||
pub const fn maximum_collective_thrust_newtons(self) -> f32 {
|
||||
self.maximum_collective_thrust_newtons
|
||||
}
|
||||
pub const fn maximum_tilt_tangent(self) -> f32 {
|
||||
self.maximum_tilt_tangent
|
||||
}
|
||||
}
|
||||
|
||||
/// Active scalar PID policy.
|
||||
#[derive(Clone, Copy, Debug, PartialEq)]
|
||||
pub struct PidSettings {
|
||||
pub(crate) proportional: f32,
|
||||
pub(crate) integral: f32,
|
||||
pub(crate) derivative: f32,
|
||||
pub(crate) output_limit: f32,
|
||||
pub(crate) integral_limit: f32,
|
||||
}
|
||||
|
||||
impl PidSettings {
|
||||
pub const fn proportional(self) -> f32 {
|
||||
self.proportional
|
||||
}
|
||||
pub const fn integral(self) -> f32 {
|
||||
self.integral
|
||||
}
|
||||
pub const fn derivative(self) -> f32 {
|
||||
self.derivative
|
||||
}
|
||||
pub const fn output_limit(self) -> f32 {
|
||||
self.output_limit
|
||||
}
|
||||
pub const fn integral_limit(self) -> f32 {
|
||||
self.integral_limit
|
||||
}
|
||||
}
|
||||
|
||||
/// Active actuator command bounds.
|
||||
#[derive(Clone, Copy, Debug, PartialEq)]
|
||||
pub struct ActuatorLimits {
|
||||
pub(crate) minimum: f32,
|
||||
pub(crate) maximum: f32,
|
||||
pub(crate) slew_per_second: f32,
|
||||
}
|
||||
|
||||
impl ActuatorLimits {
|
||||
pub const fn minimum(self) -> f32 {
|
||||
self.minimum
|
||||
}
|
||||
pub const fn maximum(self) -> f32 {
|
||||
self.maximum
|
||||
}
|
||||
pub const fn slew_per_second(self) -> f32 {
|
||||
self.slew_per_second
|
||||
}
|
||||
}
|
||||
|
||||
/// Immutable complete configuration selected at boot.
|
||||
///
|
||||
/// An editable candidate must pass validation before it can become active.
|
||||
/// ```compile_fail,E0308
|
||||
/// use kaptan_flight_config::{CandidateProfile, VehicleProfile};
|
||||
/// fn activate(candidate: CandidateProfile) -> VehicleProfile { candidate }
|
||||
/// ```
|
||||
/// Callers cannot replace the validated candidate inside an active profile.
|
||||
/// ```compile_fail,E0616
|
||||
/// use kaptan_flight_config::{CandidateProfile, VehicleProfile};
|
||||
/// fn replace(mut active: VehicleProfile, candidate: CandidateProfile) {
|
||||
/// active.candidate = candidate;
|
||||
/// }
|
||||
/// ```
|
||||
#[derive(Clone, Copy, Debug, PartialEq)]
|
||||
pub struct VehicleProfile {
|
||||
pub(crate) safety: SafetySettings,
|
||||
pub(crate) timing: TimingSettings,
|
||||
pub(crate) aiding: AidingSettings,
|
||||
pub(crate) control: ControlSettings,
|
||||
pub(crate) thrust: ThrustSettings,
|
||||
pub(crate) rate_pid: PidSettings,
|
||||
pub(crate) actuator_limits: ActuatorLimits,
|
||||
pub(crate) mavlink_logging: kaptan_log::LevelFilter,
|
||||
pub(crate) mixer: [[f32; 4]; 4],
|
||||
candidate: CandidateProfile,
|
||||
capabilities: VehicleCapabilities,
|
||||
}
|
||||
|
||||
impl VehicleProfile {
|
||||
pub(super) fn validate(
|
||||
candidate: &CandidateProfile,
|
||||
capabilities: VehicleCapabilities,
|
||||
) -> Result<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 {
|
||||
self.safety
|
||||
self.candidate.safety
|
||||
}
|
||||
pub const fn timing(self) -> TimingSettings {
|
||||
self.timing
|
||||
}
|
||||
pub const fn aiding(self) -> AidingSettings {
|
||||
self.aiding
|
||||
self.candidate.timing
|
||||
}
|
||||
pub const fn control(self) -> ControlSettings {
|
||||
self.control
|
||||
self.candidate.control
|
||||
}
|
||||
pub const fn thrust(self) -> ThrustSettings {
|
||||
self.thrust
|
||||
self.candidate.thrust
|
||||
}
|
||||
pub const fn rate_pid(self) -> PidSettings {
|
||||
self.rate_pid
|
||||
self.candidate.rate_pid
|
||||
}
|
||||
pub const fn actuator_limits(self) -> ActuatorLimits {
|
||||
self.actuator_limits
|
||||
}
|
||||
/// Immutable boot-active policy for the MAVLink diagnostic sink.
|
||||
pub const fn mavlink_logging(self) -> kaptan_log::LevelFilter {
|
||||
self.mavlink_logging
|
||||
self.candidate.actuator_limits
|
||||
}
|
||||
pub const fn mixer(self) -> [[f32; 4]; 4] {
|
||||
self.mixer
|
||||
self.capabilities.mixer
|
||||
}
|
||||
pub const fn flight(self) -> super::FlightSettings {
|
||||
self.candidate.flight
|
||||
}
|
||||
pub const fn contact_geometry(self) -> ContactGeometry {
|
||||
self.capabilities.installation.contact
|
||||
}
|
||||
pub fn navigation(self) -> NavigationConfig {
|
||||
let f = self.candidate.flight;
|
||||
NavigationConfig {
|
||||
noise: NoiseDensities {
|
||||
gyro: f.gyro_psd,
|
||||
acceleration: f.acceleration_psd,
|
||||
gyro_bias: f.gyro_bias_psd,
|
||||
acceleration_bias: f.acceleration_bias_psd,
|
||||
barometric_datum: f.barometric_datum_psd,
|
||||
},
|
||||
maximum_time_uncertainty_microseconds: f.time_error_bound_us as u32,
|
||||
maximum_imu_interval_microseconds: self.timing().maximum_imu_interval_microseconds,
|
||||
imu_freshness_microseconds: self.timing().imu_freshness_microseconds,
|
||||
aiding_freshness_microseconds: self.timing().local_state_freshness_microseconds,
|
||||
lag_microseconds: 100_000,
|
||||
installation: self.capabilities.installation,
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,93 +1,55 @@
|
||||
//! Private vehicle candidate and whole-profile validation boundary.
|
||||
//! Editable vehicle candidate and whole-profile validation boundary.
|
||||
|
||||
use kaptan_config::{
|
||||
Config, ConfigModel, ConfigValue, ConfigViolation, FieldDescriptor, FieldId, FieldWriteError,
|
||||
ValidationRule,
|
||||
};
|
||||
use kaptan_log::LevelFilter;
|
||||
|
||||
use super::{
|
||||
ActuatorLimits, AidingSettings, ControlSettings, PidSettings, SafetySettings, ThrustSettings,
|
||||
TimingSettings, VehicleCapabilities, VehicleProfile,
|
||||
control::{
|
||||
ActuatorLimitsCandidate, AidingCandidate, ControlCandidate, PidCandidate, ThrustCandidate,
|
||||
},
|
||||
mavlink_logging::MavlinkLoggingCandidate,
|
||||
safety::SafetyCandidate,
|
||||
timing::TimingCandidate,
|
||||
ActuatorLimits, ControlSettings, PidSettings, SafetySettings, ThrustSettings, TimingSettings,
|
||||
VehicleCapabilities, VehicleProfile,
|
||||
};
|
||||
|
||||
/// Current persisted CBOR schema for the vehicle candidate.
|
||||
pub const FORMAT_VERSION: u16 = 3;
|
||||
|
||||
/// Complete editable vehicle candidate.
|
||||
///
|
||||
/// Fields are private outside this crate. Local snapshot recovery and MAVLink
|
||||
/// scalar writes both use the same descriptor/mutation and full-validation
|
||||
/// path.
|
||||
#[derive(Clone, Copy, Config)]
|
||||
/// Descriptor writes require complete validation before activation.
|
||||
#[derive(Clone, Copy, Debug, PartialEq, Config)]
|
||||
#[config(post = Self::validate_flight)]
|
||||
pub struct CandidateProfile {
|
||||
pub(super) safety: SafetyCandidate,
|
||||
pub(super) timing: TimingCandidate,
|
||||
pub(super) aiding: AidingCandidate,
|
||||
pub(super) control: ControlCandidate,
|
||||
pub(super) thrust: ThrustCandidate,
|
||||
pub(super) rate_pid: PidCandidate,
|
||||
pub(super) actuator_limits: ActuatorLimitsCandidate,
|
||||
pub(super) mavlink_logging: MavlinkLoggingCandidate,
|
||||
pub safety: SafetySettings,
|
||||
pub timing: TimingSettings,
|
||||
pub control: ControlSettings,
|
||||
pub thrust: ThrustSettings,
|
||||
pub rate_pid: PidSettings,
|
||||
pub actuator_limits: ActuatorLimits,
|
||||
pub flight: super::FlightSettings,
|
||||
}
|
||||
|
||||
impl CandidateProfile {
|
||||
/// Explicit factory candidate used by the vehicle-profile examples.
|
||||
///
|
||||
/// Board firmware supplies its own factory [`Config`] through the board
|
||||
/// composition root; configuration types never synthesize values through
|
||||
/// [`Default`].
|
||||
pub const fn factory() -> Self {
|
||||
Self {
|
||||
safety: SafetyCandidate {
|
||||
prearm_dwell_microseconds: 100_000,
|
||||
},
|
||||
timing: TimingCandidate {
|
||||
maximum_imu_interval_microseconds: 50_000,
|
||||
imu_freshness_microseconds: 50_000,
|
||||
local_state_freshness_microseconds: 150_000,
|
||||
command_link_freshness_microseconds: 500_000,
|
||||
},
|
||||
aiding: AidingCandidate {
|
||||
position_variance_m2: 0.04,
|
||||
velocity_variance_m2_per_second_squared: 0.04,
|
||||
},
|
||||
control: ControlCandidate {
|
||||
position_gain_per_second_squared: 0.8,
|
||||
velocity_gain_per_second: 1.2,
|
||||
maximum_horizontal_acceleration_mps2: 3.0,
|
||||
maximum_upward_acceleration_mps2: 2.0,
|
||||
maximum_downward_acceleration_mps2: 2.0,
|
||||
},
|
||||
thrust: ThrustCandidate {
|
||||
mass_kg: 1.2,
|
||||
maximum_collective_thrust_newtons: 18.0,
|
||||
maximum_tilt_tangent: 0.700_207_53,
|
||||
},
|
||||
rate_pid: PidCandidate {
|
||||
proportional: 0.035,
|
||||
integral: 0.006,
|
||||
derivative: 0.0,
|
||||
output_limit: 0.10,
|
||||
integral_limit: 0.025,
|
||||
},
|
||||
actuator_limits: ActuatorLimitsCandidate {
|
||||
minimum: 0.0,
|
||||
maximum: 1.0,
|
||||
slew_per_second: 8.0,
|
||||
},
|
||||
mavlink_logging: MavlinkLoggingCandidate {
|
||||
level_filter: LevelFilter::Info,
|
||||
},
|
||||
fn validate_flight(&self) -> Result<(), ConfigViolation> {
|
||||
use super::ids;
|
||||
use kaptan_config::ValidationRule;
|
||||
let field = if !(10_000..=20_000).contains(&self.timing.maximum_imu_interval_microseconds) {
|
||||
Some(ids::MAXIMUM_IMU_INTERVAL_MICROSECONDS)
|
||||
} else if self.timing.imu_freshness_microseconds != 20_000 {
|
||||
Some(ids::IMU_FRESHNESS_MICROSECONDS)
|
||||
} else if !(200_000..=500_000).contains(&self.timing.local_state_freshness_microseconds) {
|
||||
Some(ids::LOCAL_STATE_FRESHNESS_MICROSECONDS)
|
||||
} else if self.timing.offboard_timeout_microseconds != 300_000 {
|
||||
Some(ids::OFFBOARD_TIMEOUT_MICROSECONDS)
|
||||
} else if self.actuator_limits.minimum != 0. {
|
||||
Some(ids::ACTUATOR_MINIMUM)
|
||||
}
|
||||
// The 0.5-s landing cap ramp can descend by at most 2 command/s.
|
||||
else if self.actuator_limits.slew_per_second < 2. {
|
||||
Some(ids::ACTUATOR_SLEW_PER_SECOND)
|
||||
} else {
|
||||
None
|
||||
};
|
||||
if let Some(id) = field {
|
||||
return Err(ConfigViolation::field(id, ValidationRule::Inconsistent));
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Returns the complete transport-neutral descriptor registry.
|
||||
pub fn descriptors() -> impl ExactSizeIterator<Item = &'static FieldDescriptor> + Clone {
|
||||
(0..Self::FIELD_COUNT).map(|index| {
|
||||
@@ -95,130 +57,11 @@ impl CandidateProfile {
|
||||
.expect("derived aggregate descriptor count must be exact")
|
||||
})
|
||||
}
|
||||
|
||||
fn from_active(active: &VehicleProfile) -> Self {
|
||||
let safety = active.safety();
|
||||
let timing = active.timing();
|
||||
let aiding = active.aiding();
|
||||
let control = active.control();
|
||||
let thrust = active.thrust();
|
||||
let rate_pid = active.rate_pid();
|
||||
let actuator_limits = active.actuator_limits();
|
||||
let mavlink_logging = active.mavlink_logging();
|
||||
Self {
|
||||
safety: SafetyCandidate {
|
||||
prearm_dwell_microseconds: safety.prearm_dwell_microseconds(),
|
||||
},
|
||||
timing: TimingCandidate {
|
||||
maximum_imu_interval_microseconds: timing.maximum_imu_interval_microseconds(),
|
||||
imu_freshness_microseconds: timing.imu_freshness_microseconds(),
|
||||
local_state_freshness_microseconds: timing.local_state_freshness_microseconds(),
|
||||
command_link_freshness_microseconds: timing.command_link_freshness_microseconds(),
|
||||
},
|
||||
aiding: AidingCandidate {
|
||||
position_variance_m2: aiding.position_variance_m2(),
|
||||
velocity_variance_m2_per_second_squared: aiding
|
||||
.velocity_variance_m2_per_second_squared(),
|
||||
},
|
||||
control: ControlCandidate {
|
||||
position_gain_per_second_squared: control.position_gain_per_second_squared(),
|
||||
velocity_gain_per_second: control.velocity_gain_per_second(),
|
||||
maximum_horizontal_acceleration_mps2: control
|
||||
.maximum_horizontal_acceleration_mps2(),
|
||||
maximum_upward_acceleration_mps2: control.maximum_upward_acceleration_mps2(),
|
||||
maximum_downward_acceleration_mps2: control.maximum_downward_acceleration_mps2(),
|
||||
},
|
||||
thrust: ThrustCandidate {
|
||||
mass_kg: thrust.mass_kg(),
|
||||
maximum_collective_thrust_newtons: thrust.maximum_collective_thrust_newtons(),
|
||||
maximum_tilt_tangent: thrust.maximum_tilt_tangent(),
|
||||
},
|
||||
rate_pid: PidCandidate {
|
||||
proportional: rate_pid.proportional(),
|
||||
integral: rate_pid.integral(),
|
||||
derivative: rate_pid.derivative(),
|
||||
output_limit: rate_pid.output_limit(),
|
||||
integral_limit: rate_pid.integral_limit(),
|
||||
},
|
||||
actuator_limits: ActuatorLimitsCandidate {
|
||||
minimum: actuator_limits.minimum(),
|
||||
maximum: actuator_limits.maximum(),
|
||||
slew_per_second: actuator_limits.slew_per_second(),
|
||||
},
|
||||
mavlink_logging: MavlinkLoggingCandidate {
|
||||
level_filter: mavlink_logging,
|
||||
},
|
||||
}
|
||||
}
|
||||
|
||||
fn validate_complete(
|
||||
&self,
|
||||
capabilities: &VehicleCapabilities,
|
||||
) -> Result<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 {
|
||||
type Active = VehicleProfile;
|
||||
type Context = VehicleCapabilities;
|
||||
@@ -237,7 +80,7 @@ impl ConfigModel for CandidateProfile {
|
||||
}
|
||||
|
||||
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> {
|
||||
@@ -245,7 +88,7 @@ impl ConfigModel for CandidateProfile {
|
||||
}
|
||||
|
||||
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 {
|
||||
use kaptan_config::{ConfigModel, ConfigValue, FieldId, FieldWriteError, ValidationRule};
|
||||
|
||||
use super::{CandidateProfile, LevelFilter, VehicleCapabilities};
|
||||
use super::{CandidateProfile, VehicleCapabilities};
|
||||
use crate::profile::ids;
|
||||
use crate::test_support::{candidate as fixture_candidate, capabilities};
|
||||
|
||||
const CAPABILITIES: VehicleCapabilities = VehicleCapabilities::quad_x();
|
||||
const CAPABILITIES: VehicleCapabilities = capabilities();
|
||||
|
||||
#[test]
|
||||
fn installation_activation() {
|
||||
let mut installation = crate::test_support::installation();
|
||||
installation.calibration = kaptan_navigation::CalibrationRevision::new(29);
|
||||
installation.reference = kaptan_navigation::SensorReferenceId(42);
|
||||
installation.atmosphere.pressure_pascals = 80_000.0;
|
||||
installation.contact.half_span_x_metres = 0.3;
|
||||
let mut candidate = fixture_candidate();
|
||||
let active = candidate
|
||||
.validate(&VehicleCapabilities::quad_x(installation))
|
||||
.unwrap();
|
||||
assert_eq!(active.navigation().installation, installation);
|
||||
assert_eq!(active.contact_geometry(), installation.contact);
|
||||
candidate.thrust.mass_kg = f32::NAN;
|
||||
assert!(candidate.validate(&CAPABILITIES).is_err());
|
||||
assert!(active.thrust().mass_kg.is_finite());
|
||||
installation.range.direction_frd = kaptan_math::Vector3::zero();
|
||||
assert_eq!(
|
||||
fixture_candidate().validate(&VehicleCapabilities::quad_x(installation)),
|
||||
Err(kaptan_config::ConfigViolation::global(
|
||||
ValidationRule::Inconsistent
|
||||
))
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn generated_schema_fingerprint_is_stable() {
|
||||
assert_eq!(
|
||||
crate::SCHEMA_ID,
|
||||
[
|
||||
0xb9, 0x8b, 0xf9, 0xa0, 0x62, 0x3f, 0x6e, 0xef, 0x4e, 0x88, 0xc6, 0xd8, 0x89, 0x4c,
|
||||
0x76, 0x8b,
|
||||
172, 176, 17, 7, 200, 246, 190, 201, 231, 26, 101, 3, 145, 64, 206, 150,
|
||||
]
|
||||
);
|
||||
}
|
||||
@@ -278,12 +146,72 @@ mod tests {
|
||||
|
||||
#[test]
|
||||
fn every_descriptor_has_a_readable_factory_value() {
|
||||
let candidate = CandidateProfile::factory();
|
||||
let candidate = fixture_candidate();
|
||||
for descriptor in CandidateProfile::descriptors() {
|
||||
assert!(candidate.field_value(descriptor.id).is_some());
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn active_field_reads_preserve_validated_bits_after_candidate_edits() {
|
||||
let factory = fixture_candidate();
|
||||
let mut candidate = factory;
|
||||
for descriptor in CandidateProfile::descriptors() {
|
||||
let value = match factory.field_value(descriptor.id).unwrap() {
|
||||
ConfigValue::U64(value) => ConfigValue::U64(match descriptor.id.get() {
|
||||
2 => 19_000,
|
||||
// Fixed engineering cadence and stream policy are still
|
||||
// checked byte-for-byte in the immutable active snapshot.
|
||||
3 | 5 | 33 | 34 => value,
|
||||
_ => value - u64::from(descriptor.id.get()),
|
||||
}),
|
||||
ConfigValue::F32(0.0) => ConfigValue::F32(-0.0),
|
||||
ConfigValue::F32(value) => ConfigValue::F32(value * 0.875),
|
||||
value => panic!("unexpected profile scalar: {value:?}"),
|
||||
};
|
||||
candidate.set_field(descriptor.id, value).unwrap();
|
||||
}
|
||||
let validated = candidate;
|
||||
let active = candidate.validate(&CAPABILITIES).unwrap();
|
||||
for edit_candidate in [false, true] {
|
||||
for descriptor in CandidateProfile::descriptors() {
|
||||
if edit_candidate {
|
||||
candidate
|
||||
.set_field(descriptor.id, factory.field_value(descriptor.id).unwrap())
|
||||
.unwrap();
|
||||
}
|
||||
let expected = validated.field_value(descriptor.id).unwrap();
|
||||
let actual = CandidateProfile::active_field_value(&active, descriptor.id).unwrap();
|
||||
match (actual, expected) {
|
||||
(ConfigValue::F32(actual), ConfigValue::F32(expected)) => {
|
||||
assert_eq!(actual.to_bits(), expected.to_bits(), "{descriptor:?}");
|
||||
}
|
||||
_ => assert_eq!(actual, expected, "{descriptor:?}"),
|
||||
}
|
||||
}
|
||||
}
|
||||
assert_eq!(candidate, factory);
|
||||
assert_eq!(
|
||||
CandidateProfile::active_field_value(&active, FieldId::new(24)),
|
||||
None
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn editing_a_section_copy_cannot_mutate_the_active_profile() {
|
||||
let active = fixture_candidate().validate(&CAPABILITIES).unwrap();
|
||||
let mut thrust = active.thrust();
|
||||
kaptan_config::Config::set_field(&mut thrust, ids::MASS_KG, ConfigValue::F32(100.0))
|
||||
.unwrap();
|
||||
assert_eq!(thrust.mass_kg, 100.0);
|
||||
assert!(kaptan_config::Config::validate(&thrust).is_err());
|
||||
assert_eq!(active.thrust().mass_kg.to_bits(), 1.2_f32.to_bits());
|
||||
assert_eq!(
|
||||
CandidateProfile::active_field_value(&active, ids::MASS_KG),
|
||||
Some(ConfigValue::F32(1.2))
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn aggregated_descriptor_ids_are_unique() {
|
||||
for (index, descriptor) in CandidateProfile::descriptors().enumerate() {
|
||||
@@ -293,11 +221,19 @@ mod tests {
|
||||
.all(|other| other.id != descriptor.id)
|
||||
);
|
||||
}
|
||||
let mut candidate = fixture_candidate();
|
||||
for retired in [FieldId::new(6), FieldId::new(7), FieldId::new(25)] {
|
||||
assert_eq!(candidate.field_value(retired), None);
|
||||
assert_eq!(
|
||||
candidate.set_field(retired, ConfigValue::F32(0.01)),
|
||||
Err(FieldWriteError::UnknownField(retired))
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn cross_field_invalid_edit_is_retained_only_in_the_candidate() {
|
||||
let mut candidate = CandidateProfile::factory();
|
||||
let mut candidate = fixture_candidate();
|
||||
candidate
|
||||
.set_field(
|
||||
ids::MAXIMUM_IMU_INTERVAL_MICROSECONDS,
|
||||
@@ -312,13 +248,42 @@ mod tests {
|
||||
|
||||
#[test]
|
||||
fn local_write_error_retains_exact_field_and_rule() {
|
||||
let mut candidate = CandidateProfile::factory();
|
||||
let mut candidate = fixture_candidate();
|
||||
assert_eq!(
|
||||
candidate.set_field(ids::MASS_KG, ConfigValue::F32(f32::NAN)),
|
||||
Err(FieldWriteError::non_finite(ids::MASS_KG))
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn pid_gain_domain_includes_derivative_and_preserves_rejected_writes() {
|
||||
for key in [
|
||||
"control.rate_pid.proportional",
|
||||
"control.rate_pid.integral",
|
||||
"control.rate_pid.derivative",
|
||||
"control.yaw_rate_pid.proportional",
|
||||
"control.yaw_rate_pid.integral",
|
||||
"control.attitude_roll_pitch_gain_per_s",
|
||||
"control.attitude_yaw_gain_per_s",
|
||||
] {
|
||||
let id = descriptor_id(key);
|
||||
for gain in [0.0, f32::MIN_POSITIVE, 0.001, 0.5, 1.0, 5.0, 10.0] {
|
||||
let mut candidate = fixture_candidate();
|
||||
candidate.set_field(id, ConfigValue::F32(gain)).unwrap();
|
||||
let active = candidate.validate(&CAPABILITIES).unwrap();
|
||||
assert_eq!(
|
||||
CandidateProfile::active_field_value(&active, id),
|
||||
Some(ConfigValue::F32(gain))
|
||||
);
|
||||
for invalid in [-1.0, f32::NAN, f32::INFINITY, 10.01] {
|
||||
let before = candidate;
|
||||
assert!(candidate.set_field(id, ConfigValue::F32(invalid)).is_err());
|
||||
assert_eq!(candidate, before);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn descriptor_bounds_do_not_conflict_with_derived_field_validation() {
|
||||
for descriptor in CandidateProfile::descriptors() {
|
||||
@@ -326,7 +291,7 @@ mod tests {
|
||||
.into_iter()
|
||||
.flatten()
|
||||
{
|
||||
let mut candidate = CandidateProfile::factory();
|
||||
let mut candidate = fixture_candidate();
|
||||
candidate
|
||||
.set_field(descriptor.id, bound)
|
||||
.expect("descriptor bound must pass the descriptor-backed setter");
|
||||
@@ -341,62 +306,4 @@ mod tests {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn mavlink_logging_policy_is_persisted_and_restart_only() {
|
||||
let mut candidate = CandidateProfile::factory();
|
||||
let level_descriptor = CandidateProfile::descriptors()
|
||||
.find(|descriptor| descriptor.display_key == "mavlink.logging.level_filter")
|
||||
.unwrap();
|
||||
assert_eq!(
|
||||
level_descriptor.enum_options,
|
||||
LevelFilter::CONFIG_ENUM_OPTIONS
|
||||
);
|
||||
candidate
|
||||
.set_field(
|
||||
descriptor_id("mavlink.logging.level_filter"),
|
||||
ConfigValue::U8(LevelFilter::Off.wire_value()),
|
||||
)
|
||||
.unwrap();
|
||||
let active = candidate.validate(&CAPABILITIES).unwrap();
|
||||
assert_eq!(active.mavlink_logging(), LevelFilter::Off);
|
||||
candidate
|
||||
.set_field(
|
||||
descriptor_id("mavlink.logging.level_filter"),
|
||||
ConfigValue::U8(LevelFilter::Trace.wire_value()),
|
||||
)
|
||||
.unwrap();
|
||||
assert_eq!(
|
||||
candidate.validate(&CAPABILITIES).unwrap().mavlink_logging(),
|
||||
LevelFilter::Trace
|
||||
);
|
||||
assert!(matches!(
|
||||
candidate.set_field(
|
||||
descriptor_id("mavlink.logging.level_filter"),
|
||||
ConfigValue::U8(6),
|
||||
),
|
||||
Err(FieldWriteError::Violation(issue))
|
||||
if issue.rule() == ValidationRule::InvalidEnumValue
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn every_persisted_log_filter_is_retained_exactly() {
|
||||
for raw in 0..=5 {
|
||||
let mut candidate = CandidateProfile::factory();
|
||||
candidate
|
||||
.set_field(
|
||||
descriptor_id("mavlink.logging.level_filter"),
|
||||
ConfigValue::U8(raw),
|
||||
)
|
||||
.expect("descriptor accepts the stable level filter");
|
||||
assert_eq!(
|
||||
candidate
|
||||
.validate(&CAPABILITIES)
|
||||
.expect("valid complete profile")
|
||||
.mavlink_logging(),
|
||||
LevelFilter::try_from_wire(raw).unwrap()
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -2,28 +2,10 @@ use kaptan_config::{Config, ConfigViolation, ValidationRule};
|
||||
|
||||
use super::ids;
|
||||
|
||||
#[derive(Clone, Copy, Config)]
|
||||
pub(super) struct AidingCandidate {
|
||||
#[config(
|
||||
id = 6,
|
||||
key = "aiding.position_variance_m2",
|
||||
unit = "m2",
|
||||
finite,
|
||||
range(min = 0.000_001, max = 10_000.0)
|
||||
)]
|
||||
pub(super) position_variance_m2: f32,
|
||||
#[config(
|
||||
id = 7,
|
||||
key = "aiding.velocity_variance_m2ps2",
|
||||
unit = "m2/s2",
|
||||
finite,
|
||||
range(min = 0.000_001, max = 10_000.0)
|
||||
)]
|
||||
pub(super) velocity_variance_m2_per_second_squared: f32,
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Config)]
|
||||
pub(super) struct ControlCandidate {
|
||||
/// Outer-loop controller policy.
|
||||
#[derive(Clone, Copy, Debug, PartialEq, Config)]
|
||||
#[config(post = Self::validate_control)]
|
||||
pub struct ControlSettings {
|
||||
#[config(
|
||||
id = 8,
|
||||
key = "control.position_gain_per_s2",
|
||||
@@ -31,7 +13,7 @@ pub(super) struct ControlCandidate {
|
||||
finite,
|
||||
range(min = 0.0, max = 100.0)
|
||||
)]
|
||||
pub(super) position_gain_per_second_squared: f32,
|
||||
pub position_gain_per_second_squared: f32,
|
||||
#[config(
|
||||
id = 9,
|
||||
key = "control.velocity_gain_per_s",
|
||||
@@ -39,7 +21,7 @@ pub(super) struct ControlCandidate {
|
||||
finite,
|
||||
range(min = 0.0, max = 100.0)
|
||||
)]
|
||||
pub(super) velocity_gain_per_second: f32,
|
||||
pub velocity_gain_per_second: f32,
|
||||
#[config(
|
||||
id = 10,
|
||||
key = "control.maximum_horizontal_acceleration_mps2",
|
||||
@@ -47,7 +29,7 @@ pub(super) struct ControlCandidate {
|
||||
finite,
|
||||
range(min = 0.001, max = 100.0)
|
||||
)]
|
||||
pub(super) maximum_horizontal_acceleration_mps2: f32,
|
||||
pub maximum_horizontal_acceleration_mps2: f32,
|
||||
#[config(
|
||||
id = 11,
|
||||
key = "control.maximum_upward_acceleration_mps2",
|
||||
@@ -55,7 +37,7 @@ pub(super) struct ControlCandidate {
|
||||
finite,
|
||||
range(min = 0.001, max = 100.0)
|
||||
)]
|
||||
pub(super) maximum_upward_acceleration_mps2: f32,
|
||||
pub maximum_upward_acceleration_mps2: f32,
|
||||
#[config(
|
||||
id = 12,
|
||||
key = "control.maximum_downward_acceleration_mps2",
|
||||
@@ -63,11 +45,43 @@ pub(super) struct ControlCandidate {
|
||||
finite,
|
||||
range(min = 0.001, max = 100.0)
|
||||
)]
|
||||
pub(super) maximum_downward_acceleration_mps2: f32,
|
||||
pub maximum_downward_acceleration_mps2: f32,
|
||||
#[config(
|
||||
id = 41,
|
||||
key = "control.attitude_roll_pitch_gain_per_s",
|
||||
unit = "1/s",
|
||||
finite,
|
||||
range(min = 0.0, max = 10.0)
|
||||
)]
|
||||
pub attitude_roll_pitch_gain_per_second: f32,
|
||||
#[config(
|
||||
id = 42,
|
||||
key = "control.attitude_yaw_gain_per_s",
|
||||
unit = "1/s",
|
||||
finite,
|
||||
range(min = 0.0, max = 10.0)
|
||||
)]
|
||||
pub attitude_yaw_gain_per_second: f32,
|
||||
}
|
||||
#[derive(Clone, Copy, Config)]
|
||||
|
||||
impl ControlSettings {
|
||||
fn validate_control(&self) -> Result<(), ConfigViolation> {
|
||||
// Positive multicopter thrust requires downward acceleration strictly
|
||||
// below gravity. An inclusive scalar maximum cannot express this rule.
|
||||
if self.maximum_downward_acceleration_mps2 >= kaptan_navigation::GRAVITY_MPS2 {
|
||||
return Err(ConfigViolation::field(
|
||||
ids::MAXIMUM_DOWNWARD_ACCELERATION_MPS2,
|
||||
ValidationRule::Inconsistent,
|
||||
));
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
/// Physical thrust envelope.
|
||||
#[derive(Clone, Copy, Debug, PartialEq, Config)]
|
||||
#[config(post = Self::validate_thrust)]
|
||||
pub(super) struct ThrustCandidate {
|
||||
pub struct ThrustSettings {
|
||||
#[config(
|
||||
id = 13,
|
||||
key = "thrust.mass_kg",
|
||||
@@ -75,7 +89,7 @@ pub(super) struct ThrustCandidate {
|
||||
finite,
|
||||
range(min = 0.01, max = 100.0)
|
||||
)]
|
||||
pub(super) mass_kg: f32,
|
||||
pub mass_kg: f32,
|
||||
#[config(
|
||||
id = 14,
|
||||
key = "thrust.maximum_collective_thrust_n",
|
||||
@@ -83,7 +97,7 @@ pub(super) struct ThrustCandidate {
|
||||
finite,
|
||||
range(min = 0.1, max = 10_000.0)
|
||||
)]
|
||||
pub(super) maximum_collective_thrust_newtons: f32,
|
||||
pub maximum_collective_thrust_newtons: f32,
|
||||
#[config(
|
||||
id = 15,
|
||||
key = "thrust.maximum_tilt_tangent",
|
||||
@@ -91,11 +105,16 @@ pub(super) struct ThrustCandidate {
|
||||
finite,
|
||||
range(min = 0.001, max = 10.0)
|
||||
)]
|
||||
pub(super) maximum_tilt_tangent: f32,
|
||||
pub maximum_tilt_tangent: f32,
|
||||
}
|
||||
impl ThrustCandidate {
|
||||
impl ThrustSettings {
|
||||
fn validate_thrust(&self) -> Result<(), ConfigViolation> {
|
||||
if self.maximum_collective_thrust_newtons <= self.mass_kg * 9.806_65 {
|
||||
if self.maximum_collective_thrust_newtons <= self.mass_kg * kaptan_navigation::GRAVITY_MPS2
|
||||
|| f64::from(self.maximum_collective_thrust_newtons)
|
||||
> f64::from(self.mass_kg)
|
||||
* (f64::from(kaptan_navigation::MAXIMUM_FREE_ACCELERATION_MPS2)
|
||||
- f64::from(kaptan_navigation::GRAVITY_MPS2))
|
||||
{
|
||||
return Err(ConfigViolation::related(
|
||||
ids::MAXIMUM_COLLECTIVE_THRUST_NEWTONS,
|
||||
ids::MASS_KG,
|
||||
@@ -106,9 +125,10 @@ impl ThrustCandidate {
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Config)]
|
||||
/// Roll/pitch and yaw P/I gains with shared derivative and contribution limits.
|
||||
#[derive(Clone, Copy, Debug, PartialEq, Config)]
|
||||
#[config(post = Self::validate_pid)]
|
||||
pub(super) struct PidCandidate {
|
||||
pub struct PidSettings {
|
||||
#[config(
|
||||
id = 16,
|
||||
key = "control.rate_pid.proportional",
|
||||
@@ -116,7 +136,7 @@ pub(super) struct PidCandidate {
|
||||
finite,
|
||||
range(min = 0.0, max = 10.0)
|
||||
)]
|
||||
pub(super) proportional: f32,
|
||||
pub proportional: f32,
|
||||
#[config(
|
||||
id = 17,
|
||||
key = "control.rate_pid.integral",
|
||||
@@ -124,7 +144,7 @@ pub(super) struct PidCandidate {
|
||||
finite,
|
||||
range(min = 0.0, max = 10.0)
|
||||
)]
|
||||
pub(super) integral: f32,
|
||||
pub integral: f32,
|
||||
#[config(
|
||||
id = 18,
|
||||
key = "control.rate_pid.derivative",
|
||||
@@ -132,7 +152,7 @@ pub(super) struct PidCandidate {
|
||||
finite,
|
||||
range(min = 0.0, max = 10.0)
|
||||
)]
|
||||
pub(super) derivative: f32,
|
||||
pub derivative: f32,
|
||||
#[config(
|
||||
id = 19,
|
||||
key = "control.rate_pid.output_limit",
|
||||
@@ -140,7 +160,7 @@ pub(super) struct PidCandidate {
|
||||
finite,
|
||||
range(min = 0.001, max = 1.0)
|
||||
)]
|
||||
pub(super) output_limit: f32,
|
||||
pub output_limit: f32,
|
||||
#[config(
|
||||
id = 20,
|
||||
key = "control.rate_pid.integral_limit",
|
||||
@@ -148,9 +168,25 @@ pub(super) struct PidCandidate {
|
||||
finite,
|
||||
range(min = 0.0, max = 1.0)
|
||||
)]
|
||||
pub(super) integral_limit: f32,
|
||||
pub integral_limit: f32,
|
||||
#[config(
|
||||
id = 43,
|
||||
key = "control.yaw_rate_pid.proportional",
|
||||
unit = "",
|
||||
finite,
|
||||
range(min = 0.0, max = 10.0)
|
||||
)]
|
||||
pub yaw_proportional: f32,
|
||||
#[config(
|
||||
id = 44,
|
||||
key = "control.yaw_rate_pid.integral",
|
||||
unit = "",
|
||||
finite,
|
||||
range(min = 0.0, max = 10.0)
|
||||
)]
|
||||
pub yaw_integral: f32,
|
||||
}
|
||||
impl PidCandidate {
|
||||
impl PidSettings {
|
||||
fn validate_pid(&self) -> Result<(), ConfigViolation> {
|
||||
if self.integral_limit > self.output_limit {
|
||||
return Err(ConfigViolation::related(
|
||||
@@ -163,9 +199,10 @@ impl PidCandidate {
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Config)]
|
||||
/// Actuator command bounds.
|
||||
#[derive(Clone, Copy, Debug, PartialEq, Config)]
|
||||
#[config(post = Self::validate_actuator_limits)]
|
||||
pub(super) struct ActuatorLimitsCandidate {
|
||||
pub struct ActuatorLimits {
|
||||
#[config(
|
||||
id = 21,
|
||||
key = "actuator.minimum",
|
||||
@@ -173,7 +210,7 @@ pub(super) struct ActuatorLimitsCandidate {
|
||||
finite,
|
||||
range(min = 0.0, max = 1.0)
|
||||
)]
|
||||
pub(super) minimum: f32,
|
||||
pub minimum: f32,
|
||||
#[config(
|
||||
id = 22,
|
||||
key = "actuator.maximum",
|
||||
@@ -181,7 +218,7 @@ pub(super) struct ActuatorLimitsCandidate {
|
||||
finite,
|
||||
range(min = 0.0, max = 1.0)
|
||||
)]
|
||||
pub(super) maximum: f32,
|
||||
pub maximum: f32,
|
||||
#[config(
|
||||
id = 23,
|
||||
key = "actuator.slew_per_second",
|
||||
@@ -189,9 +226,9 @@ pub(super) struct ActuatorLimitsCandidate {
|
||||
finite,
|
||||
range(min = 0.001, max = 100.0)
|
||||
)]
|
||||
pub(super) slew_per_second: f32,
|
||||
pub slew_per_second: f32,
|
||||
}
|
||||
impl ActuatorLimitsCandidate {
|
||||
impl ActuatorLimits {
|
||||
fn validate_actuator_limits(&self) -> Result<(), ConfigViolation> {
|
||||
if self.minimum >= self.maximum {
|
||||
return Err(ConfigViolation::related(
|
||||
|
||||
@@ -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,
|
||||
}
|
||||
@@ -1,20 +1,20 @@
|
||||
//! Symbolic projections used by relation checks and focused tests.
|
||||
//!
|
||||
//! These constants never define an ID. Each projects the metadata generated
|
||||
//! from its owning `#[config(id = ...)]` declaration.
|
||||
//! Field IDs derived from their owning `#[config(id = ...)]` declarations.
|
||||
|
||||
use kaptan_config::FieldId;
|
||||
|
||||
use super::{
|
||||
control::{ActuatorLimitsCandidate, PidCandidate, ThrustCandidate},
|
||||
timing::TimingCandidate,
|
||||
};
|
||||
use super::{ActuatorLimits, ControlSettings, PidSettings, ThrustSettings, TimingSettings};
|
||||
|
||||
pub(super) const MAXIMUM_IMU_INTERVAL_MICROSECONDS: FieldId = TimingCandidate::DESCRIPTORS[0].id;
|
||||
pub(super) const IMU_FRESHNESS_MICROSECONDS: FieldId = TimingCandidate::DESCRIPTORS[1].id;
|
||||
pub(super) const MASS_KG: FieldId = ThrustCandidate::DESCRIPTORS[0].id;
|
||||
pub(super) const MAXIMUM_COLLECTIVE_THRUST_NEWTONS: FieldId = ThrustCandidate::DESCRIPTORS[1].id;
|
||||
pub(super) const RATE_PID_OUTPUT_LIMIT: FieldId = PidCandidate::DESCRIPTORS[3].id;
|
||||
pub(super) const RATE_PID_INTEGRAL_LIMIT: FieldId = PidCandidate::DESCRIPTORS[4].id;
|
||||
pub(super) const ACTUATOR_MINIMUM: FieldId = ActuatorLimitsCandidate::DESCRIPTORS[0].id;
|
||||
pub(super) const ACTUATOR_MAXIMUM: FieldId = ActuatorLimitsCandidate::DESCRIPTORS[1].id;
|
||||
// Field IDs 6, 7 and 25 remain reserved.
|
||||
|
||||
pub(super) const MAXIMUM_IMU_INTERVAL_MICROSECONDS: FieldId = TimingSettings::DESCRIPTORS[0].id;
|
||||
pub(super) const IMU_FRESHNESS_MICROSECONDS: FieldId = TimingSettings::DESCRIPTORS[1].id;
|
||||
pub(super) const LOCAL_STATE_FRESHNESS_MICROSECONDS: FieldId = TimingSettings::DESCRIPTORS[2].id;
|
||||
pub(super) const OFFBOARD_TIMEOUT_MICROSECONDS: FieldId = TimingSettings::DESCRIPTORS[3].id;
|
||||
pub(super) const MAXIMUM_DOWNWARD_ACCELERATION_MPS2: FieldId = ControlSettings::DESCRIPTORS[4].id;
|
||||
pub(super) const MASS_KG: FieldId = ThrustSettings::DESCRIPTORS[0].id;
|
||||
pub(super) const MAXIMUM_COLLECTIVE_THRUST_NEWTONS: FieldId = ThrustSettings::DESCRIPTORS[1].id;
|
||||
pub(super) const RATE_PID_OUTPUT_LIMIT: FieldId = PidSettings::DESCRIPTORS[3].id;
|
||||
pub(super) const RATE_PID_INTEGRAL_LIMIT: FieldId = PidSettings::DESCRIPTORS[4].id;
|
||||
pub(super) const ACTUATOR_MINIMUM: FieldId = ActuatorLimits::DESCRIPTORS[0].id;
|
||||
pub(super) const ACTUATOR_MAXIMUM: FieldId = ActuatorLimits::DESCRIPTORS[1].id;
|
||||
pub(super) const ACTUATOR_SLEW_PER_SECOND: FieldId = ActuatorLimits::DESCRIPTORS[2].id;
|
||||
@@ -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,
|
||||
}
|
||||
@@ -3,13 +3,14 @@
|
||||
mod active;
|
||||
mod candidate;
|
||||
mod control;
|
||||
mod flight;
|
||||
mod ids;
|
||||
mod mavlink_logging;
|
||||
mod safety;
|
||||
mod timing;
|
||||
|
||||
pub use active::{
|
||||
ActuatorLimits, AidingSettings, ControlSettings, PidSettings, SafetySettings, ThrustSettings,
|
||||
TimingSettings, VehicleCapabilities, VehicleProfile,
|
||||
};
|
||||
pub use candidate::{CandidateProfile, FORMAT_VERSION};
|
||||
pub use active::{VehicleCapabilities, VehicleProfile};
|
||||
pub use candidate::CandidateProfile;
|
||||
pub use control::{ActuatorLimits, ControlSettings, PidSettings, ThrustSettings};
|
||||
pub use flight::FlightSettings;
|
||||
pub use safety::SafetySettings;
|
||||
pub use timing::TimingSettings;
|
||||
@@ -1,7 +1,8 @@
|
||||
use kaptan_config::Config;
|
||||
|
||||
#[derive(Clone, Copy, Config)]
|
||||
pub(super) struct SafetyCandidate {
|
||||
/// Prearm timing policy, shared by editable candidates and active snapshots.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq, Config)]
|
||||
pub struct SafetySettings {
|
||||
#[config(
|
||||
id = 1,
|
||||
key = "safety.prearm_dwell_us",
|
||||
@@ -9,5 +10,5 @@ pub(super) struct SafetyCandidate {
|
||||
nonzero,
|
||||
range(min = 1, max = 10_000_000)
|
||||
)]
|
||||
pub(super) prearm_dwell_microseconds: u64,
|
||||
pub prearm_dwell_microseconds: u64,
|
||||
}
|
||||
@@ -2,9 +2,10 @@ use kaptan_config::{Config, ConfigViolation, ValidationRule};
|
||||
|
||||
use super::ids;
|
||||
|
||||
#[derive(Clone, Copy, Config)]
|
||||
/// Stream timing policy, shared by editable candidates and active snapshots.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq, Config)]
|
||||
#[config(post = Self::validate_timing)]
|
||||
pub(super) struct TimingCandidate {
|
||||
pub struct TimingSettings {
|
||||
#[config(
|
||||
id = 2,
|
||||
key = "timing.imu_max_interval_us",
|
||||
@@ -12,7 +13,7 @@ pub(super) struct TimingCandidate {
|
||||
nonzero,
|
||||
range(min = 1, max = 1_000_000)
|
||||
)]
|
||||
pub(super) maximum_imu_interval_microseconds: u64,
|
||||
pub maximum_imu_interval_microseconds: u64,
|
||||
#[config(
|
||||
id = 3,
|
||||
key = "timing.imu_freshness_us",
|
||||
@@ -20,7 +21,7 @@ pub(super) struct TimingCandidate {
|
||||
nonzero,
|
||||
range(min = 1, max = 1_000_000)
|
||||
)]
|
||||
pub(super) imu_freshness_microseconds: u64,
|
||||
pub imu_freshness_microseconds: u64,
|
||||
#[config(
|
||||
id = 4,
|
||||
key = "timing.local_state_freshness_us",
|
||||
@@ -28,19 +29,29 @@ pub(super) struct TimingCandidate {
|
||||
nonzero,
|
||||
range(min = 1, max = 10_000_000)
|
||||
)]
|
||||
pub(super) local_state_freshness_microseconds: u64,
|
||||
pub local_state_freshness_microseconds: u64,
|
||||
#[config(
|
||||
id = 5,
|
||||
key = "timing.link_freshness_us",
|
||||
key = "timing.offboard_timeout_us",
|
||||
unit = "us",
|
||||
nonzero,
|
||||
range(min = 1, max = 10_000_000)
|
||||
)]
|
||||
pub(super) command_link_freshness_microseconds: u64,
|
||||
pub offboard_timeout_microseconds: u64,
|
||||
}
|
||||
|
||||
impl TimingCandidate {
|
||||
impl TimingSettings {
|
||||
fn validate_timing(&self) -> Result<(), ConfigViolation> {
|
||||
// The shared primitives retain their numerical upper bound. The flight
|
||||
// candidate separately validates its tighter IMU and control cadences.
|
||||
if self.maximum_imu_interval_microseconds
|
||||
> kaptan_control::MAXIMUM_CONTROL_STEP_MICROSECONDS
|
||||
{
|
||||
return Err(ConfigViolation::field(
|
||||
ids::MAXIMUM_IMU_INTERVAL_MICROSECONDS,
|
||||
ValidationRule::Inconsistent,
|
||||
));
|
||||
}
|
||||
if self.maximum_imu_interval_microseconds > self.imu_freshness_microseconds {
|
||||
return Err(ConfigViolation::related(
|
||||
ids::MAXIMUM_IMU_INTERVAL_MICROSECONDS,
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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())
|
||||
}
|
||||
@@ -9,6 +9,12 @@ version.workspace = true
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
kaptan-config = { path = "../kaptan-config" }
|
||||
kaptan-flight-config = { path = "../kaptan-flight-config" }
|
||||
kaptan-log = { path = "../kaptan-log" }
|
||||
kaptan-control = { path = "../kaptan-control" }
|
||||
kaptan-math = { path = "../kaptan-math" }
|
||||
kaptan-navigation = { path = "../kaptan-navigation" }
|
||||
kaptan-safety = { path = "../kaptan-safety" }
|
||||
kaptan-time = { path = "../kaptan-time" }
|
||||
|
||||
[dev-dependencies]
|
||||
kaptan-config = { path = "../kaptan-config" }
|
||||
@@ -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)
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large.
Load diff
@@ -1,148 +1,353 @@
|
||||
//! Central, transport-neutral flight-runtime composition.
|
||||
//! Deterministic sensor-driven flight endpoint. The caller owns the clock and
|
||||
//! borrows its bounded output sink for each effectful call. Local publication
|
||||
//! never proves remote application. Consumers enforce their own absolute expiry.
|
||||
//!
|
||||
//! This crate owns the one configuration manager and the bounded queue for the
|
||||
//! image's MAVLink diagnostic sink. MAVLink adapters receive mutable borrows
|
||||
//! for bounded dispatch or draining, but cannot own configuration lifecycle or
|
||||
//! link state.
|
||||
|
||||
//! Callbacks must return errors, not panic. If an unwind crosses this boundary,
|
||||
//! discard the endpoint and callback owner. A halted CPU does not inhibit remote
|
||||
//! outputs; independently enforced consumer expiry remains required.
|
||||
#![no_std]
|
||||
#![forbid(unsafe_code)]
|
||||
|
||||
#[cfg(test)]
|
||||
extern crate std;
|
||||
|
||||
use kaptan_config::{ConfigManager, ConfigStore, StartupError};
|
||||
use kaptan_flight_config::{CandidateProfile, VehicleCapabilities};
|
||||
use kaptan_log::{LogRecord, LogSinkQueue};
|
||||
mod controller;
|
||||
mod execution;
|
||||
pub use execution::FlightEndpoint;
|
||||
|
||||
/// Default bounded allocation for the MAVLink diagnostic sink. Error records
|
||||
/// are partitioned from optional detail.
|
||||
type RuntimeMavlinkLogQueue = LogSinkQueue<8, 16>;
|
||||
/// Core-owned time reserved for an admitted landing and its completion.
|
||||
pub const LANDING_BUDGET_US: u64 = 20_000_000;
|
||||
pub use kaptan_navigation::{
|
||||
FlightEstimate, ImuCoverage, NavigationFailure, ObservationAdmission, ObservationRejection,
|
||||
OriginId, SensorRecord,
|
||||
};
|
||||
use kaptan_navigation::{PositionNed, SensorStatus, VelocityNed};
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
/// One boot-active vehicle runtime. It deliberately has no MAVLink dependency.
|
||||
pub struct FlightRuntime<S> {
|
||||
configuration: ConfigManager<CandidateProfile, S>,
|
||||
mavlink_logs: RuntimeMavlinkLogQueue,
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct CommandOwner(pub u64);
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct TransportSessionId(pub u128);
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct RuntimeEpoch {
|
||||
session: TransportSessionId,
|
||||
generation: u64,
|
||||
}
|
||||
|
||||
impl<S> FlightRuntime<S>
|
||||
where
|
||||
S: ConfigStore<CandidateProfile>,
|
||||
{
|
||||
/// 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,
|
||||
})
|
||||
impl RuntimeEpoch {
|
||||
pub const fn from_parts(session: TransportSessionId, generation: u64) -> Self {
|
||||
Self {
|
||||
session,
|
||||
generation,
|
||||
}
|
||||
|
||||
/// The sole central configuration manager.
|
||||
pub const fn configuration(&self) -> &ConfigManager<CandidateProfile, S> {
|
||||
&self.configuration
|
||||
}
|
||||
|
||||
/// Composition-owned admission boundary for one preformatted event.
|
||||
pub fn publish_log_event(&mut self, record: LogRecord) {
|
||||
self.mavlink_logs.publish_record(record);
|
||||
pub const fn session(self) -> TransportSessionId {
|
||||
self.session
|
||||
}
|
||||
|
||||
/// Submits the next record and removes it only after transport admission.
|
||||
pub fn submit_next_mavlink<R>(
|
||||
&mut self,
|
||||
submit: impl FnOnce(&LogRecord) -> (kaptan_log::TransportAdmission, R),
|
||||
) -> Option<R> {
|
||||
self.mavlink_logs.submit_next(submit)
|
||||
pub const fn generation(self) -> u64 {
|
||||
self.generation
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use kaptan_config::{ConfigModel, ConfigStore, ConfigValue};
|
||||
use kaptan_log::TransportAdmission;
|
||||
use std::sync::Mutex;
|
||||
|
||||
use super::FlightRuntime;
|
||||
|
||||
static LOG_TEST: Mutex<()> = Mutex::new(());
|
||||
|
||||
struct Store;
|
||||
impl ConfigStore<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)
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq, Ord, PartialOrd)]
|
||||
pub struct RequestSequence(pub u64);
|
||||
impl RequestSequence {
|
||||
pub const fn checked_next(self) -> Option<Self> {
|
||||
match self.0.checked_add(1) {
|
||||
Some(v) => Some(Self(v)),
|
||||
None => 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,
|
||||
}
|
||||
@@ -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));
|
||||
}
|
||||
@@ -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()));
|
||||
}
|
||||
@@ -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());
|
||||
}
|
||||
}
|
||||
@@ -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)
|
||||
}
|
||||
@@ -7,3 +7,6 @@ version.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
kaptan-time = { path = "../kaptan-time" }
|
||||
@@ -6,6 +6,8 @@
|
||||
|
||||
#![no_std]
|
||||
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
/// Terminal outcome reported by an I²C controller.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub enum I2cCompletion {
|
||||
@@ -17,6 +19,18 @@ pub enum I2cCompletion {
|
||||
TransferFault,
|
||||
}
|
||||
|
||||
/// Completion admission or cancellation failure. Errors never copy read bytes.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub enum CompletionError {
|
||||
NotPending,
|
||||
NotActive,
|
||||
InvalidLength,
|
||||
TransferFault,
|
||||
/// Physical STOP was not observed by the shutdown deadline. The controller
|
||||
/// is halted and cannot accept another transaction during this boot.
|
||||
StopUnproven,
|
||||
}
|
||||
|
||||
/// Owns a non-blocking I²C controller route and its in-flight storage.
|
||||
///
|
||||
/// Each operation sends a STOP on successful completion. Controllers copy a
|
||||
@@ -38,13 +52,25 @@ pub trait I2cBus {
|
||||
/// Releases controller ownership after a hardware ISR acknowledged the
|
||||
/// terminal event and, for a successful read, copies into `read`.
|
||||
///
|
||||
/// This must not poll or wait. `read` must match the length submitted to
|
||||
/// [`Self::start_read`] for the completed read transaction.
|
||||
fn complete_transaction(&mut self, completion: I2cCompletion, read: Option<&mut [u8]>);
|
||||
/// This must not poll or wait. `NotPending` is entirely nonmutating. A
|
||||
/// successful read requires a matching destination length and controller
|
||||
/// proof of the current transaction's STOP, not just a caller notification.
|
||||
/// Old IRQs/completions must not retire or copy out a newer transaction.
|
||||
fn complete_transaction(
|
||||
&mut self,
|
||||
completion: I2cCompletion,
|
||||
read: Option<&mut [u8]>,
|
||||
) -> Result<(), CompletionError>;
|
||||
|
||||
/// Cancels an in-flight transaction after a platform-selected timeout.
|
||||
///
|
||||
/// Implementations must release controller-owned transaction state and
|
||||
/// arrange for any required STOP or peripheral recovery without waiting.
|
||||
fn cancel_transaction(&mut self);
|
||||
/// Requests STOP without waiting. Admission remains closed until STOP is
|
||||
/// proven. This is not permission to reuse or free in-flight storage.
|
||||
fn cancel_transaction(&mut self, now: Timestamp);
|
||||
|
||||
/// Services cancellation deadlines even without interrupts. The caller must
|
||||
/// invoke this periodically with its monotonic boot clock. A bounded STOP
|
||||
/// failure halts admission and returns `StopUnproven`. Storage must outlive
|
||||
/// all hardware access, including owner drop/forget and failed cancellation.
|
||||
fn service(&mut self, now: Timestamp) -> Result<(), CompletionError>;
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
@@ -1,19 +1,18 @@
|
||||
//! Minimal `f32` math shared by Kaptan navigation and control.
|
||||
//!
|
||||
//! This crate deliberately exposes only fixed-size vectors and a unit
|
||||
//! quaternion. Frame markers carry no runtime data and prevent accidental
|
||||
//! body/local-vector interchange at public navigation and control boundaries.
|
||||
//! Fixed-size `f32` vectors and unit quaternions with typed coordinate frames.
|
||||
|
||||
#![no_std]
|
||||
#![forbid(unsafe_code)]
|
||||
|
||||
use core::marker::PhantomData;
|
||||
use kaptan_err_reportable::KaptanError;
|
||||
|
||||
#[cfg(kani)]
|
||||
mod kani_proofs;
|
||||
|
||||
/// Squared-radian domain of the bounded SO(3) exponential and Jacobians.
|
||||
pub const MAXIMUM_ROTATION_VECTOR_NORM_SQUARED: f32 = 4.0;
|
||||
|
||||
/// Failure reported by a checked mathematical operation.
|
||||
///
|
||||
/// The flight path must treat one of these as a rejected input or an estimator
|
||||
/// failure. In particular, no quaternion constructor converts an error into
|
||||
/// the identity rotation.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
|
||||
pub enum MathError {
|
||||
/// An operand or intermediate value was not finite.
|
||||
@@ -22,7 +21,7 @@ pub enum MathError {
|
||||
/// A vector or quaternion did not have a representable non-zero norm.
|
||||
#[kaptan_error(id = 6002)]
|
||||
ZeroNorm,
|
||||
/// A small-angle quaternion was requested outside its reviewed domain.
|
||||
/// A small-angle quaternion was requested outside its supported domain.
|
||||
#[kaptan_error(id = 6003)]
|
||||
RotationIncrementOutOfRange,
|
||||
/// Rotation-matrix axes were not a finite, right-handed orthonormal basis.
|
||||
@@ -36,11 +35,16 @@ pub enum Ned {}
|
||||
/// Aircraft forward-right-down body frame.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub enum Frd {}
|
||||
/// A physical sensor's locally declared coordinate frame.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub enum Sensor {}
|
||||
|
||||
/// Three `f32` components expressed in one explicit frame.
|
||||
///
|
||||
/// Different coordinate frames cannot be added implicitly.
|
||||
/// ```compile_fail,E0308
|
||||
/// use kaptan_math::{Frd, Ned, Vector3};
|
||||
/// let body = Vector3::<Frd>::zero();
|
||||
/// let local = Vector3::<Ned>::zero();
|
||||
/// let _ = body.add(local);
|
||||
/// ```
|
||||
#[derive(Debug, PartialEq)]
|
||||
pub struct Vector3<Frame> {
|
||||
x: f32,
|
||||
@@ -67,20 +71,16 @@ impl<Frame> Vector3<Frame> {
|
||||
}
|
||||
}
|
||||
|
||||
/// Returns the x component.
|
||||
pub const fn x(self) -> f32 {
|
||||
self.x
|
||||
}
|
||||
/// Returns the y component.
|
||||
pub const fn y(self) -> f32 {
|
||||
self.y
|
||||
}
|
||||
/// Returns the z component.
|
||||
pub const fn z(self) -> f32 {
|
||||
self.z
|
||||
}
|
||||
|
||||
/// Returns the zero vector.
|
||||
pub const fn zero() -> Self {
|
||||
Self::new(0.0, 0.0, 0.0)
|
||||
}
|
||||
@@ -124,79 +124,56 @@ impl<Frame> Vector3<Frame> {
|
||||
self.dot(self)
|
||||
}
|
||||
|
||||
/// Euclidean norm using a bounded Newton iteration.
|
||||
/// Euclidean norm, scaled to avoid intermediate overflow and underflow.
|
||||
pub fn norm(self) -> f32 {
|
||||
sqrt(self.norm_squared())
|
||||
if !self.is_finite() {
|
||||
return f32::NAN;
|
||||
}
|
||||
let scale = self.x.abs().max(self.y.abs()).max(self.z.abs());
|
||||
if scale == 0.0 {
|
||||
return 0.0;
|
||||
}
|
||||
let scaled = Self::new(self.x / scale, self.y / scale, self.z / scale);
|
||||
scale * sqrt(scaled.norm_squared())
|
||||
}
|
||||
|
||||
/// Returns a unit vector, rejecting zero and non-finite input.
|
||||
///
|
||||
/// Scaling before normalization prevents an otherwise finite, large vector
|
||||
/// from overflowing while its direction is still perfectly representable.
|
||||
/// Scaling prevents overflow when the direction is representable.
|
||||
pub fn normalized(self) -> Option<Self> {
|
||||
if !self.is_finite() {
|
||||
return None;
|
||||
}
|
||||
let scale = self.x.abs().max(self.y.abs()).max(self.z.abs());
|
||||
if scale <= 1.0e-8 {
|
||||
if scale == 0.0 {
|
||||
return None;
|
||||
}
|
||||
let scaled = self.scale(1.0 / scale);
|
||||
let scaled = Self::new(self.x / scale, self.y / scale, self.z / scale);
|
||||
let norm = sqrt(scaled.norm_squared());
|
||||
// `scale` is a finite non-zero component, so `scaled` contains one
|
||||
// component with magnitude one. Its squared norm is therefore finite
|
||||
// and at least one; keep that proof local rather than carrying an
|
||||
// unreachable fallback after the public input validation above.
|
||||
// component with magnitude one. The squared norm is in [1, 3], so
|
||||
// multiplying components in [-1, 1] by its reciprocal root is finite.
|
||||
debug_assert!(norm.is_finite());
|
||||
debug_assert!(norm > 0.0);
|
||||
let normalized = scaled.scale(1.0 / norm);
|
||||
normalized.is_finite().then_some(normalized)
|
||||
}
|
||||
}
|
||||
|
||||
/// Two `f32` components expressed in one explicit frame.
|
||||
#[derive(Debug, PartialEq)]
|
||||
pub struct Vector2<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()
|
||||
debug_assert!(normalized.is_finite());
|
||||
Some(normalized)
|
||||
}
|
||||
}
|
||||
|
||||
/// A normalized Hamilton rotation from `From` into `To`.
|
||||
///
|
||||
/// Source and destination frames are zero-sized type parameters, so a caller
|
||||
/// cannot apply a valid quaternion to a vector in an unrelated frame. The
|
||||
/// navigation convention is body-to-NED.
|
||||
/// A rotation accepts vectors in its source frame.
|
||||
/// ```compile_fail,E0308
|
||||
/// use kaptan_math::{Frd, Ned, UnitQuaternion, Vector3};
|
||||
/// let body_to_local = UnitQuaternion::<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)]
|
||||
pub struct UnitQuaternion<From, To> {
|
||||
w: f32,
|
||||
@@ -220,16 +197,13 @@ impl<From, To> UnitQuaternion<From, To> {
|
||||
}
|
||||
|
||||
/// Normalizes raw Hamilton components, rejecting non-finite or zero input.
|
||||
///
|
||||
/// Values are scaled before the norm is evaluated, so valid directions
|
||||
/// with very large finite components do not overflow to an identity-like
|
||||
/// result.
|
||||
/// Scaling prevents intermediate norm overflow.
|
||||
pub fn from_components(w: f32, x: f32, y: f32, z: f32) -> Result<Self, MathError> {
|
||||
if !w.is_finite() | !x.is_finite() | !y.is_finite() | !z.is_finite() {
|
||||
return Err(MathError::NonFinite);
|
||||
}
|
||||
let scale = w.abs().max(x.abs()).max(y.abs()).max(z.abs());
|
||||
if scale <= 1.0e-8 {
|
||||
if scale == 0.0 {
|
||||
return Err(MathError::ZeroNorm);
|
||||
}
|
||||
let sw = w / scale;
|
||||
@@ -314,10 +288,8 @@ impl<From, To> UnitQuaternion<From, To> {
|
||||
}
|
||||
|
||||
impl<Frame> UnitQuaternion<Frame, Frame> {
|
||||
/// Builds an incremental same-frame rotation from a rotation vector in
|
||||
/// radians. The flight estimator admits increments no larger than two
|
||||
/// radians before calling this bounded polynomial, avoiding a hidden
|
||||
/// `libm` dependency in the rate path.
|
||||
/// Builds a same-frame rotation using a polynomial valid up to two radians.
|
||||
/// Larger rotation vectors are rejected.
|
||||
pub fn from_small_rotation_vector(rotation: Vector3<Frame>) -> Result<Self, MathError> {
|
||||
if !rotation.is_finite() {
|
||||
return Err(MathError::NonFinite);
|
||||
@@ -326,13 +298,21 @@ impl<Frame> UnitQuaternion<Frame, Frame> {
|
||||
if !rotation_squared.is_finite() {
|
||||
return Err(MathError::NonFinite);
|
||||
}
|
||||
if rotation_squared > 4.0 {
|
||||
if rotation_squared > MAXIMUM_ROTATION_VECTOR_NORM_SQUARED {
|
||||
return Err(MathError::RotationIncrementOutOfRange);
|
||||
}
|
||||
let half_squared = 0.25 * rotation_squared;
|
||||
let sin_over_angle = 0.5 * (1.0 - half_squared / 6.0 + half_squared * half_squared / 120.0);
|
||||
let half_fourth = half_squared * half_squared;
|
||||
let half_sixth = half_fourth * half_squared;
|
||||
let half_eighth = half_fourth * half_fourth;
|
||||
let sin_over_angle = 0.5
|
||||
* (1.0 - half_squared / 6.0 + half_squared * half_squared / 120.0
|
||||
- half_sixth / 5_040.0
|
||||
+ half_eighth / 362_880.0);
|
||||
let cosine = 1.0 - half_squared / 2.0 + half_squared * half_squared / 24.0
|
||||
- half_squared * half_squared * half_squared / 720.0;
|
||||
- half_squared * half_squared * half_squared / 720.0
|
||||
+ half_eighth / 40_320.0
|
||||
- half_eighth * half_squared / 3_628_800.0;
|
||||
// The expression is close to unit length for its stated domain. A
|
||||
// normalization removes accumulated polynomial/rounding error.
|
||||
Self::from_components(
|
||||
@@ -407,10 +387,8 @@ impl UnitQuaternion<Frd, Ned> {
|
||||
|
||||
/// Bounded square root for positive finite `f32` values.
|
||||
///
|
||||
/// A scaled exponent estimate plus four Newton iterations provides bounded
|
||||
/// vector/quaternion normalization precision across the finite `f32` range.
|
||||
/// Negative/non-finite input is rejected as `NaN` rather than silently
|
||||
/// converted.
|
||||
/// Uses an exponent estimate and four Newton iterations. Returns `NaN` for
|
||||
/// negative or non-finite input.
|
||||
pub fn sqrt(value: f32) -> f32 {
|
||||
if !value.is_finite() || value < 0.0 {
|
||||
return f32::NAN;
|
||||
@@ -418,16 +396,11 @@ pub fn sqrt(value: f32) -> f32 {
|
||||
if value == 0.0 {
|
||||
return value;
|
||||
}
|
||||
// The exponent shortcut below assumes a normal input. Scale a
|
||||
// subnormal by an exact power of two, then undo half that exponent after
|
||||
// the same bounded iteration. This keeps the smallest finite positive
|
||||
// `f32` in the reviewed accuracy envelope without a libm dependency.
|
||||
// The exponent estimate requires normal input. Scale subnormals by 2^24
|
||||
// and divide their square root by 2^12.
|
||||
if value < f32::MIN_POSITIVE {
|
||||
return sqrt(value * 16_777_216.0) / 4_096.0;
|
||||
}
|
||||
// A bit-level exponent estimate makes the fixed iteration count accurate
|
||||
// for subnormal through maximum finite inputs. It avoids linking a
|
||||
// platform libm implementation in the embedded rate path.
|
||||
let mut estimate = f32::from_bits((value.to_bits() >> 1) + 0x1fc0_0000);
|
||||
let mut iteration = 0;
|
||||
while iteration < 4 {
|
||||
@@ -439,10 +412,9 @@ pub fn sqrt(value: f32) -> f32 {
|
||||
|
||||
/// Applies the SO(3) right Jacobian to a vector in one frame.
|
||||
///
|
||||
/// The same bounded two-radian increment domain as
|
||||
/// [`UnitQuaternion::from_small_rotation_vector`] avoids a hidden `libm`
|
||||
/// dependency. The coefficients are fixed Taylor polynomials for
|
||||
/// `(1 - cos(theta)) / theta²` and `(theta - sin(theta)) / theta³`.
|
||||
/// Uses the two-radian domain of [`UnitQuaternion::from_small_rotation_vector`]
|
||||
/// and Taylor polynomials for `(1 - cos(theta)) / theta²` and
|
||||
/// `(theta - sin(theta)) / theta³`.
|
||||
pub fn right_jacobian_apply<Frame>(
|
||||
rotation: Vector3<Frame>,
|
||||
vector: Vector3<Frame>,
|
||||
@@ -473,18 +445,21 @@ fn jacobian_apply<Frame>(
|
||||
if !theta_squared.is_finite() {
|
||||
return Err(MathError::NonFinite);
|
||||
}
|
||||
if theta_squared > 4.0 {
|
||||
if theta_squared > MAXIMUM_ROTATION_VECTOR_NORM_SQUARED {
|
||||
return Err(MathError::RotationIncrementOutOfRange);
|
||||
}
|
||||
let theta_fourth = theta_squared * theta_squared;
|
||||
let theta_sixth = theta_fourth * theta_squared;
|
||||
let theta_eighth = theta_fourth * theta_fourth;
|
||||
let theta_tenth = theta_eighth * theta_squared;
|
||||
let first_coefficient = 0.5 - theta_squared / 24.0 + theta_fourth / 720.0
|
||||
- theta_sixth / 40_320.0
|
||||
+ theta_eighth / 3_628_800.0;
|
||||
+ theta_eighth / 3_628_800.0
|
||||
- theta_tenth / 479_001_600.0;
|
||||
let second_coefficient = 1.0 / 6.0 - theta_squared / 120.0 + theta_fourth / 5_040.0
|
||||
- theta_sixth / 362_880.0
|
||||
+ theta_eighth / 39_916_800.0;
|
||||
+ theta_eighth / 39_916_800.0
|
||||
- theta_tenth / 6_227_020_800.0;
|
||||
let cross = rotation.cross(vector);
|
||||
let result = vector
|
||||
.add(cross.scale(first_cross_sign * first_coefficient))
|
||||
@@ -665,12 +640,6 @@ mod tests {
|
||||
] {
|
||||
assert!(!invalid.is_finite());
|
||||
}
|
||||
|
||||
let two = super::Vector2::<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]
|
||||
@@ -750,75 +719,35 @@ mod tests {
|
||||
] {
|
||||
assert!(!vector.is_finite());
|
||||
}
|
||||
for components in [
|
||||
for (w, x, y, z) in [
|
||||
(f32::NAN, 1.0, 0.0, 0.0),
|
||||
(1.0, f32::NAN, 0.0, 0.0),
|
||||
(1.0, 0.0, f32::NAN, 0.0),
|
||||
(1.0, 0.0, 0.0, f32::NAN),
|
||||
(f32::INFINITY, 0.0, 0.0, 0.0),
|
||||
(1.0, f32::INFINITY, 0.0, 0.0),
|
||||
(1.0, 0.0, f32::INFINITY, 0.0),
|
||||
(1.0, 0.0, 0.0, f32::INFINITY),
|
||||
] {
|
||||
assert_eq!(
|
||||
UnitQuaternion::<Frd, Ned>::from_components(
|
||||
components.0,
|
||||
components.1,
|
||||
components.2,
|
||||
components.3,
|
||||
),
|
||||
UnitQuaternion::<Frd, Ned>::from_components(w, x, y, z),
|
||||
Err(MathError::NonFinite)
|
||||
);
|
||||
}
|
||||
assert_eq!(
|
||||
UnitQuaternion::<Frd, Ned>::from_components(f32::INFINITY, 0.0, 0.0, 0.0),
|
||||
Err(MathError::NonFinite)
|
||||
);
|
||||
assert_eq!(
|
||||
UnitQuaternion::<Frd, Ned>::from_components(1.0, f32::INFINITY, 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,
|
||||
},
|
||||
for (w, x, y, z) in [
|
||||
(f32::NAN, 0.0, 0.0, 0.0),
|
||||
(1.0, f32::NAN, 0.0, 0.0),
|
||||
(1.0, 0.0, f32::NAN, 0.0),
|
||||
(1.0, 0.0, 0.0, f32::NAN),
|
||||
(2.0, 0.0, 0.0, 0.0),
|
||||
] {
|
||||
let quaternion = UnitQuaternion::<Frd, Ned> {
|
||||
w,
|
||||
x,
|
||||
y,
|
||||
z,
|
||||
_frames: PhantomData,
|
||||
};
|
||||
assert!(!quaternion.is_normalized());
|
||||
}
|
||||
for axes in [
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -9,14 +9,10 @@ version.workspace = true
|
||||
workspace = true
|
||||
|
||||
[features]
|
||||
# Sensor-domain adapters are physical telemetry only.
|
||||
sensor-telemetry = ["dep:kaptan-sensor"]
|
||||
|
||||
[dependencies]
|
||||
kaptan-err-reportable = { path = "../kaptan-err-reportable" }
|
||||
kaptan-dma = { path = "../kaptan-dma" }
|
||||
kaptan-sensor = { path = "../kaptan-sensor", optional = true }
|
||||
kaptan-tx = { path = "../kaptan-tx" }
|
||||
mavlink-core = { version = "0.18", default-features = false, features = [
|
||||
"embedded",
|
||||
] }
|
||||
|
||||
@@ -5,9 +5,7 @@ fn main() {
|
||||
let definitions = manifest_directory.join("definitions");
|
||||
let dialect = definitions.join("kaptan.xml");
|
||||
println!("cargo::rerun-if-changed={}", definitions.display());
|
||||
// The upstream generator conditionally emits derives for these optional
|
||||
// ecosystem crates. Kaptan deliberately does not expose those incomplete
|
||||
// features, so tell rustc the generated inactive cfgs are intentional.
|
||||
// The generator emits these feature gates, but this crate leaves them disabled.
|
||||
println!("cargo::rustc-check-cfg=cfg(feature, values(\"serde\", \"arbitrary\", \"ts-rs\"))");
|
||||
let output_directory = std::env::var("OUT_DIR").unwrap();
|
||||
mavlink_bindgen::generate(
|
||||
@@ -15,9 +13,7 @@ fn main() {
|
||||
&output_directory,
|
||||
)
|
||||
.unwrap();
|
||||
// Bindgen's index feature-gates every dialect module. Kaptan has exactly
|
||||
// one unconditional dialect, so load its generated file as a normal Rust
|
||||
// module instead of exposing a redundant Cargo feature.
|
||||
// Bypass bindgen's feature-gated index to load the Kaptan dialect unconditionally.
|
||||
std::fs::write(
|
||||
PathBuf::from(output_directory).join("kaptan_module.rs"),
|
||||
"#[allow(non_camel_case_types, non_snake_case)]\n\
|
||||
|
||||
@@ -1,7 +1,6 @@
|
||||
<?xml version="1.0"?>
|
||||
<mavlink>
|
||||
<!-- The root is the only bindgen input. Every current message has one leaf
|
||||
under its interaction contract. exchange/ intentionally has no message. -->
|
||||
<!-- The root is the only generator input. Each message has one private leaf. -->
|
||||
<include>publish/log.xml</include>
|
||||
<include>publish/error.xml</include>
|
||||
<include>publish/heartbeat.xml</include>
|
||||
@@ -10,4 +9,5 @@
|
||||
<include>publish/optical_flow.xml</include>
|
||||
<include>transfer/file_control.xml</include>
|
||||
<include>transfer/file_data.xml</include>
|
||||
<messages/>
|
||||
</mavlink>
|
||||
@@ -4,7 +4,7 @@
|
||||
<dialect>0</dialect>
|
||||
<messages>
|
||||
<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="uint8_t[96]" name="raised_at">Zero-padded compile-time file:line of the reporting point.</field>
|
||||
</message>
|
||||
|
||||
@@ -11,7 +11,7 @@
|
||||
</enums>
|
||||
<messages>
|
||||
<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"/>
|
||||
</message>
|
||||
</messages>
|
||||
|
||||
@@ -4,10 +4,10 @@
|
||||
<dialect>0</dialect>
|
||||
<messages>
|
||||
<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="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>
|
||||
</message>
|
||||
</messages>
|
||||
|
||||
@@ -21,7 +21,7 @@
|
||||
</enums>
|
||||
<messages>
|
||||
<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="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>
|
||||
|
||||
@@ -5,7 +5,7 @@
|
||||
<messages>
|
||||
<message id="51029" name="RANGE">
|
||||
<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>
|
||||
</message>
|
||||
</messages>
|
||||
|
||||
Loaded 100 of 211 files, more files were not shown because too many files have changed in this diff.
Show more
Reference in new issue
Block a user