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

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

Browse Source 
Validate configuration through its owning schemas and keep board
declarations separate from shared F722 resource ownership. Bound USB
transitions, isolate transport failures and retain restart-only activation.

Migrate navigation and flight consumers with the sensor and schema
contracts. Require explicit installation data and synchronous output
authority; board motor outputs remain inhibited.

Validation: assembled-prefix host tests, strict Clippy, platform/USB tests
and both ARM release builds. Final-source coverage and scoped Miri/Kani
evidence are retained separately from hardware qualification.
This commit is contained in:
Onur Özkan committed 2026-11-07 18:12:11 +03:00
1 parent fb1b8af2d1
commit f9a61a086f
211 files changed
+45657 -12740
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

firmware/Cargo.lock
Generated
+23 -37
View File
@@ -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",
firmware/Cargo.toml
+5 -1
View File
@@ -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" }
firmware/README.md
+6 -6
View File
@@ -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.
firmware/boards/geprc-f722-v2/Cargo.toml
+1 -3
View File
@@ -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",
] }
firmware/boards/geprc-f722-v2/src/board.rs
+24 -68
View File
@@ -1,79 +1,35 @@
//! Declarative support for the GEPRC TAKER F722 BLS 60A V2 stack.
//! Declarative support for the GEPRC GEP-F722-HD V2.
#![forbid(unsafe_code)]
use platform_stm32f7::kaptan_ports::{PortCatalog, PortDescriptor};
use platform_stm32f7::{
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,
),
};
}
firmware/boards/kakute-f7-mini-v3/Cargo.toml
+1 -3
View File
@@ -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",
] }
firmware/boards/kakute-f7-mini-v3/src/board.rs
+30 -98
View File
@@ -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 {
accelerometer: AccelerometerRange::PlusMinus4G,
gyroscope: GyroscopeRange::PlusMinus500DegreesPerSecond,
digital_low_pass_filter: DigitalLowPassFilter::Bandwidth42Hz,
sample_rate_divider: 9,
}),
)
.mavlink(Mavlink::on_usart1().baud(115_200))
.vertical_sensor(VerticalSensor::mtf01_on_uart4().baud(115_200))
.status_led(Pd2ActiveLow)
.telemetry_only(),
hardware: F722Hardware::new(
Mpu6000Spi1 {
mount: ImuMount::Yaw270,
config: Mpu6000Config {
accelerometer: AccelerometerRange::PlusMinus4G,
gyroscope: GyroscopeRange::PlusMinus500DegreesPerSecond,
digital_low_pass_filter: DigitalLowPassFilter::Bandwidth42Hz,
sample_rate_divider: 9,
},
},
SerialInventory::new(&Self::DESCRIPTORS),
StatusLed::ActiveLowPd2,
VerticalSensor::None,
MotorPins::Six,
),
};
}
firmware/libs/kaptan-config-derive/src/expand.rs
+49 -5
View File
@@ -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,
firmware/libs/kaptan-config-derive/src/parse.rs
+17 -34
View File
@@ -482,10 +482,12 @@ fn scalar_literal(expression: &Expr, ty: &str) -> Result<ScalarLiteral> {
})?;
Ok(ScalarLiteral::Unsigned(value))
}
"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,
})
}
firmware/libs/kaptan-config-derive/tests/section.rs
+88
View File
@@ -290,3 +290,91 @@ fn recursive_tree_rejects_duplicate_display_keys() {
Err(issue) if issue.rule() == ValidationRule::Inconsistent
));
}
#[derive(Config)]
struct FloatRules {
#[config(id = 1, key = "finite", unit = "", finite)]
finite: f32,
#[config(id = 2, key = "nonzero", unit = "", nonzero)]
nonzero: f64,
#[config(id = 3, key = "lower", unit = "", range(min = 0.0))]
lower: f32,
#[config(id = 4, key = "upper", unit = "", range(max = 1.0))]
upper: f64,
#[config(id = 5, key = "free", unit = "")]
free: f64,
#[config(id = 6, key = "nonzero32", unit = "", nonzero)]
nonzero32: f32,
}
#[test]
fn scalar_rules_preserve_local_repair_and_float_domains() {
let mut values = FloatRules {
finite: f32::NAN,
nonzero: 0.0,
lower: f32::NAN,
upper: f64::NAN,
free: f64::NAN,
nonzero32: 0.0,
};
// Every other field is invalid. A local repair must still succeed.
values
.set_field(FieldId::new(1), ConfigValue::F32(1.0))
.unwrap();
for (id, value, rule) in [
(
1,
ConfigValue::F32(f32::INFINITY),
ValidationRule::NonFinite,
),
(2, ConfigValue::F64(0.0), ValidationRule::Zero),
(2, ConfigValue::F64(-0.0), ValidationRule::Zero),
(3, ConfigValue::F32(f32::NAN), ValidationRule::Unordered),
(4, ConfigValue::F64(f64::NAN), ValidationRule::Unordered),
(6, ConfigValue::F32(0.0), ValidationRule::Zero),
(6, ConfigValue::F32(-0.0), ValidationRule::Zero),
] {
let id = FieldId::new(id);
let before = values.field_value(id);
assert_eq!(
values.set_field(id, value),
Err(FieldWriteError::Violation(ConfigViolation::field(id, rule)))
);
assert_eq!(values.field_value(id), before);
}
// Neither nonzero nor a one-sided range implies finite.
for (id, value) in [
(2, ConfigValue::F64(f64::from_bits(0xfff0_0000_0000_0042))),
(3, ConfigValue::F32(f32::INFINITY)),
(4, ConfigValue::F64(f64::NEG_INFINITY)),
(5, ConfigValue::F64(f64::from_bits(0x7ff8_0000_0000_1234))),
(6, ConfigValue::F32(f32::INFINITY)),
] {
values.set_field(FieldId::new(id), value).unwrap();
assert_eq!(values.field_value(FieldId::new(id)), Some(value));
}
values.validate().unwrap();
}
#[test]
fn rule_metadata_changes_fingerprints() {
let descriptor = FloatRules::DESCRIPTORS[0];
assert!(descriptor.finite);
assert!(!descriptor.nonzero);
assert!(FloatRules::DESCRIPTORS[1].nonzero);
let original = kaptan_config::validation::__schema_id(&[descriptor]);
assert_ne!(
original,
kaptan_config::validation::__schema_id(&[kaptan_config::FieldDescriptor {
finite: false,
..descriptor
}])
);
assert_ne!(
original,
kaptan_config::validation::__schema_id(&[kaptan_config::FieldDescriptor {
nonzero: true,
..descriptor
}])
);
}
firmware/libs/kaptan-config-derive/tests/ui/f32_double_rounded_bounds.rs
+9
View File
@@ -0,0 +1,9 @@
use kaptan_config_derive::Config;
#[derive(Config)]
struct InvertedNativeBounds {
#[config(id = 1, key = "value", unit = "", range(min = 1.0000000596046448, max = 1.0))]
value: f32,
}
fn main() {}
firmware/libs/kaptan-config-derive/tests/ui/f32_double_rounded_bounds.stderr
+5
View File
@@ -0,0 +1,5 @@
error: configuration range minimum exceeds maximum
--> tests/ui/f32_double_rounded_bounds.rs:6:5
|
6 | value: f32,
| ^^^^^
firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_bool.rs
+24
View File
@@ -0,0 +1,24 @@
use kaptan_config_derive::Config;
#[derive(Config)]
struct NonzeroBool {
#[config(id = 1, key = "value", unit = "")]
#[config(nonzero)]
value: bool,
}
#[derive(Config)]
struct RangedBool {
#[config(id = 1, key = "value", unit = "")]
#[config(range(min = 0, max = 1))]
value: bool,
}
#[derive(Config)]
struct FiniteBool {
#[config(id = 1, key = "value", unit = "")]
#[config(finite)]
value: bool,
}
fn main() {}
firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_bool.stderr
+17
View File
@@ -0,0 +1,17 @@
error: nonzero and range require a supported primitive numeric field
--> tests/ui/numeric_rules_on_bool.rs:6:14
|
6 | #[config(nonzero)]
| ^^^^^^^
error: nonzero and range require a supported primitive numeric field
--> tests/ui/numeric_rules_on_bool.rs:13:14
|
13 | #[config(range(min = 0, max = 1))]
| ^^^^^
error: finite requires an f32 or f64 field
--> tests/ui/numeric_rules_on_bool.rs:20:14
|
20 | #[config(finite)]
| ^^^^^^
firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_enum.rs
+31
View File
@@ -0,0 +1,31 @@
use kaptan_config_derive::{Config, ConfigEnum};
#[derive(Clone, Copy, ConfigEnum)]
#[repr(u8)]
enum Mode {
Off = 0,
On = 1,
}
#[derive(Config)]
struct NonzeroEnum {
#[config(enum, id = 1, key = "value", unit = "")]
#[config(nonzero)]
value: Mode,
}
#[derive(Config)]
struct RangedEnum {
#[config(enum, id = 1, key = "value", unit = "")]
#[config(range(min = 0, max = 1))]
value: Mode,
}
#[derive(Config)]
struct FiniteEnum {
#[config(enum, id = 1, key = "value", unit = "")]
#[config(finite)]
value: Mode,
}
fn main() {}
firmware/libs/kaptan-config-derive/tests/ui/numeric_rules_on_enum.stderr
+17
View File
@@ -0,0 +1,17 @@
error: nonzero and range require a supported primitive numeric field
--> tests/ui/numeric_rules_on_enum.rs:13:14
|
13 | #[config(nonzero)]
| ^^^^^^^
error: nonzero and range require a supported primitive numeric field
--> tests/ui/numeric_rules_on_enum.rs:20:14
|
20 | #[config(range(min = 0, max = 1))]
| ^^^^^
error: finite requires an f32 or f64 field
--> tests/ui/numeric_rules_on_enum.rs:27:14
|
27 | #[config(finite)]
| ^^^^^^
firmware/libs/kaptan-config-derive/tests/ui/unsupported_scalar.rs
+15
View File
@@ -0,0 +1,15 @@
use kaptan_config_derive::Config;
#[derive(Config)]
struct UnsupportedScalar {
#[config(id = 1, key = "wide", unit = "")]
wide: u128,
}
#[derive(Config)]
struct UnsupportedCheckedScalar {
#[config(id = 2, key = "signed", unit = "", nonzero)]
signed: i64,
}
fn main() {}
firmware/libs/kaptan-config-derive/tests/ui/unsupported_scalar.stderr
+11
View File
@@ -0,0 +1,11 @@
error: non-primitive configuration fields require config(enum)
--> tests/ui/unsupported_scalar.rs:6:11
|
6 | wide: u128,
| ^^^^
error: nonzero and range require a supported primitive numeric field
--> tests/ui/unsupported_scalar.rs:11:49
|
11 | #[config(id = 2, key = "signed", unit = "", nonzero)]
| ^^^^^^^
firmware/libs/kaptan-config/src/artifact.rs
+270 -96
View File
@@ -2,7 +2,9 @@
//!
//! The artifact is independent of any transport and of the private flash
//! 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(&current, &factory, true, &mut bytes).unwrap();
let artifact = decode_kcfg(encoded).unwrap();
let artifact = decode_kcfg(encoded, &mut [0; KCFG_FIELD_ID_SCRATCH_BYTES]).unwrap();
assert_eq!(encoded[VERSION_OFFSET], 3);
assert_eq!(artifact.schema_id(), Candidate::SCHEMA_ID);
assert!(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));
}
}
firmware/libs/kaptan-config/src/error.rs
+3
View File
@@ -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)
firmware/libs/kaptan-config/src/field.rs
+19 -13
View File
@@ -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.
firmware/libs/kaptan-config/src/journal.rs
+127 -82
View File
@@ -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"
);
}
}
}
firmware/libs/kaptan-config/src/lib.rs
+13 -11
View File
@@ -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;
firmware/libs/kaptan-config/src/lifecycle.rs
+298 -62
View File
@@ -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(_))
));
}
firmware/libs/kaptan-config/src/snapshot.rs
+25 -15
View File
@@ -12,9 +12,7 @@
//! field_id:u16 | value_kind:u8 | value:[value_kind.width]
//! ```
//!
//! Field 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]
firmware/libs/kaptan-config/src/storage.rs
+23 -12
View File
@@ -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>;
}
firmware/libs/kaptan-config/src/validation.rs
+10 -6
View File
@@ -14,7 +14,8 @@ pub trait Config {
/// Exact value bytes in the flattened private snapshot.
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)
firmware/libs/kaptan-config/tests/transactions.rs
+318
View File
@@ -0,0 +1,318 @@
use core::cell::Cell;
use kaptan_config::{
ConfigManager, ConfigModel, ConfigStore, ConfigValue, ConfigViolation, FieldDescriptor,
FieldId, FieldWriteError, KcfgDecodeError, KcfgExporter, KcfgImporter, KcfgUpdate,
ReplaceError, ValidationRule, decode_kcfg, encode_edited_kcfg,
};
#[derive(Clone, Debug, PartialEq, kaptan_config::Config)]
struct Scalars {
#[config(id = 1, key = "test.u8", unit = "")]
u8_value: u8,
#[config(id = 2, key = "test.u16", unit = "")]
u16_value: u16,
#[config(id = 3, key = "test.u32", unit = "")]
u32_value: u32,
#[config(id = 4, key = "test.u64", unit = "")]
u64_value: u64,
#[config(id = 7, key = "test.i32", unit = "")]
i32_value: i32,
#[config(id = 9, key = "test.f32", unit = "")]
f32_value: f32,
#[config(id = 10, key = "test.f64", unit = "")]
f64_value: f64,
#[config(id = 11, key = "test.bool", unit = "")]
bool_value: bool,
#[config(id = 12, key = "test.finite", unit = "", finite)]
finite: f32,
#[config(id = 13, key = "test.nonzero", unit = "", nonzero)]
nonzero: f64,
}
impl ConfigModel for Scalars {
type Active = Self;
type Context = ();
const SCHEMA_ID: [u8; 16] = <Self as kaptan_config::Config>::SCHEMA_ID;
fn descriptor(index: u16) -> Option<&'static FieldDescriptor> {
<Self as kaptan_config::Config>::descriptor(usize::from(index))
}
fn field_count() -> u16 {
<Self as kaptan_config::Config>::FIELD_COUNT as u16
}
fn field_value(&self, id: FieldId) -> Option<ConfigValue> {
<Self as kaptan_config::Config>::field_value(self, id)
}
fn active_field_value(active: &Self::Active, id: FieldId) -> Option<ConfigValue> {
active.field_value(id)
}
fn set_field(&mut self, id: FieldId, value: ConfigValue) -> Result<(), FieldWriteError> {
<Self as kaptan_config::Config>::set_field(self, id, value)
}
fn validate(&self, _: &()) -> Result<Self::Active, ConfigViolation> {
<Self as kaptan_config::Config>::validate(self)?;
Ok(self.clone())
}
}
fn factory() -> Scalars {
Scalars {
u8_value: u8::MAX,
u16_value: u16::MAX,
u32_value: u32::MAX,
u64_value: u64::MAX,
i32_value: i32::MIN,
f32_value: 0.0,
f64_value: 0.0,
bool_value: true,
finite: 1.0,
nonzero: 1.0,
}
}
struct Store<'a>(&'a Cell<usize>);
impl ConfigStore<Scalars> for Store<'_> {
type Error = ();
fn load(&mut self, _: &Scalars) -> Result<Option<Scalars>, ()> {
Ok(None)
}
fn persist(&mut self, _: &Scalars) -> Result<(), ()> {
self.0.set(self.0.get() + 1);
Ok(())
}
}
fn export(current: &Scalars, factory: &Scalars) -> Vec<u8> {
let exporter = KcfgExporter::new(current, factory, false).unwrap();
let mut bytes = vec![0; exporter.length()];
exporter.read(0, &mut bytes).unwrap();
bytes
}
fn edit(bytes: &[u8], candidate: &Scalars) -> Vec<u8> {
let source = decode_kcfg(bytes, &mut [0; kaptan_config::KCFG_FIELD_ID_SCRATCH_BYTES]).unwrap();
let mut edited = vec![0; bytes.len()];
encode_edited_kcfg(&source, false, &mut edited, |field| {
candidate.field_value(field.id()).unwrap()
})
.unwrap();
edited
}
fn import(bytes: &[u8], factory: &Scalars) -> KcfgUpdate<Scalars> {
let mut importer = KcfgImporter::new(factory).unwrap();
for byte in bytes {
importer.push(factory, core::slice::from_ref(byte)).unwrap();
}
importer.finish().unwrap()
}
fn assert_exact(actual: ConfigValue, expected: ConfigValue) {
match (actual, expected) {
(ConfigValue::F32(a), ConfigValue::F32(b)) => assert_eq!(a.to_bits(), b.to_bits()),
(ConfigValue::F64(a), ConfigValue::F64(b)) => assert_eq!(a.to_bits(), b.to_bits()),
_ => assert_eq!(actual, expected),
}
}
#[test]
fn independent_editors_compare_the_downloaded_base_before_even_noop_or_validation() {
let a = factory();
let writes = Cell::new(0);
let mut manager = ConfigManager::recover(a.clone(), (), Store(&writes)).unwrap();
let editor_a = export(manager.committed().unwrap(), &a);
let editor_b = export(manager.committed().unwrap(), &a);
let mut b = a.clone();
b.u64_value -= 1; // Above the exact-integer range of floating-point transports.
b.f32_value = -0.0;
b.f64_value = -0.0;
let update = import(&edit(&editor_b, &b), &a);
assert!(
manager
.replace_if_current(update.candidate, &update.expected_base)
.unwrap()
.restart_required
);
assert_eq!(writes.get(), 1);
// Editing an already-edited draft must not rebase it. Even a requested
// value set exactly equal to current B is stale when its base was A.
let draft = edit(&edit(&editor_a, &b), &b);
let update = import(&draft, &a);
assert_eq!(
manager.replace_if_current(update.candidate, &update.expected_base),
Err(ReplaceError::Conflict)
);
let update = import(&editor_a, &a);
assert_eq!(
manager.replace_if_current(update.candidate, &update.expected_base),
Err(ReplaceError::Conflict)
);
let mut invalid = b.clone();
invalid.finite = f32::NAN;
let violation = ConfigViolation::field(FieldId::new(12), ValidationRule::NonFinite);
assert_eq!(invalid.validate(&()).unwrap_err(), violation);
assert_eq!(
manager.replace_if_current(invalid.clone(), &a),
Err(ReplaceError::Conflict)
);
assert_eq!(
manager.replace_if_current(invalid, &b),
Err(ReplaceError::InvalidCandidate(violation))
);
assert_eq!(writes.get(), 1);
let fresh = import(&export(manager.committed().unwrap(), &a), &a);
manager
.replace_if_current(fresh.candidate, &fresh.expected_base)
.unwrap();
assert_eq!(writes.get(), 1, "same-base no-op does not consume flash");
assert_eq!(manager.active().f32_value.to_bits(), 0.0_f32.to_bits());
assert_eq!(
manager.committed().unwrap().f32_value.to_bits(),
(-0.0_f32).to_bits()
);
assert!(!manager.is_factory(&b));
assert!(!manager.replace_if_current(a, &b).unwrap().restart_required);
assert_eq!(writes.get(), 2);
}
#[test]
fn v3_preserves_all_native_widths_and_signed_zero_in_both_value_sets() {
let factory = factory();
let mut current = factory.clone();
current.f32_value = -0.0;
current.f64_value = -0.0;
let bytes = export(&current, &factory);
assert_eq!(bytes[4], 3);
let artifact =
decode_kcfg(&bytes, &mut [0; kaptan_config::KCFG_FIELD_ID_SCRATCH_BYTES]).unwrap();
assert_eq!(artifact.schema_id(), Scalars::SCHEMA_ID);
for field in artifact.fields() {
assert_eq!(field.finite(), field.id() == FieldId::new(12));
assert_eq!(field.nonzero(), field.id() == FieldId::new(13));
let expected = current.field_value(field.id()).unwrap();
assert_exact(field.value(), expected);
assert_exact(field.expected_base_value(), expected);
}
let update = import(&edit(&bytes, &factory), &factory);
for index in 0..Scalars::field_count() {
let id = Scalars::descriptor(index).unwrap().id;
assert_exact(
update.candidate.field_value(id).unwrap(),
factory.field_value(id).unwrap(),
);
assert_exact(
update.expected_base.field_value(id).unwrap(),
current.field_value(id).unwrap(),
);
}
// 24 header + 4 CRC + 70 prefix + 132 value + 87 key bytes.
assert_eq!(bytes.len(), 317);
let mut importer = KcfgImporter::new(&factory).unwrap();
assert_eq!(
importer.push(&factory, &export(&current, &current)),
Err(KcfgDecodeError::FactoryMismatch(FieldId::new(9)))
);
// Base checks, no-op checks, restart checks, and factory checks must all
// distinguish signed zero even when every other scalar matches.
for id in [FieldId::new(9), FieldId::new(10)] {
let writes = Cell::new(0);
let mut manager = ConfigManager::recover(factory.clone(), (), Store(&writes)).unwrap();
let mut negative_zero = factory.clone();
negative_zero
.set_field(id, current.field_value(id).unwrap())
.unwrap();
assert!(!manager.is_factory(&negative_zero));
assert_eq!(
manager.replace_if_current(factory.clone(), &negative_zero),
Err(ReplaceError::Conflict)
);
assert!(
manager
.replace_if_current(negative_zero.clone(), &factory)
.unwrap()
.restart_required
);
assert_eq!(writes.get(), 1);
assert_eq!(
manager.replace_if_current(negative_zero, &factory),
Err(ReplaceError::Conflict)
);
}
}
#[test]
fn nonfinite_payload_identity_survives_artifacts_cas_noops_and_factory_checks() {
for (single, double) in [
(0x7f80_0000, 0xfff0_0000_0000_0000), // infinities
(0x7fc0_0042, 0xfff8_0000_0000_0042), // quiet payloads
(0xff80_0042, 0x7ff0_0000_0000_0042), // signaling payloads
] {
let mut factory = factory();
factory.f32_value = f32::from_bits(single);
factory.f64_value = f64::from_bits(double);
let writes = Cell::new(0);
let mut manager = ConfigManager::recover(factory.clone(), (), Store(&writes)).unwrap();
let bytes = export(&factory, &factory);
let update = import(&bytes, &factory);
assert!(manager.is_factory(&update.candidate));
assert!(
!manager
.replace_if_current(update.candidate, &update.expected_base)
.unwrap()
.restart_required
);
assert_eq!(writes.get(), 0);
let mut other = factory.clone();
other.f64_value = f64::from_bits(double ^ 1);
assert!(!manager.is_factory(&other));
assert_eq!(
manager.replace_if_current(factory.clone(), &other),
Err(ReplaceError::Conflict)
);
assert!(
manager
.replace_if_current(other.clone(), &factory)
.unwrap()
.restart_required
);
assert_eq!(writes.get(), 1);
let update = import(&export(&other, &factory), &factory);
assert_eq!(
update.candidate.field_value(FieldId::new(10)),
Some(ConfigValue::F64(other.f64_value))
);
}
assert_ne!(ConfigValue::U32(0), ConfigValue::F32(0.0));
}
#[test]
fn streaming_import_checks_rule_flags_and_rejects_the_unshipped_old_format() {
let factory = factory();
let bytes = export(&factory, &factory);
let mut old = bytes.clone();
old[4] = 2;
let mut importer = KcfgImporter::new(&factory).unwrap();
assert_eq!(
importer.push(&factory, &old),
Err(KcfgDecodeError::UnsupportedVersion(2))
);
let mut offset = 24;
for index in 0..Scalars::field_count() {
let field = Scalars::descriptor(index).unwrap();
if field.finite || field.nonzero {
let mut edited = bytes.clone();
edited[offset + 3] ^= if field.finite { 4 } else { 8 };
let mut importer = KcfgImporter::new(&factory).unwrap();
assert_eq!(
importer.push(&factory, &edited),
Err(KcfgDecodeError::DescriptorMismatch(field.id))
);
}
offset += 7 + field.kind.width() * 3 + field.display_key.len() + field.unit.len();
}
}
firmware/libs/kaptan-control/src/kani_proofs.rs
+95
View File
@@ -0,0 +1,95 @@
//! Constructor and one-step inductive PID invariants over all f32 bit patterns.
use super::{MAXIMUM_CONTROL_STEP_SECONDS, Pid, PidConfig, PidError};
fn any_config() -> PidConfig {
PidConfig {
proportional: f32::from_bits(kani::any()),
integral: f32::from_bits(kani::any()),
derivative: f32::from_bits(kani::any()),
output_limit: f32::from_bits(kani::any()),
integral_limit: f32::from_bits(kani::any()),
}
}
fn bits(pid: Pid) -> [u32; 6] {
[
pid.config.proportional.to_bits(),
pid.config.integral.to_bits(),
pid.config.derivative.to_bits(),
pid.config.output_limit.to_bits(),
pid.config.integral_limit.to_bits(),
pid.integral_contribution.to_bits(),
]
}
#[kani::proof]
// The generated equality check compares up to 24 bytes through memcmp.
#[kani::unwind(25)]
fn pid_constructor_establishes_invariant() {
let config = any_config();
let result = Pid::new(config);
let valid_gains = [config.proportional, config.integral, config.derivative]
.into_iter()
.all(|gain| gain.is_finite() && gain >= 0.0);
let valid_limits = config.output_limit.is_finite()
&& config.output_limit > 0.0
&& config.integral_limit.is_finite()
&& config.integral_limit >= 0.0;
assert_eq!(result.is_ok(), valid_gains && valid_limits);
if let Ok(pid) = result {
assert_eq!(pid.integral_contribution.to_bits(), 0.0_f32.to_bits());
// Replace the returned configuration with the original input to check
// exact preservation, including the sign of zero-valued gains.
assert_eq!(bits(pid)[..5], bits(Pid { config, ..pid })[..5]);
}
kani::cover!(result.is_ok());
kani::cover!(result.is_err());
kani::cover!(config.proportional.is_nan());
}
#[kani::proof]
#[kani::unwind(25)]
fn pid_update_preserves_bounds_or_state() {
let Ok(mut pid) = Pid::new(any_config()) else {
return;
};
// The constructor establishes zero. This step permits every finite retained
// integral within its limit, a superset of all reachable PID histories.
let initial = f32::from_bits(kani::any());
kani::assume(initial.is_finite());
kani::assume(initial.abs() <= pid.config.integral_limit);
pid.integral_contribution = initial;
let before = bits(pid);
let error = f32::from_bits(kani::any());
let error_rate = f32::from_bits(kani::any());
let dt_seconds = f32::from_bits(kani::any());
let result = pid.update(error, error_rate, dt_seconds);
if let Ok(output) = result {
assert!(output.is_finite());
assert!(output.abs() <= pid.config.output_limit);
assert!(pid.integral_contribution.is_finite());
assert!(pid.integral_contribution.abs() <= pid.config.integral_limit);
assert_eq!(bits(pid)[..5], before[..5]);
} else {
assert_eq!(bits(pid), before);
}
kani::cover!(result.is_ok() && pid.integral_contribution != initial);
kani::cover!(result.is_ok_and(|output| output.abs() == pid.config.output_limit));
kani::cover!(
result == Err(PidError::NonFinite)
&& error.is_finite()
&& error_rate.is_finite()
&& dt_seconds.is_finite()
&& dt_seconds > 0.0
&& dt_seconds <= MAXIMUM_CONTROL_STEP_SECONDS
);
pid.reset();
assert_eq!(pid.integral_contribution.to_bits(), 0.0_f32.to_bits());
assert_eq!(bits(pid)[..5], before[..5]);
}
firmware/libs/kaptan-control/src/lib.rs
+932 -190
View File
File diff suppressed because it is too large. Load diff
firmware/libs/kaptan-data-channel/Cargo.toml
+4
View File
@@ -9,6 +9,7 @@ version.workspace = true
workspace = true
[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"] }
firmware/libs/kaptan-data-channel/src/lib.rs
+90 -139
View File
@@ -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 {
class: FrameClass::BestEffort,
frame: self.encoder.imu_snapshot(&sample),
})
let sample = samples.imu.filter(|sample| {
self.last_imu_timestamp
.is_none_or(|last| sample.timestamp > last)
})?;
self.last_imu_timestamp = Some(sample.timestamp);
Some(PendingFrame {
class: FrameClass::BestEffort,
frame: self.encoder.imu_snapshot(&sample),
})
}
Some(DueData::Distance) if self.range_age_ticks <= self.maximum_vertical_age_ticks => {
self.last_range.map(|range| PendingFrame {
Some(DueData::Distance) => samples
.range
.filter(|range| self.vertical_is_fresh(range.timestamp, now))
.map(|range| PendingFrame {
class: FrameClass::BestEffort,
frame: self.encoder.range_snapshot(&range),
})
}
Some(DueData::OpticalFlow)
if self.optical_flow_age_ticks <= self.maximum_vertical_age_ticks =>
{
self.last_optical_flow.map(|flow| PendingFrame {
}),
Some(DueData::OpticalFlow) => samples
.optical_flow
.filter(|flow| self.vertical_is_fresh(flow.timestamp, now))
.map(|flow| PendingFrame {
class: FrameClass::BestEffort,
frame: self.encoder.optical_flow_snapshot(&flow),
})
}
}),
Some(DueData::Heartbeat) => Some(PendingFrame {
class: FrameClass::Vital,
frame: self.encoder.heartbeat(),
}),
Some(DueData::Distance | DueData::OpticalFlow) | None => None,
None => None,
}
}
@@ -149,23 +179,10 @@ impl<'a, const SERVICE_HZ: usize> DataChannel<'a, SERVICE_HZ> {
}
}
fn refresh_vertical_data(&mut self) {
if let Some(range) = self.sources.range.take_newer(&mut self.observed_range) {
self.last_range = Some(range);
self.range_age_ticks = 0;
} else {
self.range_age_ticks = self.range_age_ticks.saturating_add(1);
}
if let Some(flow) = self
.sources
.optical_flow
.take_newer(&mut self.observed_optical_flow)
{
self.last_optical_flow = Some(flow);
self.optical_flow_age_ticks = 0;
} else {
self.optical_flow_age_ticks = self.optical_flow_age_ticks.saturating_add(1);
}
fn vertical_is_fresh(&self, timestamp: Timestamp, now: Timestamp) -> bool {
now.as_microseconds()
.checked_sub(timestamp.as_microseconds())
.is_some_and(|age| age <= self.maximum_vertical_age_microseconds)
}
}
@@ -245,70 +262,4 @@ impl<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;
firmware/libs/kaptan-data-channel/src/mailbox.rs
+185 -192
View File
@@ -1,227 +1,220 @@
//! Lock-free single-producer, single-consumer latest-value handoff.
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);
});
}
}
firmware/libs/kaptan-data-channel/src/tests.rs
+442
View File
@@ -0,0 +1,442 @@
use super::{
DataChannel, DataChannelError, DataChannelRates, DataCollector, DataSources, LatestImuMailbox,
LatestOpticalFlowMailbox, LatestRangeMailbox, Producer, TelemetrySamples,
};
use kaptan_mavlink::{
MavlinkFrame,
generated::kaptan::{HEARTBEAT_DATA, HeartbeatState, MavMessage},
receiver::IntegrityMavlinkReceiver,
};
use kaptan_sensor::{imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot};
use kaptan_time::Timestamp;
use kaptan_tx::{EnqueueResult, FrameClass, PendingFrame, TransmitQueue};
const RATES: DataChannelRates = DataChannelRates {
imu_hz: 20,
range_hz: Some(20),
optical_flow_hz: Some(20),
heartbeat_hz: 2,
};
const ALL_STREAMS: DataChannelRates = DataChannelRates {
imu_hz: 1,
range_hz: Some(1),
optical_flow_hz: Some(1),
heartbeat_hz: 1,
};
const HEARTBEAT: HEARTBEAT_DATA = HEARTBEAT_DATA {
state: HeartbeatState::HEARTBEAT_STATE_INITIALIZED_OUTPUTS_INHIBITED,
};
#[derive(Default)]
struct Mailboxes {
imu: LatestImuMailbox,
range: LatestRangeMailbox,
optical_flow: LatestOpticalFlowMailbox,
}
impl Mailboxes {
fn split(&mut self) -> (Publishers<'_>, DataCollector<'_>) {
let (imu_producer, imu) = self.imu.split();
let (range_producer, range) = self.range.split();
let (flow_producer, optical_flow) = self.optical_flow.split();
(
Publishers {
imu: imu_producer,
range: range_producer,
optical_flow: flow_producer,
},
DataCollector::new(DataSources {
imu,
range,
optical_flow,
}),
)
}
}
struct Publishers<'a> {
imu: Producer<'a, ImuSnapshot>,
range: Producer<'a, RangeSnapshot>,
optical_flow: Producer<'a, OpticalFlowSnapshot>,
}
impl Publishers<'_> {
fn publish(&mut self, samples: TelemetrySamples) {
if let Some(imu) = samples.imu {
self.imu.publish(imu);
}
if let Some(range) = samples.range {
self.range.publish(range);
}
if let Some(flow) = samples.optical_flow {
self.optical_flow.publish(flow);
}
}
}
fn samples_at(microseconds: u64) -> TelemetrySamples {
let timestamp = Timestamp::from_microseconds(microseconds);
TelemetrySamples {
imu: Some(ImuSnapshot::new(
timestamp,
[-1_001, 2, 3],
[4, -5_006, 7],
2_501,
)),
range: Some(RangeSnapshot::new(timestamp, 2_913)),
optical_flow: Some(OpticalFlowSnapshot::new(timestamp, [-123, 456], 2_917, 201)),
}
}
fn decode(pending: &PendingFrame<MavlinkFrame>) -> MavMessage {
assert!(pending.frame.has_valid_crc::<MavMessage>());
let mut receiver = IntegrityMavlinkReceiver::<1>::new();
for byte in pending.frame.raw_bytes() {
receiver.receive_byte(*byte);
}
receiver.take_received().unwrap().decode().unwrap()
}
fn assert_sample_frame(pending: &PendingFrame<MavlinkFrame>, samples: &TelemetrySamples) -> usize {
let index = match decode(pending) {
MavMessage::IMU(data) => {
let imu = samples.imu.unwrap();
assert_eq!(data.time_us, imu.timestamp.as_microseconds());
assert_eq!(
data.acceleration_milligravity,
imu.acceleration_milligravity
);
assert_eq!(
data.angular_rate_milliradians_per_second,
imu.angular_rate_milliradians_per_second
);
assert_eq!(
data.temperature_centidegrees_celsius,
imu.temperature_centidegrees_celsius
);
0
}
MavMessage::RANGE(data) => {
let range = samples.range.unwrap();
assert_eq!(data.time_us, range.timestamp.as_microseconds());
assert_eq!(data.distance_millimetres, range.distance_millimetres);
1
}
MavMessage::OPTICAL_FLOW(data) => {
let flow = samples.optical_flow.unwrap();
assert_eq!(data.time_us, flow.timestamp.as_microseconds());
assert_eq!(
data.velocity_millimetres_per_second,
flow.velocity_millimetres_per_second
);
assert_eq!(
data.ground_distance_millimetres,
flow.ground_distance_millimetres
);
assert_eq!(data.quality, flow.quality);
2
}
MavMessage::HEARTBEAT(data) => {
assert_eq!(data.state, HEARTBEAT.state);
3
}
_ => panic!("unexpected telemetry message"),
};
assert_eq!(
pending.class,
if index == 3 {
FrameClass::Vital
} else {
FrameClass::BestEffort
}
);
index
}
#[test]
fn collector_retains_latest_samples_between_publications() {
let mut mailboxes = Mailboxes::default();
let (mut publishers, mut collector) = mailboxes.split();
assert_eq!(collector.snapshot(), TelemetrySamples::default());
publishers.publish(samples_at(1));
publishers.publish(samples_at(2));
let first = collector.snapshot();
assert_eq!(first, samples_at(2));
assert_eq!(collector.snapshot(), first);
let range = RangeSnapshot::new(Timestamp::from_microseconds(3), 4_567);
publishers.range.publish(range);
let updated = TelemetrySamples {
range: Some(range),
..first
};
assert_eq!(collector.snapshot(), updated);
assert_eq!(collector.snapshot(), updated);
assert_eq!(first, samples_at(2));
}
#[test]
fn owns_rate_selection_encoding_and_classification() {
let mut mailboxes = Mailboxes::default();
let (mut publishers, mut collector) = mailboxes.split();
let mut channel = DataChannel::<100>::new(1, 1, RATES, 100_000).unwrap();
let mut counts = [0; 4];
let mut sequence = 0;
for tick in 0..100 {
let now = Timestamp::from_microseconds(tick * 10_000);
publishers.publish(samples_at(now.as_microseconds()));
let samples = collector.snapshot();
if let Some(frame) = channel.on_service_tick(&samples, now) {
assert_eq!(frame.frame.sequence(), sequence);
sequence += 1;
counts[assert_sample_frame(&frame, &samples)] += 1;
}
}
assert_eq!(counts, [20, 20, 20, 2]);
}
#[test]
fn links_share_one_snapshot_with_independent_rates_and_sequences() {
let mut mailboxes = Mailboxes::default();
let (mut publishers, mut collector) = mailboxes.split();
let mut fast = DataChannel::<100>::new(1, 2, RATES, 100_000).unwrap();
let mut slow = DataChannel::<100>::new(
3,
4,
DataChannelRates {
imu_hz: 10,
range_hz: Some(5),
optical_flow_hz: None,
heartbeat_hz: 1,
},
100_000,
)
.unwrap();
let external = slow.encode_message(FrameClass::Critical, &HEARTBEAT);
assert_eq!(external.class, FrameClass::Critical);
assert_eq!(external.frame.sequence(), 0);
assert!(matches!(decode(&external), MavMessage::HEARTBEAT(_)));
let mut sequences = [0, 1];
let mut counts = [[0; 4]; 2];
for tick in 0..100 {
let now = Timestamp::from_microseconds(tick * 10_000);
publishers.publish(samples_at(now.as_microseconds()));
let samples = collector.snapshot();
for (link, channel) in [&mut fast, &mut slow].into_iter().enumerate() {
if let Some(frame) = channel.on_service_tick(&samples, now) {
assert_eq!(frame.frame.sequence(), sequences[link]);
assert_eq!(
&frame.frame.raw_bytes()[5..7],
if link == 0 { &[1, 2] } else { &[3, 4] }
);
sequences[link] += 1;
counts[link][assert_sample_frame(&frame, &samples)] += 1;
}
}
}
assert_eq!(counts, [[20, 20, 20, 2], [10, 5, 0, 1]]);
assert_eq!(sequences, [62, 17]);
}
#[test]
fn stalled_and_backpressured_link_does_not_consume_another_links_data() {
let mut mailboxes = Mailboxes::default();
let (mut publishers, mut collector) = mailboxes.split();
let mut healthy = DataChannel::<4>::new(1, 1, ALL_STREAMS, 100_000).unwrap();
let mut blocked = DataChannel::<4>::new(1, 2, ALL_STREAMS, 100_000).unwrap();
let mut queue = TransmitQueue::<_, 1>::new();
assert_eq!(
queue.enqueue(blocked.encode_message(FrameClass::Vital, &HEARTBEAT)),
EnqueueResult::Enqueued
);
let mut counts = [[0; 4]; 2];
for tick in 0..8 {
let now = Timestamp::from_microseconds(tick * 250_000);
publishers.publish(samples_at(now.as_microseconds()));
let samples = collector.snapshot();
if tick >= 4 {
let frame = blocked.on_service_tick(&samples, now).unwrap();
assert_eq!(frame.frame.sequence(), (tick - 3) as u8);
counts[1][assert_sample_frame(&frame, &samples)] += 1;
assert_eq!(
queue.enqueue(frame),
if frame.class == FrameClass::Vital {
EnqueueResult::VitalQueueFull
} else {
EnqueueResult::BestEffortDropped
}
);
}
let frame = healthy.on_service_tick(&samples, now).unwrap();
assert_eq!(frame.frame.sequence(), tick as u8);
counts[0][assert_sample_frame(&frame, &samples)] += 1;
}
assert_eq!(counts, [[2; 4], [1; 4]]);
}
#[test]
fn imu_requires_a_strictly_newer_timestamp_even_after_clock_regression() {
let mut channel = DataChannel::<2>::new(
1,
1,
DataChannelRates {
range_hz: None,
optical_flow_hz: None,
..ALL_STREAMS
},
100_000,
)
.unwrap();
let mut sequence = 0;
for (timestamp, expected) in [
(Some(0), true),
(Some(0), false),
(None, false),
(Some(0), false),
(Some(1), true),
(Some(0), false),
(Some(2), true),
] {
let now = Timestamp::from_microseconds(timestamp.unwrap_or(0));
let mut samples = samples_at(now.as_microseconds());
samples.imu = timestamp.map(|_| ImuSnapshot::new(now, [sequence as i16; 3], [0; 3], 0));
let frame = channel.on_service_tick(&samples, now);
assert_eq!(frame.is_some(), expected);
if let Some(frame) = frame {
assert_eq!(frame.frame.sequence(), sequence);
sequence += 1;
assert_eq!(assert_sample_frame(&frame, &samples), 0);
}
let heartbeat = channel.on_service_tick(&samples, now).unwrap();
assert_eq!(heartbeat.frame.sequence(), sequence);
sequence += 1;
assert_eq!(assert_sample_frame(&heartbeat, &samples), 3);
}
}
fn assert_vertical_cycle(
channel: &mut DataChannel<4>,
samples: &TelemetrySamples,
now: Timestamp,
expected: [bool; 2],
) {
channel.on_service_tick(samples, now);
let heartbeat = channel.on_service_tick(samples, now).unwrap();
assert_eq!(assert_sample_frame(&heartbeat, samples), 3);
for (index, expected) in expected.into_iter().enumerate() {
let frame = channel.on_service_tick(samples, now);
assert_eq!(frame.is_some(), expected);
if let Some(frame) = frame {
assert_eq!(assert_sample_frame(&frame, samples), index + 1);
}
}
}
#[test]
fn vertical_freshness_uses_sample_time_on_first_collection() {
for (sample_time, now, maximum_age, expected) in [
(0, 100_001, 100_000, false),
(100_001, 100_000, 100_000, false),
(u64::MAX, 0, u64::MAX, false),
(0, 100_000, 100_000, true),
(42, 42, 0, true),
(42, 43, 0, false),
] {
let mut mailboxes = Mailboxes::default();
let (mut publishers, mut collector) = mailboxes.split();
publishers.publish(samples_at(sample_time));
let samples = collector.snapshot();
let mut channel = DataChannel::<4>::new(1, 1, ALL_STREAMS, maximum_age).unwrap();
assert_vertical_cycle(
&mut channel,
&samples,
Timestamp::from_microseconds(now),
[expected; 2],
);
}
}
#[test]
fn repeated_ticks_and_republication_do_not_change_sample_age() {
let mut mailboxes = Mailboxes::default();
let (mut publishers, mut collector) = mailboxes.split();
let mut channel = DataChannel::<4>::new(1, 1, ALL_STREAMS, 100_000).unwrap();
for (now, expected, repetitions) in [
(110_000, true, 64),
(110_001, false, 1),
(1_000_000, false, 1),
] {
for _ in 0..repetitions {
publishers.publish(samples_at(10_000));
let samples = collector.snapshot();
assert_vertical_cycle(
&mut channel,
&samples,
Timestamp::from_microseconds(now),
[expected; 2],
);
}
}
}
#[test]
fn vertical_streams_use_their_own_sample_timestamps() {
for (range_time, flow_time, expected) in [
(99_999, 100_000, [false, true]),
(200_000, 200_001, [true, false]),
] {
let samples = TelemetrySamples {
range: samples_at(range_time).range,
optical_flow: samples_at(flow_time).optical_flow,
..TelemetrySamples::default()
};
let mut channel = DataChannel::<4>::new(1, 1, ALL_STREAMS, 100_000).unwrap();
assert_vertical_cycle(
&mut channel,
&samples,
Timestamp::from_microseconds(200_000),
expected,
);
}
}
#[test]
fn invalid_rates_keep_existing_errors() {
for (rates, expected) in [
(
DataChannelRates { imu_hz: 0, ..RATES },
DataChannelError::ZeroRate,
),
(
DataChannelRates {
range_hz: Some(0),
..RATES
},
DataChannelError::ZeroRate,
),
(
DataChannelRates {
imu_hz: 101,
..RATES
},
DataChannelError::RateExceedsServiceFrequency,
),
(
DataChannelRates {
imu_hz: 100,
..RATES
},
DataChannelError::ScheduleFull,
),
] {
assert_eq!(
DataChannel::<100>::new(1, 1, rates, 100_000).err(),
Some(expected)
);
}
}
firmware/libs/kaptan-diagnostics-mavlink/src/lib.rs
+3 -5
View File
@@ -1,8 +1,6 @@
//! MAVLink projection for Kaptan logs and reportable firmware errors.
//!
//! 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);
firmware/libs/kaptan-dma/src/lib.rs
-73
View File
@@ -1,73 +0,0 @@
//! DMA memory ownership and non-blocking transfer contracts.
//!
//! Board/HAL code proves the memory region, cache policy, and peripheral route.
//! Runtime code sees only prepared immutable TX ranges and terminal outcomes.
#![no_std]
/// A borrowed byte range prepared for a device-to-read DMA transfer.
///
/// This is a capability rather than a general byte-slice wrapper. It exists
/// only after a platform-selected policy proves DMA reachability and CPU-to-device
/// visibility for the selected controller.
pub struct DmaReadableBuffer<'a> {
bytes: &'a [u8],
}
impl<'a> DmaReadableBuffer<'a> {
/// Creates a DMA-read capability after platform-specific validation.
///
/// # Safety
///
/// - `bytes` is reachable by the selected DMA engine.
/// - CPU writes to `bytes` are visible before transfer begins.
/// - `bytes` remains addressable, immovable, and immutable until a terminal
/// completion or a documented route-halt outcome releases ownership.
pub const unsafe fn new_unchecked(bytes: &'a [u8]) -> Self {
Self { bytes }
}
/// Returns the DMA-visible byte range.
pub const fn bytes(&self) -> &'a [u8] {
self.bytes
}
}
/// Prepares CPU-owned bytes for a DMA transmit handoff.
///
/// A cache-disabled board validates the RAM range. A cached board must clean
/// the exact range and issue the required ordering barrier before returning.
pub trait DmaTxBufferFactory {
/// Platform-specific preparation failure.
type Error;
/// Validates and prepares a frame for device DMA reads.
fn prepare<'a>(&mut self, bytes: &'a [u8]) -> Result<DmaReadableBuffer<'a>, Self::Error>;
}
/// Terminal result reported by a DMA transmit interrupt handler.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum TxCompletion {
/// The selected engine has no terminal event pending.
NotPending,
/// DMA reached the peripheral data-register endpoint.
///
/// This does not prove that the final UART stop bit left the wire.
Complete,
/// The engine reported a terminal transfer fault.
TransferFault,
/// The route entered a halted state and requires explicit reinitialization.
Halted,
}
/// Owns one non-blocking DMA-backed transmit route.
///
/// The caller retains the prepared range until a terminal completion or fault.
/// Implementations must not poll for a hardware state transition from this API.
pub trait DmaTxEngine {
/// Start-operation failure.
type StartError;
/// Starts one DMA transfer from a prepared readable buffer.
fn start(&mut self, buffer: DmaReadableBuffer<'_>) -> Result<(), Self::StartError>;
}
firmware/libs/kaptan-drivers/src/icm42688/error.rs
+12
View File
@@ -18,4 +18,16 @@ pub enum Error<BusError> {
/// The selected SPI route reported a terminal transfer fault.
#[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,
}
firmware/libs/kaptan-drivers/src/icm42688/mod.rs
+426 -125
View File
@@ -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>> {
/// 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);
}
match self.state {
State::WaitingForReset => {
self.prepare_read(WHO_AM_I, 2);
self.submit(2)?;
self.state = State::ReadingIdentity;
State::Waiting {
next,
since,
delay_us,
} if elapsed(now, since, delay_us) => {
self.queue(next, now);
}
State::WaitingForSensorStart => {
self.start_configuration(7)?;
State::Settling { since } if elapsed(now, since, SAMPLE_PERIOD_US) => {
self.state = State::Ready { since: now };
return Icm42688Event::Initialized;
}
_ => {}
}
Ok(())
match self.submit_pending(now) {
Ok(()) => Icm42688Event::NotPending,
Err(error) => Icm42688Event::Fault(error),
}
}
/// Starts one temperature, accelerometer and gyroscope burst read.
pub fn request_sample(&mut self, timestamp: Timestamp) -> Result<(), Error<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 complete_transfer(&mut self, _: SpiCompletion) {}
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));
}
#[test]
fn accepts_a_sample_only_after_identity_and_wake_delays() {
let mut response = [0; TRANSFER_CAPACITY];
response[1] = 0x47;
let mut imu = Icm42688::new(Bus { response });
imu.begin_initialization().unwrap();
fn initialized() -> Icm42688<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),
imu.on_transfer_complete(SpiCompletion::Complete, t(100)),
Icm42688Event::Initializing
));
assert!(matches!(imu.advance_initialization(), Ok(())));
imu.on_periodic_tick(t(1_099));
assert_eq!(imu.bus.starts, 1);
imu.on_periodic_tick(t(1_100));
assert!(matches!(
imu.on_transfer_complete(SpiCompletion::Complete),
imu.on_transfer_complete(SpiCompletion::Complete, t(1_200)),
Icm42688Event::Initializing
));
for _ in 0..7 {
for index in 0..7 {
assert!(matches!(
imu.on_transfer_complete(SpiCompletion::Complete),
imu.on_transfer_complete(SpiCompletion::Complete, t(1_300 + index * 100)),
Icm42688Event::Initializing
));
}
assert!(matches!(imu.advance_initialization(), Ok(())));
for now in [1_900, 2_099, 2_100, 31_900, 46_899] {
imu.on_periodic_tick(t(now));
assert_eq!(imu.bus.starts, 9);
}
imu.on_periodic_tick(t(46_900));
assert!(matches!(
imu.on_transfer_complete(SpiCompletion::Complete),
imu.on_transfer_complete(SpiCompletion::Complete, t(47_000)),
Icm42688Event::Initializing
));
assert!(matches!(
imu.on_transfer_complete(SpiCompletion::Complete),
Icm42688Event::Initialized
));
imu.on_transfer_complete(SpiCompletion::Complete, t(47_100));
imu.on_periodic_tick(t(48_099));
assert!(!imu.is_ready());
assert_eq!(imu.request_sample(t(48_099)), Err(Error::NotReady));
assert_eq!(imu.on_periodic_tick(t(48_100)), Icm42688Event::Initialized);
assert!(imu.is_ready());
imu.request_sample(Timestamp::from_microseconds(1)).unwrap();
imu
}
#[test]
fn accepts_a_sample_only_after_elapsed_reset_wake_and_data_settling() {
let mut imu = initialized();
imu.request_sample(t(48_101)).unwrap();
assert_eq!(imu.receive, [0; TRANSFER_CAPACITY]);
assert_eq!(
imu.on_transfer_complete(SpiCompletion::NotPending, t(48_102)),
Icm42688Event::NotPending
);
assert_eq!(imu.receive, [0; TRANSFER_CAPACITY]);
imu.transmit.fill(0xff);
imu.bus.response.fill(0xff);
let mut imu = core::mem::replace(&mut imu, Icm42688::new(FakeBus::ready()));
assert!(matches!(
imu.on_transfer_complete(SpiCompletion::Complete),
Icm42688Event::Sample(_)
imu.on_transfer_complete(SpiCompletion::Complete, t(48_300)),
Icm42688Event::Sample(sample) if sample.timestamp == t(48_101)
));
assert_eq!(imu.receive[1], 0x47);
assert_eq!(
imu.on_transfer_complete(SpiCompletion::Complete, t(48_301)),
Icm42688Event::NotPending
);
}
#[test]
fn rejects_a_wrong_identity() {
let mut imu = Icm42688::new(Bus {
response: [0; TRANSFER_CAPACITY],
});
imu.begin_initialization().unwrap();
let _ = imu.on_transfer_complete(SpiCompletion::Complete);
imu.advance_initialization().unwrap();
let mut imu = Icm42688::new(FakeBus::ready());
start(&mut imu);
let _ = imu.on_transfer_complete(SpiCompletion::Complete, t(0));
imu.on_periodic_tick(t(1_000));
assert!(matches!(
imu.on_transfer_complete(SpiCompletion::Complete),
imu.on_transfer_complete(SpiCompletion::Complete, t(1_100)),
Icm42688Event::Fault(Error::UnexpectedIdentity(0))
));
}
#[test]
fn completion_errors_never_republish_a_previous_sample() {
for (event, error) in [
(
SpiCompletion::Complete,
Some(CompletionError::InvalidLength),
),
(SpiCompletion::Complete, Some(CompletionError::Quarantined)),
(SpiCompletion::Complete, Some(CompletionError::NotActive)),
(
SpiCompletion::Complete,
Some(CompletionError::TransferFault),
),
(SpiCompletion::TransferFault, None),
] {
let mut driver = Icm42688::new(FakeBus::ready());
driver.state = State::Ready { since: t(0) };
driver.request_sample(t(1)).unwrap();
assert!(matches!(
driver.on_transfer_complete(SpiCompletion::Complete, t(1)),
Icm42688Event::Sample(_)
));
driver.request_sample(t(2)).unwrap();
driver.receive.fill(0xa5);
driver.bus.fail_completion = error;
assert!(matches!(
driver.on_transfer_complete(event, t(2)),
Icm42688Event::Fault(_)
));
assert_eq!(driver.receive, [0xa5; TRANSFER_CAPACITY]);
assert!(!driver.is_ready());
assert!(!matches!(
driver.on_transfer_complete(SpiCompletion::Complete, t(3)),
Icm42688Event::Sample(_)
));
if error == Some(CompletionError::Quarantined) {
assert_eq!(driver.begin_initialization(t(4)), Err(Error::NotReady));
}
}
}
#[test]
fn rejected_submission_cannot_complete_a_stale_sample() {
let mut driver = Icm42688::new(FakeBus::ready());
driver.state = State::Ready { since: t(0) };
driver.receive.fill(0xa5);
driver.bus.fail_start = true;
assert_eq!(
driver.request_sample(t(1)),
Err(Error::Start(FakeError::Start))
);
assert_eq!(
driver.on_transfer_complete(SpiCompletion::Complete, t(2)),
Icm42688Event::NotPending
);
}
#[test]
fn every_phase_times_out_and_cancels_once_without_rearming() {
for operation in [Operation::Reset, Operation::Identity, Operation::Sample]
.into_iter()
.chain((0..9).map(Operation::Configure))
{
let mut driver = Icm42688::new(FakeBus::ready());
driver.queue(operation, t(100));
driver.submit_pending(t(100)).unwrap();
assert_eq!(
driver.on_periodic_tick(t(10_099)),
Icm42688Event::NotPending
);
assert_eq!(driver.bus.cancellations, 0);
assert_eq!(
driver.on_periodic_tick(t(10_100)),
Icm42688Event::Fault(Error::TransferTimeout)
);
assert_eq!(driver.bus.cancellations, 1);
assert_eq!(
driver.on_transfer_complete(SpiCompletion::Complete, t(10_101)),
Icm42688Event::NotPending
);
driver.on_periodic_tick(t(1_000_000));
assert_eq!(driver.bus.starts, 1);
assert_eq!(
driver.begin_initialization(t(1_000_000)),
Err(Error::NotReady)
);
}
}
#[test]
fn every_busy_phase_preserves_pending_operation_and_has_an_admission_deadline() {
for operation in [Operation::Reset, Operation::Identity, Operation::Sample]
.into_iter()
.chain((0..9).map(Operation::Configure))
{
let mut driver = Icm42688::new(FakeBus::ready());
driver.bus.busy = true;
driver.queue(operation, t(100));
driver.submit_pending(t(100)).unwrap();
assert_eq!(
driver.state,
State::Pending {
operation,
since: t(100)
}
);
driver.on_periodic_tick(t(200));
driver.bus.busy = false;
driver.on_periodic_tick(t(300));
assert_eq!(
driver.state,
State::Active {
operation,
since: t(300)
}
);
assert_eq!(driver.bus.starts, 1);
}
let mut driver = Icm42688::new(FakeBus::ready());
driver.bus.busy = true;
start(&mut driver);
assert_eq!(
driver.on_periodic_tick(t(20_000)),
Icm42688Event::Fault(Error::AdmissionTimeout)
);
assert_eq!(driver.bus.cancellations, 0);
}
#[test]
fn missed_drdy_late_completion_and_failed_cancellation_never_publish() {
let mut driver = initialized();
driver.on_periodic_tick(t(60_099));
assert_eq!(driver.bus.starts, 11);
assert_eq!(
driver.on_periodic_tick(t(60_100)),
Icm42688Event::Fault(Error::DataReadyTimeout)
);
let mut driver = initialized();
driver.request_sample(t(49_000)).unwrap();
assert_eq!(
driver.on_transfer_complete(SpiCompletion::Complete, t(59_000)),
Icm42688Event::Fault(Error::TransferTimeout)
);
let mut driver = initialized();
driver.request_sample(t(49_000)).unwrap();
driver.bus.fail_completion = Some(CompletionError::Quarantined);
assert_eq!(
driver.on_periodic_tick(t(59_000)),
Icm42688Event::Fault(Error::Completion(CompletionError::Quarantined))
);
assert_eq!(driver.bus.start_transfer(&[1]), Err(FakeError::Quarantined));
}
}
firmware/libs/kaptan-drivers/src/icm42688/sample.rs
+66 -4
View File
@@ -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;
}
}
}
firmware/libs/kaptan-drivers/src/icm42688/service.rs
+110 -20
View File
@@ -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) {
Icm42688Event::NotPending | Icm42688Event::Initializing => Icm42688ServiceEvent::Idle,
Icm42688Event::Initialized => Icm42688ServiceEvent::Initialized,
Icm42688Event::Sample(sample) => Icm42688ServiceEvent::Sample(sample),
Icm42688Event::Fault(error) => Icm42688ServiceEvent::Fault(error),
service_event(self.imu.on_transfer_complete(completion, self.clock.now()))
}
}
fn service_event<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);
}
}
firmware/libs/kaptan-drivers/src/lib.rs
-4
View File
@@ -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]
firmware/libs/kaptan-drivers/src/lidar_lite_v3/error.rs
+3
View File
@@ -29,6 +29,9 @@ pub enum Error<BusError> {
/// The device reported a documented no-signal sentinel distance of zero or one centimetre.
#[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.
firmware/libs/kaptan-drivers/src/lidar_lite_v3/mod.rs
+286 -49
View File
@@ -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());
}
}
}
}
firmware/libs/kaptan-drivers/src/lidar_lite_v3/service.rs
+137 -22
View File
@@ -3,16 +3,16 @@
use super::{AcquisitionMode, Error, LidarLiteV3, LidarLiteV3Event, StartupError};
use 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);
}
}
}
firmware/libs/kaptan-drivers/src/mpu6000/error.rs
+12
View File
@@ -18,4 +18,16 @@ pub enum Error<BusError> {
/// The selected SPI route reported a terminal transfer fault.
#[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,
}
firmware/libs/kaptan-drivers/src/mpu6000/mod.rs
+489 -163
View File
@@ -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));
}
}
firmware/libs/kaptan-drivers/src/mpu6000/sample.rs
+96 -2
View File
@@ -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);
}
}
}
firmware/libs/kaptan-drivers/src/mpu6000/service.rs
+63 -50
View File
@@ -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),
};
}
let _ = self.clock.now();
match self.imu.advance_initialization() {
Ok(()) => Mpu6000ServiceEvent::Idle,
Err(error) => Mpu6000ServiceEvent::Fault(error),
}
service_event(self.imu.on_periodic_tick(self.clock.now()))
}
/// Handles one coalesced data-ready event.
/// Handles one coalesced data-ready notification. Time is captured here in
/// the worker, at read submission, not at IRQ entry or physical sampling.
pub fn on_data_ready(&mut self) -> Mpu6000ServiceEvent<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) {
Mpu6000Event::NotPending | Mpu6000Event::Initializing => Mpu6000ServiceEvent::Idle,
Mpu6000Event::Initialized => Mpu6000ServiceEvent::Initialized,
Mpu6000Event::Sample(sample) => Mpu6000ServiceEvent::Sample(sample),
Mpu6000Event::Fault(error) => Mpu6000ServiceEvent::Fault(error),
}
service_event(self.imu.on_transfer_complete(completion, self.clock.now()))
}
}
fn service_event<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
));
}
}
firmware/libs/kaptan-drivers/src/mpu6000/test_bus.rs
+97
View File
@@ -0,0 +1,97 @@
//! Test transport with owned pending bytes, never retained caller pointers.
use kaptan_spi::{CompletionError, SpiCompletion, SpiDevice};
#[derive(Debug, Eq, PartialEq)]
pub(crate) enum FakeError {
Start,
Busy,
InvalidLength,
Quarantined,
}
pub(crate) struct FakeBus {
pub response: [u8; 15],
pub fail_start: bool,
pub fail_completion: Option<CompletionError>,
pub busy: bool,
pub starts: usize,
pub cancellations: usize,
pub transmitted: [u8; 15],
pending: Option<([u8; 15], usize)>,
quarantined: bool,
}
impl FakeBus {
pub fn ready() -> Self {
Self {
response: [0; 15],
fail_start: false,
fail_completion: None,
busy: false,
starts: 0,
cancellations: 0,
transmitted: [0; 15],
pending: None,
quarantined: false,
}
}
}
impl SpiDevice for FakeBus {
type StartError = FakeError;
fn is_busy(error: &Self::StartError) -> bool {
*error == FakeError::Busy
}
fn start_transfer(&mut self, transmit: &[u8]) -> Result<(), Self::StartError> {
if self.quarantined {
return Err(FakeError::Quarantined);
}
if self.pending.is_some() || self.busy {
return Err(FakeError::Busy);
}
if self.fail_start {
return Err(FakeError::Start);
}
if transmit.is_empty() || transmit.len() > self.response.len() {
return Err(FakeError::InvalidLength);
}
// Simulated hardware computes its response at submission, but only the
// accepted completion is allowed to copy those owned bytes to a caller.
self.pending = Some((self.response, transmit.len()));
self.transmitted[..transmit.len()].copy_from_slice(transmit);
self.starts += 1;
Ok(())
}
fn complete_transfer(
&mut self,
completion: SpiCompletion,
receive: &mut [u8],
) -> Result<(), CompletionError> {
if completion == SpiCompletion::NotPending {
return Err(CompletionError::NotPending);
}
if self.quarantined {
return Err(CompletionError::Quarantined);
}
if completion == SpiCompletion::TransferFault {
self.cancellations += 1;
}
let (pending, length) = self.pending.take().ok_or(CompletionError::NotActive)?;
if let Some(error) = self.fail_completion.take() {
self.quarantined = error == CompletionError::Quarantined;
return Err(error);
}
if completion == SpiCompletion::TransferFault {
return Err(CompletionError::TransferFault);
}
if receive.len() != length {
return Err(CompletionError::InvalidLength);
}
receive.copy_from_slice(&pending[..length]);
Ok(())
}
}
firmware/libs/kaptan-drivers/src/mtf01/mod.rs
+53 -133
View File
@@ -1,14 +1,7 @@
//! MicoAir MTF-01 Microlink UART frame decoder.
//!
//! 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));
firmware/libs/kaptan-file-mavlink/src/lib.rs
+1 -1
View File
@@ -18,7 +18,7 @@ pub use protocol::{
Content, ControlRequest, ControlResponse, DataRequest, DataResponse, FileFailure, FileMetadata,
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;
firmware/libs/kaptan-file-mavlink/src/protocol.rs
+3 -3
View File
@@ -36,7 +36,7 @@ impl Target {
}
}
/// Exact MAVLink source that owns an open transfer.
/// Claimed MAVLink source identity; these wire fields are not authentication.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
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,
firmware/libs/kaptan-file-mavlink/src/service.rs
+269 -183
View File
@@ -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,44 +321,37 @@ 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(
principal,
request.operation,
request.transfer_id,
request.position,
session.metadata,
FileStatus::InvalidState,
Content::EMPTY,
));
return DispatchResult::control(
response(
principal,
request.operation,
request.transfer_id,
request.position,
session.metadata,
FileStatus::InvalidState,
Content::EMPTY,
),
false,
);
}
if request.position != session.position {
if let Some(last) = session
@@ -321,44 +359,53 @@ impl FileService {
.as_ref()
.filter(|last| last.position == request.position)
{
return FileResponse::Data(DataResponse {
return DispatchResult::data(DataResponse {
target: principal.target(),
position: request.position,
transfer_id: request.transfer_id,
data: last.data,
});
}
return FileResponse::Control(response(
principal,
request.operation,
request.transfer_id,
request.position,
session.metadata,
FileStatus::InvalidPosition,
Content::EMPTY,
));
return DispatchResult::control(
response(
principal,
request.operation,
request.transfer_id,
request.position,
session.metadata,
FileStatus::InvalidPosition,
Content::EMPTY,
),
false,
);
}
let Some(logical) = chunk_length(session.metadata.size, request.position) else {
return FileResponse::Control(response(
principal,
request.operation,
request.transfer_id,
request.position,
session.metadata,
FileStatus::InvalidPosition,
Content::EMPTY,
));
return DispatchResult::control(
response(
principal,
request.operation,
request.transfer_id,
request.position,
session.metadata,
FileStatus::InvalidPosition,
Content::EMPTY,
),
false,
);
};
if logical == 0 {
return FileResponse::Control(response(
principal,
request.operation,
request.transfer_id,
request.position,
session.metadata,
FileStatus::InvalidPosition,
Content::EMPTY,
));
return DispatchResult::control(
response(
principal,
request.operation,
request.transfer_id,
request.position,
session.metadata,
FileStatus::InvalidPosition,
Content::EMPTY,
),
false,
);
}
let mut data = [0_u8; DATA_BYTES];
if let Err(error) = provider.read(request.position, &mut data[..logical]) {
@@ -373,15 +420,14 @@ impl FileService {
error.status(),
error.content(),
);
self.remember(principal, FileRequest::Control(request), response);
return FileResponse::Control(response);
return self.remember(peer, FileRequest::Control(request), response);
}
session.last_chunk = Some(LastChunk {
position: request.position,
data,
});
session.position += logical as u32;
FileResponse::Data(DataResponse {
DispatchResult::data(DataResponse {
target: principal.target(),
position: request.position,
transfer_id: request.transfer_id,
@@ -392,66 +438,64 @@ impl FileService {
fn data<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 {
write_ack(principal, session)
} else {
data_failure(principal, request, FileStatus::RequestConflict)
});
return DispatchResult::control(
if replayed {
write_ack(principal, session)
} else {
data_failure(principal, request, FileStatus::RequestConflict)
},
replayed,
);
}
if request.position != session.position {
return FileResponse::Control(data_failure(
principal,
request,
FileStatus::InvalidPosition,
));
return DispatchResult::control(
data_failure(principal, request, FileStatus::InvalidPosition),
false,
);
}
if let Err(error) = provider.write(request.position, &request.data[..logical]) {
provider.abort();
let response = data_provider_failure(principal, request, error);
self.remember(principal, FileRequest::Data(request), response);
return FileResponse::Control(response);
return self.remember(peer, FileRequest::Data(request), response);
}
session.checksum.update(&request.data[..logical]);
session.position += logical as u32;
@@ -459,22 +503,23 @@ impl FileService {
position: request.position,
data: request.data,
});
FileResponse::Control(write_ack(principal, session))
DispatchResult::control(write_ack(principal, session), true)
}
fn close<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,15 +620,18 @@ impl FileService {
principal: Principal,
operation: FileOperation,
session: &Session,
) -> ControlResponse {
response(
principal,
operation,
session.transfer_id,
session.position,
session.metadata,
FileStatus::Ok,
Content::EMPTY,
) -> DispatchResult {
DispatchResult::control(
response(
principal,
operation,
session.transfer_id,
session.position,
session.metadata,
FileStatus::Ok,
Content::EMPTY,
),
true,
)
}
@@ -589,32 +641,36 @@ impl FileService {
request: ControlRequest,
status: FileStatus,
content: Content,
) -> ControlResponse {
response(
principal,
request.operation,
request.transfer_id,
request.position,
FileMetadata {
size: request.file_size,
checksum_crc32c: request.checksum_crc32c,
},
status,
content,
) -> DispatchResult {
DispatchResult::control(
response(
principal,
request.operation,
request.transfer_id,
request.position,
FileMetadata {
size: request.file_size,
checksum_crc32c: request.checksum_crc32c,
},
status,
content,
),
false,
)
}
fn replay_or_failure(&self, principal: Principal, request: ControlRequest) -> ControlResponse {
fn replay_or_failure(&self, peer: TransferPeer, request: ControlRequest) -> DispatchResult {
let principal = peer.principal;
match &self.state {
TransferState::Completed(completed)
if completed.owner == principal
if completed.owner == peer
&& completed.transfer_id == request.transfer_id
&& completed.request == FileRequest::Control(request) =>
{
completed.response
DispatchResult::control(completed.response, true)
}
TransferState::Completed(completed)
if completed.owner == principal && completed.transfer_id == request.transfer_id =>
if completed.owner == peer && completed.transfer_id == request.transfer_id =>
{
self.failure(
principal,
@@ -633,31 +689,39 @@ impl FileService {
}
}
fn replay_data_or_failure(
&self,
principal: Principal,
request: DataRequest,
) -> ControlResponse {
fn replay_data_or_failure(&self, peer: TransferPeer, request: DataRequest) -> DispatchResult {
let principal = peer.principal;
match &self.state {
TransferState::Completed(completed)
if completed.owner == principal
if completed.owner == peer
&& completed.transfer_id == request.transfer_id
&& completed.request == FileRequest::Data(request) =>
{
completed.response
DispatchResult::control(completed.response, true)
}
TransferState::Completed(completed)
if completed.owner == principal && completed.transfer_id == request.transfer_id =>
if completed.owner == peer && completed.transfer_id == request.transfer_id =>
{
data_failure(principal, request, FileStatus::RequestConflict)
DispatchResult::control(
data_failure(principal, request, FileStatus::RequestConflict),
false,
)
}
TransferState::Idle | TransferState::Open(_) | TransferState::Completed(_) => {
data_failure(principal, request, FileStatus::InvalidTransfer)
DispatchResult::control(
data_failure(principal, request, FileStatus::InvalidTransfer),
false,
)
}
}
}
fn remember(&mut self, owner: Principal, request: FileRequest, response: ControlResponse) {
fn remember(
&mut self,
owner: TransferPeer,
request: FileRequest,
response: ControlResponse,
) -> DispatchResult {
let transfer_id = match request {
FileRequest::Control(request) => request.transfer_id,
FileRequest::Data(request) => request.transfer_id,
@@ -668,6 +732,7 @@ impl FileService {
request,
response,
});
DispatchResult::control(response, true)
}
}
@@ -753,7 +818,10 @@ mod tests {
use super::*;
const SERVICE: Target = Target::new(1, 1);
const CLIENT: Principal = Principal::new(42, 7);
const CLIENT: TransferPeer = TransferPeer {
principal: Principal::new(42, 7),
ingress: IngressId(0),
};
const PATH: &[u8] = b"/test/data.bin";
#[derive(Clone, Copy, Eq, PartialEq)]
@@ -903,8 +971,12 @@ mod tests {
})
}
fn control_response(response: Option<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(
firmware/libs/kaptan-file-mavlink/tests/peer_lease.rs
+632
View File
@@ -0,0 +1,632 @@
use kaptan_file_mavlink::{
Content, ControlRequest, ControlResponse, DATA_BYTES, DataRequest, DispatchResult, FileFailure,
FileMetadata, FileOperation, FilePath, FileProvider, FileRequest, FileResponse, FileService,
FileStatus, IngressId, Principal, Target, TransferPeer, crc32c, wire,
};
const TARGET: Target = Target::new(1, 1);
const UART: TransferPeer = TransferPeer {
principal: Principal::new(42, 7),
ingress: IngressId(0),
};
const USB: TransferPeer = TransferPeer {
ingress: IngressId(1),
..UART
};
const ID: u16 = 9;
#[derive(Default)]
struct Provider {
bytes: Vec<u8>,
opens: usize,
reads: usize,
writes: usize,
closes: usize,
aborts: usize,
fail_open: bool,
fail_read: bool,
fail_write: bool,
fail_close: bool,
}
fn failure(status: FileStatus) -> FileFailure {
FileFailure::new(status, Content::try_new(b"provider detail").unwrap()).unwrap()
}
impl FileProvider for Provider {
fn open_read(&mut self, _: &FilePath) -> Result<FileMetadata, FileFailure> {
self.opens += 1;
if self.fail_open {
return Err(failure(FileStatus::NotFound));
}
Ok(FileMetadata {
size: self.bytes.len() as u32,
checksum_crc32c: crc32c(&self.bytes),
})
}
fn open_write(&mut self, path: &FilePath, _: FileMetadata) -> Result<(), FileFailure> {
self.open_read(path)?;
self.bytes.clear();
Ok(())
}
fn read(&mut self, position: u32, output: &mut [u8]) -> Result<(), FileFailure> {
self.reads += 1;
if self.fail_read {
return Err(failure(FileStatus::InvalidPosition));
}
let start = position as usize;
output.copy_from_slice(&self.bytes[start..start + output.len()]);
Ok(())
}
fn write(&mut self, position: u32, data: &[u8]) -> Result<(), FileFailure> {
self.writes += 1;
if self.fail_write {
return Err(failure(FileStatus::InvalidTransfer));
}
assert_eq!(position as usize, self.bytes.len());
self.bytes.extend_from_slice(data);
Ok(())
}
fn close(&mut self) -> Result<Content, FileFailure> {
self.closes += 1;
if self.fail_close {
return Err(failure(FileStatus::StorageFailure));
}
Ok(Content::try_new(b"committed").unwrap())
}
fn abort(&mut self) {
self.aborts += 1;
}
}
fn control(operation: FileOperation, position: u32) -> FileRequest {
FileRequest::Control(ControlRequest {
operation,
target: TARGET,
position,
file_size: 0,
checksum_crc32c: 0,
transfer_id: ID,
content: Content::EMPTY,
})
}
fn alter(request: FileRequest, change: impl FnOnce(&mut ControlRequest)) -> FileRequest {
let FileRequest::Control(mut request) = request else {
panic!("expected control request");
};
change(&mut request);
FileRequest::Control(request)
}
fn open(operation: FileOperation, bytes: &[u8]) -> FileRequest {
alter(control(operation, 0), |request| {
request.content = Content::try_new(b"/test/data.bin").unwrap();
if operation == FileOperation::OpenWrite {
request.file_size = bytes.len() as u32;
request.checksum_crc32c = crc32c(bytes);
}
})
}
fn data(position: u32, bytes: &[u8]) -> FileRequest {
let mut data = [0; DATA_BYTES];
data[..bytes.len()].copy_from_slice(bytes);
FileRequest::Data(DataRequest {
target: TARGET,
transfer_id: ID,
position,
data,
})
}
// Exercise the real message codecs while keeping ingress entirely out of band.
fn exchange(
service: &mut FileService,
provider: &mut Provider,
peer: TransferPeer,
request: FileRequest,
) -> DispatchResult {
let decoded = wire::decode_request(&wire::encode_request(request)).unwrap();
let result = service.dispatch(provider, peer, decoded).unwrap();
assert_eq!(
wire::decode_response(&wire::encode_response(result.response)),
Ok(result.response)
);
let target = match result.response {
FileResponse::Control(response) => response.target,
FileResponse::Data(response) => response.target,
};
assert_eq!(
target,
Target::new(peer.principal.system(), peer.principal.component())
);
result
}
fn expect_control(result: DispatchResult, status: FileStatus, refresh: bool) -> ControlResponse {
assert_eq!(result.refresh_lease, refresh);
let FileResponse::Control(response) = result.response else {
panic!("expected control response");
};
assert_eq!(response.status, status);
response
}
#[test]
fn equal_wire_identities_cannot_modify_or_replay_another_ingress_upload() {
for (owner, other_ingress) in [(UART, USB), (USB, UART)] {
let mut service = FileService::new(TARGET);
let mut provider = Provider::default();
let start = open(FileOperation::OpenWrite, b"abc");
let chunk = data(0, b"abc");
let close = control(FileOperation::Close, 3);
let opened = exchange(&mut service, &mut provider, owner, start);
expect_control(opened, FileStatus::Ok, true);
assert_eq!(exchange(&mut service, &mut provider, owner, start), opened);
assert_eq!(provider.opens, 1);
// Check both components of ownership, including identical transfer IDs.
for foreign in [
other_ingress,
TransferPeer {
principal: Principal::new(43, 7),
..owner
},
] {
for (request, status) in [
(start, FileStatus::Busy),
(alter(start, |r| r.transfer_id += 1), FileStatus::Busy),
(chunk, FileStatus::InvalidTransfer),
(control(FileOperation::Read, 0), FileStatus::InvalidTransfer),
(
control(FileOperation::Abort, 0),
FileStatus::InvalidTransfer,
),
(close, FileStatus::InvalidTransfer),
(
control(FileOperation::WriteAck, 0),
FileStatus::InvalidState,
),
] {
expect_control(
exchange(&mut service, &mut provider, foreign, request),
status,
false,
);
}
}
assert_eq!(
(
provider.opens,
provider.reads,
provider.writes,
provider.closes,
provider.aborts
),
(1, 0, 0, 0, 0)
);
let ack = exchange(&mut service, &mut provider, owner, chunk);
expect_control(ack, FileStatus::Ok, true);
expect_control(
exchange(&mut service, &mut provider, other_ingress, chunk),
FileStatus::InvalidTransfer,
false,
);
assert_eq!(exchange(&mut service, &mut provider, owner, chunk), ack);
assert_eq!(provider.writes, 1);
// Lose the durable close reply, then try to steal, evict, or corrupt it.
let lost_reply = exchange(&mut service, &mut provider, owner, close);
expect_control(lost_reply, FileStatus::Ok, true);
for (request, status) in [
(close, FileStatus::InvalidTransfer),
(chunk, FileStatus::InvalidTransfer),
(start, FileStatus::Busy),
(alter(start, |r| r.transfer_id += 1), FileStatus::Busy),
(
control(FileOperation::Abort, 3),
FileStatus::InvalidTransfer,
),
(
control(FileOperation::WriteAck, 3),
FileStatus::InvalidState,
),
] {
expect_control(
exchange(&mut service, &mut provider, other_ingress, request),
status,
false,
);
}
expect_control(
exchange(
&mut service,
&mut provider,
owner,
control(FileOperation::Close, 2),
),
FileStatus::RequestConflict,
false,
);
expect_control(
exchange(&mut service, &mut provider, owner, start),
FileStatus::RequestConflict,
false,
);
for _ in 0..3 {
assert_eq!(
exchange(&mut service, &mut provider, owner, close),
lost_reply
);
}
assert_eq!(provider.bytes, b"abc");
assert_eq!(
(
provider.opens,
provider.writes,
provider.closes,
provider.aborts
),
(1, 1, 1, 0)
);
assert!(!service.expire(&mut provider));
expect_control(
exchange(&mut service, &mut provider, owner, close),
FileStatus::InvalidTransfer,
false,
);
expect_control(
exchange(&mut service, &mut provider, other_ingress, start),
FileStatus::Ok,
true,
);
expect_control(
exchange(&mut service, &mut provider, owner, close),
FileStatus::InvalidTransfer,
false,
);
assert_eq!(provider.closes, 1);
}
}
#[test]
fn read_and_abort_replay_are_bound_to_the_full_peer() {
let mut service = FileService::new(TARGET);
let mut provider = Provider {
bytes: b"snapshot".to_vec(),
..Provider::default()
};
let start = open(FileOperation::OpenRead, &[]);
expect_control(
exchange(&mut service, &mut provider, UART, start),
FileStatus::Ok,
true,
);
let read = control(FileOperation::Read, 0);
expect_control(
exchange(&mut service, &mut provider, USB, read),
FileStatus::InvalidTransfer,
false,
);
let first = exchange(&mut service, &mut provider, UART, read);
assert!(first.refresh_lease);
let FileResponse::Data(response) = first.response else {
panic!("expected data")
};
assert_eq!(&response.data[..8], b"snapshot");
expect_control(
exchange(&mut service, &mut provider, USB, read),
FileStatus::InvalidTransfer,
false,
);
expect_control(
exchange(&mut service, &mut provider, UART, data(0, b"changed")),
FileStatus::InvalidState,
false,
);
expect_control(
exchange(
&mut service,
&mut provider,
UART,
control(FileOperation::Read, 8),
),
FileStatus::InvalidPosition,
false,
);
assert_eq!(exchange(&mut service, &mut provider, UART, read), first);
assert_eq!(provider.reads, 1);
let abort = control(FileOperation::Abort, 8);
expect_control(
exchange(&mut service, &mut provider, USB, abort),
FileStatus::InvalidTransfer,
false,
);
let aborted = exchange(&mut service, &mut provider, UART, abort);
expect_control(aborted, FileStatus::Ok, true);
expect_control(
exchange(&mut service, &mut provider, USB, abort),
FileStatus::InvalidTransfer,
false,
);
assert_eq!(exchange(&mut service, &mut provider, UART, abort), aborted);
assert!(!service.expire(&mut provider));
assert!(!service.expire(&mut provider));
expect_control(
exchange(&mut service, &mut provider, UART, abort),
FileStatus::InvalidTransfer,
false,
);
expect_control(
exchange(&mut service, &mut provider, USB, start),
FileStatus::Ok,
true,
);
assert_eq!(provider.aborts, 1);
}
#[test]
fn rejected_activity_cannot_retain_open_or_completed_lease() {
const LEASE: u64 = 10;
for completed in [false, true] {
let mut service = FileService::new(TARGET);
let mut provider = Provider::default();
let start = open(FileOperation::OpenWrite, &[]);
expect_control(
exchange(&mut service, &mut provider, UART, start),
FileStatus::Ok,
true,
);
if completed {
expect_control(
exchange(
&mut service,
&mut provider,
UART,
control(FileOperation::Close, 0),
),
FileStatus::Ok,
true,
);
}
let mut lease_until = LEASE;
for now in 1..LEASE {
for peer in [UART, USB] {
for request in [
control(FileOperation::WriteAck, 0),
alter(start, |r| {
r.content = Content::try_new(b"bad path").unwrap()
}),
alter(start, |r| r.transfer_id = 0),
alter(control(FileOperation::Read, 0), |r| r.file_size = 1),
alter(control(FileOperation::Abort, 0), |r| r.checksum_crc32c = 1),
alter(control(FileOperation::Close, 0), |r| {
r.content = Content::try_new(b"bad").unwrap()
}),
data(1, b"x"),
] {
let result = exchange(&mut service, &mut provider, peer, request);
if result.refresh_lease {
lease_until = now + LEASE;
}
assert_eq!(
lease_until, LEASE,
"rejection retained the lease: {result:?}"
);
}
}
for request in [
start,
control(FileOperation::Close, 0),
control(FileOperation::Abort, 0),
] {
assert!(!exchange(&mut service, &mut provider, USB, request).refresh_lease);
}
}
assert_eq!(provider.opens, 1);
assert_eq!(provider.closes, usize::from(completed));
assert_eq!(provider.aborts, 0);
// A clock-owning caller expires the entire slot at the unchanged deadline.
assert_eq!(service.expire(&mut provider), !completed);
assert!(!service.expire(&mut provider));
assert_eq!(provider.aborts, usize::from(!completed));
expect_control(
exchange(
&mut service,
&mut provider,
UART,
control(FileOperation::Close, 0),
),
FileStatus::InvalidTransfer,
false,
);
expect_control(
exchange(&mut service, &mut provider, USB, start),
FileStatus::Ok,
true,
);
}
}
#[test]
fn invalid_owner_requests_do_not_mutate_a_live_upload_or_its_retry() {
let mut service = FileService::new(TARGET);
let mut provider = Provider::default();
let start = open(FileOperation::OpenWrite, b"abc");
expect_control(
exchange(&mut service, &mut provider, UART, start),
FileStatus::Ok,
true,
);
for (request, status) in [
(control(FileOperation::Read, 0), FileStatus::InvalidState),
(
control(FileOperation::Close, 0),
FileStatus::InvalidPosition,
),
(data(1, b"bc"), FileStatus::InvalidPosition),
(data(0, b"abc!"), FileStatus::RequestConflict),
(
alter(start, |r| r.checksum_crc32c ^= 1),
FileStatus::RequestConflict,
),
(
alter(control(FileOperation::Close, 0), |r| r.transfer_id += 1),
FileStatus::InvalidTransfer,
),
] {
expect_control(
exchange(&mut service, &mut provider, UART, request),
status,
false,
);
}
assert_eq!(
(provider.writes, provider.closes, provider.aborts),
(0, 0, 0)
);
let accepted = exchange(&mut service, &mut provider, UART, data(0, b"abc"));
expect_control(accepted, FileStatus::Ok, true);
expect_control(
exchange(&mut service, &mut provider, UART, data(0, b"abd")),
FileStatus::RequestConflict,
false,
);
expect_control(
exchange(&mut service, &mut provider, UART, start),
FileStatus::RequestConflict,
false,
);
assert_eq!(
exchange(&mut service, &mut provider, UART, data(0, b"abc")),
accepted
);
expect_control(
exchange(
&mut service,
&mut provider,
UART,
control(FileOperation::Close, 3),
),
FileStatus::Ok,
true,
);
assert_eq!(provider.bytes, b"abc");
assert_eq!(
(provider.writes, provider.closes, provider.aborts),
(1, 1, 0)
);
}
#[test]
fn terminal_failures_refresh_and_replay_only_for_the_accepted_peer() {
for stage in 0..4 {
let mut service = FileService::new(TARGET);
let mut provider = Provider {
bytes: b"abc".to_vec(),
fail_read: stage == 0,
fail_write: stage == 1,
fail_close: stage == 2,
..Provider::default()
};
let operation = if stage == 0 {
FileOperation::OpenRead
} else {
FileOperation::OpenWrite
};
let start = open(operation, b"abc");
expect_control(
exchange(&mut service, &mut provider, UART, start),
FileStatus::Ok,
true,
);
if stage >= 2 {
let bytes = if stage == 3 { b"abd" } else { b"abc" };
expect_control(
exchange(&mut service, &mut provider, UART, data(0, bytes)),
FileStatus::Ok,
true,
);
}
let (request, status) = match stage {
0 => (control(FileOperation::Read, 0), FileStatus::InvalidPosition),
1 => (data(0, b"abc"), FileStatus::InvalidTransfer),
2 => (control(FileOperation::Close, 3), FileStatus::StorageFailure),
_ => (
control(FileOperation::Close, 3),
FileStatus::ChecksumMismatch,
),
};
let lost_reply = exchange(&mut service, &mut provider, UART, request);
expect_control(lost_reply, status, true);
expect_control(
exchange(&mut service, &mut provider, USB, request),
FileStatus::InvalidTransfer,
false,
);
let conflict = match request {
FileRequest::Control(_) => alter(request, |r| r.position += 1),
FileRequest::Data(_) => data(0, b"abd"),
};
expect_control(
exchange(&mut service, &mut provider, UART, conflict),
FileStatus::RequestConflict,
false,
);
assert_eq!(
exchange(&mut service, &mut provider, UART, request),
lost_reply
);
assert_eq!(provider.reads, usize::from(stage == 0));
assert_eq!(provider.writes, usize::from(stage != 0));
assert_eq!(provider.closes, usize::from(stage == 2));
assert_eq!(provider.aborts, 1);
}
}
#[test]
fn failed_open_does_not_claim_or_renew_the_slot_or_evict_a_completed_reply() {
let mut service = FileService::new(TARGET);
let mut provider = Provider {
fail_open: true,
..Provider::default()
};
let start = open(FileOperation::OpenWrite, &[]);
expect_control(
exchange(&mut service, &mut provider, UART, start),
FileStatus::NotFound,
false,
);
provider.fail_open = false;
expect_control(
exchange(&mut service, &mut provider, USB, start),
FileStatus::Ok,
true,
);
let close = control(FileOperation::Close, 0);
let completed = exchange(&mut service, &mut provider, USB, close);
expect_control(completed, FileStatus::Ok, true);
provider.fail_open = true;
expect_control(
exchange(
&mut service,
&mut provider,
USB,
alter(start, |r| r.transfer_id += 1),
),
FileStatus::NotFound,
false,
);
assert_eq!(exchange(&mut service, &mut provider, USB, close), completed);
assert_eq!(provider.closes, 1);
}
firmware/libs/kaptan-flight-config/Cargo.toml
+5 -3
View File
@@ -10,6 +10,8 @@ workspace = true
[dependencies]
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" }
firmware/libs/kaptan-flight-config/src/cbor.rs
-382
View File
@@ -1,382 +0,0 @@
//! Bounded CBOR persistence codec for one explicit current record.
use kaptan_config::{
ConfigModel, ConfigValue, ConfigViolation, FieldWriteError, ValidationRule, ValueKind,
};
use kaptan_err_reportable::KaptanError;
use minicbor::{Decoder, Encoder, encode::Write};
use crate::{CandidateProfile, FORMAT_VERSION, SCHEMA_ID, VehicleCapabilities};
/// Maximum complete candidate snapshot accepted by the current vehicle schema.
pub const MAXIMUM_CANDIDATE_BYTES: usize = 4096;
const FORMAT_KEY: u16 = 0;
const SCHEMA_KEY: u16 = u16::MAX;
/// CBOR snapshot decoding failure.
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
pub enum CborError {
/// The supplied payload exceeded the bounded configuration storage budget.
#[kaptan_error(id = 3001)]
TooLarge,
/// CBOR was malformed or did not match a supported versioned wire record.
#[kaptan_error(id = 3002)]
Malformed,
/// A second CBOR item followed the candidate snapshot.
#[kaptan_error(id = 3003)]
TrailingData,
/// The decoded candidate violated complete schema/capability validation.
#[kaptan_error(id = 3004)]
InvalidProfile(ConfigViolation),
}
/// Failure while encoding one complete candidate snapshot.
///
/// The candidate is validated before the first CBOR item is written, so a
/// caller can never export a locally valid but relationally invalid edit.
#[derive(Debug)]
pub enum CborEncodeError<E> {
/// The complete candidate did not validate against immutable board
/// capabilities.
InvalidProfile(ConfigViolation),
/// The caller-owned CBOR writer rejected the encoded snapshot.
Encode(minicbor::encode::Error<E>),
}
/// Decodes exactly one bounded current record and validates it against immutable
/// board capabilities before returning a candidate to its caller.
pub fn decode_candidate(
bytes: &[u8],
capabilities: &VehicleCapabilities,
) -> Result<CandidateProfile, CborError> {
if bytes.len() > MAXIMUM_CANDIDATE_BYTES {
return Err(CborError::TooLarge);
}
let candidate = decode_current(bytes)?;
candidate
.validate(capabilities)
.map_err(CborError::InvalidProfile)?;
Ok(candidate)
}
/// Validates and encodes one complete current snapshot to a caller-owned CBOR
/// writer.
///
/// Validation happens before creating the encoder, ensuring a failed
/// cross-field or capability check emits no partial CBOR record.
pub fn encode_candidate<W>(
candidate: CandidateProfile,
capabilities: &VehicleCapabilities,
writer: W,
) -> Result<(), CborEncodeError<W::Error>>
where
W: Write,
{
candidate
.validate(capabilities)
.map_err(CborEncodeError::InvalidProfile)?;
let mut encoder = Encoder::new(writer);
encoder
.map(CandidateProfile::FIELD_COUNT as u64 + 2)
.map_err(CborEncodeError::Encode)?;
encoder
.u16(FORMAT_KEY)
.and_then(|encoder| encoder.u16(FORMAT_VERSION))
.map_err(CborEncodeError::Encode)?;
encoder
.u16(SCHEMA_KEY)
.and_then(|encoder| encoder.bytes(&SCHEMA_ID))
.map_err(CborEncodeError::Encode)?;
for descriptor in CandidateProfile::descriptors() {
encoder
.u16(descriptor.id.get())
.map_err(CborEncodeError::Encode)?;
encode_value(
&mut encoder,
candidate
.field_value(descriptor.id)
.expect("descriptor field"),
)
.map_err(CborEncodeError::Encode)?;
}
Ok(())
}
fn decode_current(bytes: &[u8]) -> Result<CandidateProfile, CborError> {
let mut decoder = Decoder::new(bytes);
let entry_count = CandidateProfile::FIELD_COUNT as u64 + 2;
if decoder.map().map_err(|_| CborError::Malformed)? != Some(entry_count) {
return Err(CborError::Malformed);
}
let mut format_version = None;
let mut schema_id = None;
let mut values = [None; CandidateProfile::FIELD_COUNT];
for _ in 0..entry_count {
let key = decoder.u16().map_err(|_| CborError::Malformed)?;
match key {
FORMAT_KEY => {
if format_version
.replace(decoder.u16().map_err(|_| CborError::Malformed)?)
.is_some()
{
return Err(CborError::Malformed);
}
}
SCHEMA_KEY => {
let bytes = decoder.bytes().map_err(|_| CborError::Malformed)?;
let value: [u8; 16] = bytes.try_into().map_err(|_| CborError::Malformed)?;
if schema_id.replace(value).is_some() {
return Err(CborError::Malformed);
}
}
_ => {
let (index, descriptor) = CandidateProfile::descriptors()
.enumerate()
.find(|(_, descriptor)| descriptor.id.get() == key)
.ok_or(CborError::Malformed)?;
if values[index].is_some() {
return Err(CborError::Malformed);
}
values[index] = Some(decode_value(&mut decoder, descriptor.kind)?);
}
}
}
if decoder.position() != bytes.len() {
return Err(CborError::TrailingData);
}
if format_version != Some(FORMAT_VERSION) || schema_id != Some(SCHEMA_ID) {
return Err(CborError::InvalidProfile(ConfigViolation::global(
ValidationRule::UnsupportedFormat,
)));
}
if values.iter().any(Option::is_none) {
return Err(CborError::Malformed);
}
let mut candidate = CandidateProfile::factory();
for (index, value) in values.into_iter().enumerate() {
let descriptor = CandidateProfile::descriptors()
.nth(index)
.ok_or(CborError::Malformed)?;
candidate
.set_field(descriptor.id, value.ok_or(CborError::Malformed)?)
.map_err(field_write_error)?;
}
Ok(candidate)
}
fn encode_value<W>(
encoder: &mut Encoder<W>,
value: ConfigValue,
) -> Result<(), minicbor::encode::Error<W::Error>>
where
W: Write,
{
match value {
ConfigValue::Bool(value) => encoder.bool(value)?,
ConfigValue::U8(value) => encoder.u8(value)?,
ConfigValue::U16(value) => encoder.u16(value)?,
ConfigValue::I32(value) => encoder.i32(value)?,
ConfigValue::U32(value) => encoder.u32(value)?,
ConfigValue::U64(value) => encoder.u64(value)?,
ConfigValue::F32(value) => encoder.f32(value)?,
ConfigValue::F64(value) => encoder.f64(value)?,
};
Ok(())
}
fn decode_value(decoder: &mut Decoder<'_>, kind: ValueKind) -> Result<ConfigValue, CborError> {
match kind {
ValueKind::Bool => decoder.bool().map(ConfigValue::Bool),
ValueKind::U8 => decoder.u8().map(ConfigValue::U8),
ValueKind::U16 => decoder.u16().map(ConfigValue::U16),
ValueKind::I32 => decoder.i32().map(ConfigValue::I32),
ValueKind::U32 => decoder.u32().map(ConfigValue::U32),
ValueKind::U64 => decoder.u64().map(ConfigValue::U64),
ValueKind::F32 => decoder.f32().map(ConfigValue::F32),
ValueKind::F64 => decoder.f64().map(ConfigValue::F64),
}
.map_err(|_| CborError::Malformed)
}
fn field_write_error(error: FieldWriteError) -> CborError {
match error {
FieldWriteError::Violation(issue) => CborError::InvalidProfile(issue),
FieldWriteError::UnknownField(_) | FieldWriteError::WrongType { .. } => {
CborError::Malformed
}
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn round_trip_and_trailing_rejection_are_exact() {
let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]);
let capabilities = VehicleCapabilities::quad_x();
encode_candidate(CandidateProfile::factory(), &capabilities, &mut writer).unwrap();
let length = writer.position();
let bytes = writer.into_inner();
assert!(decode_candidate(&bytes[..length], &capabilities).is_ok());
let mut trailing = bytes;
trailing[length] = 0xf6;
assert!(matches!(
decode_candidate(&trailing[..=length], &capabilities),
Err(CborError::TrailingData)
));
}
#[test]
fn encoding_rejects_a_cross_field_invalid_candidate_before_writing() {
let capabilities = VehicleCapabilities::quad_x();
let mut candidate = CandidateProfile::factory();
candidate
.set_field(kaptan_config::FieldId::new(2), ConfigValue::U64(50_001))
.expect("the scalar is locally valid");
let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]);
assert!(matches!(
encode_candidate(candidate, &capabilities, &mut writer),
Err(CborEncodeError::InvalidProfile(violation))
if violation.rule() == ValidationRule::Inconsistent
));
assert_eq!(writer.position(), 0);
}
#[test]
fn rejects_wrong_format_and_schema_before_candidate_conversion() {
let capabilities = VehicleCapabilities::quad_x();
let (bytes, length) = encoded_test_record(FORMAT_VERSION + 1, SCHEMA_ID, |key| key);
assert!(matches!(
decode_candidate(&bytes[..length], &capabilities),
Err(CborError::InvalidProfile(issue))
if issue.rule() == ValidationRule::UnsupportedFormat
));
let (bytes, length) = encoded_test_record(FORMAT_VERSION, [0; 16], |key| key);
assert!(matches!(
decode_candidate(&bytes[..length], &capabilities),
Err(CborError::InvalidProfile(issue))
if issue.rule() == kaptan_config::ValidationRule::UnsupportedFormat
));
}
#[test]
fn exact_map_rejects_unknown_duplicate_and_missing_fields() {
let capabilities = VehicleCapabilities::quad_x();
let (bytes, length) = encoded_test_record(FORMAT_VERSION, SCHEMA_ID, |key| key);
assert!(decode_candidate(&bytes[..length], &capabilities).is_ok());
let (unknown, length) =
encoded_test_record(
FORMAT_VERSION,
SCHEMA_ID,
|key| {
if key == 1 { 25 } else { key }
},
);
assert!(matches!(
decode_candidate(&unknown[..length], &capabilities),
Err(CborError::Malformed)
));
let (duplicate, length) = encoded_test_record(FORMAT_VERSION, SCHEMA_ID, |key| {
if key == 1 { FORMAT_KEY } else { key }
});
assert!(matches!(
decode_candidate(&duplicate[..length], &capabilities),
Err(CborError::Malformed)
));
let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]);
let mut encoder = Encoder::new(&mut writer);
encoder
.map(CandidateProfile::FIELD_COUNT as u64 + 1)
.unwrap();
encoder
.u16(FORMAT_KEY)
.unwrap()
.u16(FORMAT_VERSION)
.unwrap();
encoder.u16(SCHEMA_KEY).unwrap().bytes(&SCHEMA_ID).unwrap();
let candidate = CandidateProfile::factory();
for descriptor in CandidateProfile::descriptors().skip(1) {
encoder.u16(descriptor.id.get()).unwrap();
encode_value(&mut encoder, candidate.field_value(descriptor.id).unwrap()).unwrap();
}
let length = writer.position();
let missing = writer.into_inner();
assert!(matches!(
decode_candidate(&missing[..length], &capabilities),
Err(CborError::Malformed)
));
}
#[test]
fn cbor_scalar_write_preserves_the_generated_field_and_rule() {
let capabilities = VehicleCapabilities::quad_x();
let mass = CandidateProfile::descriptors()
.find(|descriptor| descriptor.display_key == "thrust.mass_kg")
.expect("declared mass descriptor");
let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]);
let mut encoder = Encoder::new(&mut writer);
encoder
.map(CandidateProfile::FIELD_COUNT as u64 + 2)
.unwrap();
encoder
.u16(FORMAT_KEY)
.unwrap()
.u16(FORMAT_VERSION)
.unwrap();
encoder.u16(SCHEMA_KEY).unwrap().bytes(&SCHEMA_ID).unwrap();
let candidate = CandidateProfile::factory();
for descriptor in CandidateProfile::descriptors() {
encoder.u16(descriptor.id.get()).unwrap();
let value = if descriptor.id == mass.id {
ConfigValue::F32(0.009)
} else {
candidate.field_value(descriptor.id).unwrap()
};
encode_value(&mut encoder, value).unwrap();
}
let length = writer.position();
let bytes = writer.into_inner();
assert!(matches!(
decode_candidate(&bytes[..length], &capabilities),
Err(CborError::InvalidProfile(issue))
if issue.primary() == Some(mass.id)
&& issue.related_field().is_none()
&& issue.rule() == ValidationRule::BelowMinimum
));
}
fn encoded_test_record<F>(
format_version: u16,
schema_id: [u8; 16],
mut remap_key: F,
) -> ([u8; MAXIMUM_CANDIDATE_BYTES], usize)
where
F: FnMut(u16) -> u16,
{
let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]);
let mut encoder = Encoder::new(&mut writer);
encoder
.map(CandidateProfile::FIELD_COUNT as u64 + 2)
.unwrap();
encoder
.u16(FORMAT_KEY)
.unwrap()
.u16(format_version)
.unwrap();
encoder.u16(SCHEMA_KEY).unwrap().bytes(&schema_id).unwrap();
let candidate = CandidateProfile::factory();
for descriptor in CandidateProfile::descriptors() {
encoder.u16(remap_key(descriptor.id.get())).unwrap();
encode_value(&mut encoder, candidate.field_value(descriptor.id).unwrap()).unwrap();
}
let length = writer.position();
(writer.into_inner(), length)
}
}
firmware/libs/kaptan-flight-config/src/lib.rs
+8 -6
View File
@@ -1,17 +1,19 @@
//! Vehicle configuration schema and CBOR snapshot codec.
//! Validated configuration for the portable flight runtime.
#![no_std]
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;
firmware/libs/kaptan-flight-config/src/profile/active.rs
+81 -197
View File
@@ -1,30 +1,28 @@
//! Immutable configuration projections consumed by runtime owners.
/// Immutable 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,
}
}
}
firmware/libs/kaptan-flight-config/src/profile/candidate.rs
+171 -264
View File
@@ -1,93 +1,55 @@
//! Private vehicle candidate and whole-profile validation boundary.
//! Editable vehicle candidate and whole-profile validation boundary.
use kaptan_config::{
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()
);
}
}
}
firmware/libs/kaptan-flight-config/src/profile/control.rs
+85 -48
View File
@@ -2,28 +2,10 @@ use kaptan_config::{Config, ConfigViolation, ValidationRule};
use super::ids;
#[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(
firmware/libs/kaptan-flight-config/src/profile/flight.rs
+88
View File
@@ -0,0 +1,88 @@
//! Restart-only flight policy and continuous process-noise densities.
use kaptan_config::Config;
#[derive(Clone, Copy, Debug, PartialEq, Config)]
pub struct FlightSettings {
#[config(
id = 30,
key = "flight.command_ttl_us",
unit = "us",
range(min = 1, max = 250_000)
)]
pub command_ttl_us: u64,
#[config(
id = 31,
key = "flight.fallback_hold_us",
unit = "us",
range(min = 1, max = 1_000_000)
)]
pub fallback_hold_us: u64,
#[config(
id = 32,
key = "flight.output_lifetime_us",
unit = "us",
range(min = 10_000, max = 50_000)
)]
pub output_lifetime_us: u64,
#[config(
id = 33,
key = "flight.control_period_us",
unit = "us",
range(min = 10_000, max = 10_000)
)]
pub control_period_us: u64,
#[config(
id = 34,
key = "flight.control_deadline_us",
unit = "us",
range(min = 20_000, max = 20_000)
)]
pub control_deadline_us: u64,
#[config(
id = 35,
key = "navigation.time_error_bound_us",
unit = "us",
range(min = 0, max = 2_000)
)]
pub time_error_bound_us: u64,
#[config(
id = 36,
key = "navigation.gyro_psd",
unit = "rad2/s",
finite,
range(min = 1e-12, max = 0.01)
)]
pub gyro_psd: f32,
#[config(
id = 37,
key = "navigation.acceleration_psd",
unit = "m2/s3",
finite,
range(min = 1e-12, max = 0.1)
)]
pub acceleration_psd: f32,
#[config(
id = 38,
key = "navigation.gyro_bias_psd",
unit = "rad2/s3",
finite,
range(min = 1e-14, max = 0.01)
)]
pub gyro_bias_psd: f32,
#[config(
id = 39,
key = "navigation.acceleration_bias_psd",
unit = "m2/s5",
finite,
range(min = 1e-14, max = 0.1)
)]
pub acceleration_bias_psd: f32,
#[config(
id = 40,
key = "navigation.barometric_datum_psd",
unit = "m2/s",
finite,
range(min = 1e-12, max = 0.1)
)]
pub barometric_datum_psd: f32,
}
firmware/libs/kaptan-flight-config/src/profile/ids.rs
+16 -16
View File
@@ -1,20 +1,20 @@
//! Symbolic projections used by relation checks and focused tests.
//!
//! 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;
firmware/libs/kaptan-flight-config/src/profile/mavlink_logging.rs
-16
View File
@@ -1,16 +0,0 @@
//! Persisted, restart-only MAVLink diagnostic-sink policy.
use kaptan_config::Config;
use kaptan_log::LevelFilter;
/// Candidate policy for the MAVLink diagnostic sink.
#[derive(Clone, Copy, Config)]
pub(super) struct MavlinkLoggingCandidate {
#[config(
enum,
id = 25,
key = "mavlink.logging.level_filter",
unit = "log_level"
)]
pub(super) level_filter: LevelFilter,
}
firmware/libs/kaptan-flight-config/src/profile/mod.rs
+7 -6
View File
@@ -3,13 +3,14 @@
mod active;
mod 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;
firmware/libs/kaptan-flight-config/src/profile/safety.rs
+4 -3
View File
@@ -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,
}
firmware/libs/kaptan-flight-config/src/profile/timing.rs
+19 -8
View File
@@ -2,9 +2,10 @@ use kaptan_config::{Config, ConfigViolation, ValidationRule};
use super::ids;
#[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,
firmware/libs/kaptan-flight-config/tests/control_domain.rs
+275
View File
@@ -0,0 +1,275 @@
mod support;
use kaptan_config::{ConfigModel, ConfigValue, ConfigViolation, FieldId, ValidationRule};
use kaptan_control::{
HeadingNed, KinematicLimits, Pid, PidConfig, ThrustEnvelope, Trajectory,
TranslationalControlConfig, TranslationalController, TranslationalEstimate, force_to_attitude,
};
use kaptan_flight_config::{CandidateProfile, VehicleCapabilities};
use kaptan_navigation::{GRAVITY_MPS2, MAXIMUM_FREE_ACCELERATION_MPS2, PositionNed, VelocityNed};
const CAPABILITIES: VehicleCapabilities = support::capabilities();
const IMU_INTERVAL: &str = "timing.imu_max_interval_us";
const IMU_FRESHNESS: &str = "timing.imu_freshness_us";
const DOWNWARD_ACCELERATION: &str = "control.maximum_downward_acceleration_mps2";
fn id(key: &str) -> FieldId {
CandidateProfile::descriptors()
.find(|field| field.display_key == key)
.unwrap()
.id
}
fn set(candidate: &mut CandidateProfile, key: &str, value: ConfigValue) {
candidate.set_field(id(key), value).unwrap();
}
fn with_interval(microseconds: u64) -> CandidateProfile {
let mut candidate = support::candidate();
set(&mut candidate, IMU_INTERVAL, ConfigValue::U64(microseconds));
set(&mut candidate, IMU_FRESHNESS, ConfigValue::U64(20_000));
candidate
}
fn assert_rejected(candidate: CandidateProfile, issue: ConfigViolation) {
// Both whole-tree validation and active-profile construction must reject.
assert_eq!(
<CandidateProfile as kaptan_config::Config>::validate(&candidate),
Err(issue)
);
assert_eq!(candidate.validate(&CAPABILITIES), Err(issue));
}
#[test]
fn imu_interval_cannot_exceed_the_controller_domain() {
// The generic primitive still permits 100 ms, but this flight profile's
// delayed estimator and consumer require 10..20 ms inertial intervals.
assert_eq!(kaptan_control::MAXIMUM_CONTROL_STEP_MICROSECONDS, 100_000);
for interval in [10_000, 15_000, 19_999, 20_000] {
let active = with_interval(interval).validate(&CAPABILITIES).unwrap();
assert_eq!(active.timing().maximum_imu_interval_microseconds, interval);
}
for interval in [
1, 9_999, 20_001, 50_000, 100_000, 100_001, 200_000, 1_000_000,
] {
let candidate = with_interval(interval);
assert!(candidate.validate(&CAPABILITIES).is_err());
assert!(<CandidateProfile as kaptan_config::Config>::validate(&candidate).is_err());
}
}
#[test]
fn downward_acceleration_has_a_strict_gravity_boundary() {
let below = f32::from_bits(GRAVITY_MPS2.to_bits() - 1);
let above = f32::from_bits(GRAVITY_MPS2.to_bits() + 1);
for acceleration in [0.001, 2.0, below] {
let mut candidate = support::candidate();
set(
&mut candidate,
DOWNWARD_ACCELERATION,
ConfigValue::F32(acceleration),
);
let active = candidate.validate(&CAPABILITIES).unwrap();
assert_eq!(
active.control().maximum_downward_acceleration_mps2,
acceleration
);
}
for acceleration in [GRAVITY_MPS2, above, 100.0] {
let mut candidate = support::candidate();
set(
&mut candidate,
DOWNWARD_ACCELERATION,
ConfigValue::F32(acceleration),
);
assert_rejected(
candidate,
ConfigViolation::field(id(DOWNWARD_ACCELERATION), ValidationRule::Inconsistent),
);
}
}
#[test]
fn thrust_margin_uses_the_same_gravity_contract() {
for thrust in [f32::from_bits(GRAVITY_MPS2.to_bits() - 1), GRAVITY_MPS2] {
let mut candidate = support::candidate();
set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(1.0));
set(
&mut candidate,
"thrust.maximum_collective_thrust_n",
ConfigValue::F32(thrust),
);
assert_rejected(
candidate,
ConfigViolation::related(
id("thrust.maximum_collective_thrust_n"),
id("thrust.mass_kg"),
ValidationRule::Inconsistent,
),
);
}
let mut candidate = support::candidate();
set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(1.0));
set(
&mut candidate,
"thrust.maximum_collective_thrust_n",
ConfigValue::F32(f32::from_bits(GRAVITY_MPS2.to_bits() + 1)),
);
assert!(candidate.validate(&CAPABILITIES).is_ok());
}
#[test]
fn thrust_to_mass_certifies_navigation_free_acceleration_without_epsilon() {
let issue = ConfigViolation::related(
id("thrust.maximum_collective_thrust_n"),
id("thrust.mass_kg"),
ValidationRule::Inconsistent,
);
let mut former = support::candidate();
set(&mut former, "thrust.mass_kg", ConfigValue::F32(0.01));
assert_rejected(former, issue);
assert!(support::candidate().validate(&CAPABILITIES).is_ok());
let acceleration = f64::from(MAXIMUM_FREE_ACCELERATION_MPS2) - f64::from(GRAVITY_MPS2);
for mass in [0.01_f32, 1.0, 1.2, 100.0] {
let exact = f64::from(mass) * acceleration;
let rounded = exact as f32;
let floor = if f64::from(rounded) > exact {
rounded.next_down()
} else {
rounded
};
let mut candidate = support::candidate();
set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(mass));
for thrust in [floor.next_down(), floor] {
set(
&mut candidate,
"thrust.maximum_collective_thrust_n",
ConfigValue::F32(thrust),
);
assert!(candidate.validate(&CAPABILITIES).is_ok());
}
set(
&mut candidate,
"thrust.maximum_collective_thrust_n",
ConfigValue::F32(floor.next_up()),
);
assert_rejected(candidate, issue);
}
// At m=1 the upper boundary is exactly representable in binary32.
let exact_thrust = MAXIMUM_FREE_ACCELERATION_MPS2 - GRAVITY_MPS2;
assert_eq!(f64::from(exact_thrust), acceleration);
let mut candidate = support::candidate();
set(
&mut candidate,
"thrust.maximum_collective_thrust_n",
ConfigValue::F32(exact_thrust),
);
set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(1.0));
assert!(candidate.validate(&CAPABILITIES).is_ok());
set(
&mut candidate,
"thrust.mass_kg",
ConfigValue::F32(1.0_f32.next_down()),
);
assert_rejected(candidate, issue);
set(
&mut candidate,
"thrust.mass_kg",
ConfigValue::F32(1.0_f32.next_up()),
);
assert!(candidate.validate(&CAPABILITIES).is_ok());
}
#[test]
fn accepted_boundary_profiles_operate_the_real_control_primitives() {
for mass in [0.01, 1.0] {
let mut candidate = with_interval(20_000);
let downward_limit = f32::from_bits(GRAVITY_MPS2.to_bits() - 1);
set(
&mut candidate,
DOWNWARD_ACCELERATION,
ConfigValue::F32(downward_limit),
);
set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(mass));
set(
&mut candidate,
"thrust.maximum_collective_thrust_n",
ConfigValue::F32(15.0 * mass),
);
for (key, gain) in [
("control.rate_pid.proportional", 0.1),
("control.rate_pid.integral", 0.02),
("control.rate_pid.derivative", 0.05),
] {
set(&mut candidate, key, ConfigValue::F32(gain));
}
let active = candidate.validate(&CAPABILITIES).unwrap();
assert_eq!(active.timing().maximum_imu_interval_microseconds, 20_000);
// Preserve the independent 100-ms known answers of the generic
// primitives, without claiming it is a valid flight-estimator cadence.
let dt = 0.1;
let gains = active.rate_pid();
let mut pid = Pid::new(PidConfig {
proportional: gains.proportional,
integral: gains.integral,
derivative: gains.derivative,
output_limit: gains.output_limit,
integral_limit: gains.integral_limit,
})
.unwrap();
// Known answer with all three terms active at the 100-ms boundary.
assert!((pid.update(0.2, 0.3, dt).unwrap() - 0.0354).abs() < 1.0e-7);
let mut trajectory = Trajectory::new(
PositionNed::metres(0.0, 0.0, 0.0).vector(),
KinematicLimits {
maximum_horizontal_speed_mps: 1.0,
maximum_vertical_speed_mps: 1.0,
maximum_horizontal_acceleration_mps2: 1.0,
maximum_vertical_acceleration_mps2: 1.0,
},
)
.unwrap();
trajectory
.set_goal(PositionNed::metres(0.0, 0.0, 1.0).vector())
.unwrap();
let reference = trajectory.update(dt).unwrap();
assert!((reference.position.z() - 0.01).abs() < 1.0e-8);
let control = active.control();
let controller = TranslationalController::new(TranslationalControlConfig {
position_gain_per_second_squared: control.position_gain_per_second_squared,
velocity_gain_per_second: control.velocity_gain_per_second,
maximum_horizontal_acceleration_mps2: control.maximum_horizontal_acceleration_mps2,
maximum_upward_acceleration_mps2: control.maximum_upward_acceleration_mps2,
maximum_downward_acceleration_mps2: control.maximum_downward_acceleration_mps2,
})
.unwrap();
let acceleration = controller
.update(
reference,
TranslationalEstimate {
position: PositionNed::metres(0.0, 0.0, -100.0).vector(),
velocity: VelocityNed::metres_per_second(0.0, 0.0, 0.0).vector(),
},
)
.unwrap();
assert_eq!(acceleration.z(), downward_limit);
let thrust = active.thrust();
let envelope = ThrustEnvelope {
mass_kg: thrust.mass_kg,
maximum_collective_thrust_newtons: thrust.maximum_collective_thrust_newtons,
maximum_tilt_tangent: thrust.maximum_tilt_tangent,
};
let (force, saturated) = envelope
.force_for_acceleration(acceleration, GRAVITY_MPS2)
.unwrap();
// Gravity is in [8, 16), so its f32 predecessor is exactly 2^-20 lower.
assert_eq!(force.vector().z(), -mass / 1_048_576.0);
assert!(!saturated);
let attitude = force_to_attitude(force, HeadingNed::new(1.0, 0.0).unwrap()).unwrap();
assert_eq!(attitude.w(), 1.0);
}
}
firmware/libs/kaptan-flight-config/tests/support/mod.rs
+96
View File
@@ -0,0 +1,96 @@
use kaptan_flight_config::profile::*;
use kaptan_math::Vector3;
use kaptan_navigation::{
BarometricReference, CalibrationRevision, ContactGeometry, FrameId, NavigationInstallation,
NavigationSources, RangeInstallation, SensorReferenceId, SourceId,
};
pub const fn candidate() -> CandidateProfile {
CandidateProfile {
safety: SafetySettings {
prearm_dwell_microseconds: 500_000,
},
timing: TimingSettings {
maximum_imu_interval_microseconds: 20_000,
imu_freshness_microseconds: 20_000,
local_state_freshness_microseconds: 250_000,
offboard_timeout_microseconds: 300_000,
},
control: ControlSettings {
position_gain_per_second_squared: 0.8,
velocity_gain_per_second: 1.2,
maximum_horizontal_acceleration_mps2: 3.0,
maximum_upward_acceleration_mps2: 2.0,
maximum_downward_acceleration_mps2: 2.0,
attitude_roll_pitch_gain_per_second: 4.0,
attitude_yaw_gain_per_second: 2.0,
},
thrust: ThrustSettings {
mass_kg: 1.2,
maximum_collective_thrust_newtons: 18.0,
maximum_tilt_tangent: 0.700_207_53,
},
rate_pid: PidSettings {
proportional: 0.080,
integral: 0.006,
derivative: 0.0,
output_limit: 0.10,
integral_limit: 0.025,
yaw_proportional: 0.990,
yaw_integral: 0.074,
},
actuator_limits: ActuatorLimits {
minimum: 0.0,
maximum: 1.0,
slew_per_second: 8.0,
},
flight: FlightSettings {
command_ttl_us: 250_000,
fallback_hold_us: 1_000_000,
output_lifetime_us: 50_000,
control_period_us: 10_000,
control_deadline_us: 20_000,
time_error_bound_us: 2_000,
gyro_psd: 2.5e-7,
acceleration_psd: 2.5e-5,
gyro_bias_psd: 1e-10,
acceleration_bias_psd: 1e-8,
barometric_datum_psd: 1e-5,
},
}
}
pub const fn installation() -> NavigationInstallation {
NavigationInstallation {
sources: NavigationSources {
imu: SourceId::new(1),
local_position_velocity: SourceId::new(2),
magnetic: SourceId::new(3),
pressure: SourceId::new(4),
range: SourceId::new(5),
},
range: RangeInstallation {
frame: FrameId::new(3),
offset_frd_metres: Vector3::new(0.0, 0.0, 0.04),
direction_frd: Vector3::new(0.0, 0.0, 1.0),
quantization_error_metres: 0.0005,
},
contact: ContactGeometry {
half_span_x_metres: 0.12,
half_span_y_metres: 0.12,
centre_z_metres: 0.065,
radius_metres: 0.015,
},
atmosphere: BarometricReference {
pressure_pascals: 101_325.0,
temperature_kelvin: 288.15,
},
magnetic_field_ned_microtesla: Vector3::new(20.0, 0.0, 45.0),
calibration: CalibrationRevision::new(1),
reference: SensorReferenceId(1),
}
}
pub const fn capabilities() -> VehicleCapabilities {
VehicleCapabilities::quad_x(installation())
}
firmware/libs/kaptan-flight-runtime/Cargo.toml
+8 -2
View File
@@ -9,6 +9,12 @@ version.workspace = true
workspace = true
[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" }
firmware/libs/kaptan-flight-runtime/src/controller.rs
+214
View File
@@ -0,0 +1,214 @@
use kaptan_control::{
ActuatorLimits, Allocator, BodyRate, BodyWrench, HeadingNed, KinematicReference, MixerOutput,
PidConfig, PrecomputedMixer, RatePid, ThrustEnvelope, TranslationalControlConfig,
TranslationalController, TranslationalEstimate, force_to_attitude,
};
use kaptan_flight_config::VehicleProfile;
use kaptan_math::{Frd, Ned, UnitQuaternion, Vector3};
use kaptan_navigation::GRAVITY_MPS2;
use crate::{FlightEstimate, RuntimeConfigError};
pub(crate) struct QuadController {
translational: TranslationalController,
thrust: ThrustEnvelope,
rate: RatePid,
allocator: Allocator<4>,
heading: Vector3<Ned>,
attitude_gain_per_second: [f32; 3],
}
impl QuadController {
pub fn new(config: VehicleProfile) -> Result<Self, RuntimeConfigError> {
let control = config.control();
let thrust = config.thrust();
let rate = config.rate_pid();
let limits = config.actuator_limits();
let translational = TranslationalController::new(TranslationalControlConfig {
position_gain_per_second_squared: control.position_gain_per_second_squared,
velocity_gain_per_second: control.velocity_gain_per_second,
maximum_horizontal_acceleration_mps2: control.maximum_horizontal_acceleration_mps2,
maximum_upward_acceleration_mps2: control.maximum_upward_acceleration_mps2,
maximum_downward_acceleration_mps2: control.maximum_downward_acceleration_mps2,
})
.map_err(|_| RuntimeConfigError::Control)?;
let thrust = ThrustEnvelope {
mass_kg: thrust.mass_kg,
maximum_collective_thrust_newtons: thrust.maximum_collective_thrust_newtons,
maximum_tilt_tangent: thrust.maximum_tilt_tangent,
};
thrust.validate().map_err(|_| RuntimeConfigError::Control)?;
let yaw_gains = (rate.yaw_proportional, rate.yaw_integral);
let rate = PidConfig {
proportional: rate.proportional,
integral: rate.integral,
derivative: rate.derivative,
output_limit: rate.output_limit,
integral_limit: rate.integral_limit,
};
let limits = ActuatorLimits {
minimum: limits.minimum,
maximum: limits.maximum,
slew_per_second: limits.slew_per_second,
};
let mixer = PrecomputedMixer::new(config.mixer().map(|coefficients| MixerOutput {
coefficients,
limits,
}));
Ok(Self {
translational,
thrust,
rate: RatePid::new(
rate,
rate,
PidConfig {
proportional: yaw_gains.0,
integral: yaw_gains.1,
..rate
},
)
.map_err(|_| RuntimeConfigError::Control)?,
allocator: Allocator::new(mixer).map_err(|_| RuntimeConfigError::Control)?,
heading: Vector3::new(1., 0., 0.),
attitude_gain_per_second: [
control.attitude_roll_pitch_gain_per_second,
control.attitude_roll_pitch_gain_per_second,
control.attitude_yaw_gain_per_second,
],
})
}
pub fn update(
&mut self,
state: FlightEstimate,
mut reference: KinematicReference,
dt_seconds: f32,
yaw: Option<f32>,
position_feedback: [bool; 3],
collective_cap: f32,
) -> Result<[f32; 4], ()> {
let measured_position = state.position.vector();
reference.position = Vector3::new(
if position_feedback[0] {
reference.position.x()
} else {
measured_position.x()
},
if position_feedback[1] {
reference.position.y()
} else {
measured_position.y()
},
if position_feedback[2] {
reference.position.z()
} else {
measured_position.z()
},
);
let acceleration = self
.translational
.update(
reference,
TranslationalEstimate {
position: state.position.vector(),
velocity: state.velocity.vector(),
},
)
.map_err(|_| ())?;
let (force, limited) = self
.thrust
.force_for_acceleration(acceleration, GRAVITY_MPS2)
.map_err(|_| ())?;
if let Some(yaw) = yaw {
let half = UnitQuaternion::<Ned, Ned>::from_small_rotation_vector(Vector3::new(
0.,
0.,
yaw * 0.5,
))
.map_err(|_| ())?;
let goal = half
.compose(half)
.map_err(|_| ())?
.rotate(Vector3::new(1., 0., 0.));
let sign = if self.heading.cross(goal).z() < 0. {
-1.
} else {
1.
};
let step = UnitQuaternion::<Ned, Ned>::from_small_rotation_vector(Vector3::new(
0.,
0.,
sign * dt_seconds,
))
.map_err(|_| ())?;
let cosine = step.rotate(Vector3::new(1., 0., 0.)).x();
self.heading = if self.heading.dot(goal) >= cosine {
goal
} else {
step.rotate(self.heading)
};
}
let heading = HeadingNed::new(self.heading.x(), self.heading.y()).ok_or(())?;
let target = force_to_attitude(force, heading).map_err(|_| ())?;
let error = state.attitude.inverse().compose(target).map_err(|_| ())?;
let direction = if error.w() < 0.0 { -1.0 } else { 1.0 };
let target_rate = BodyRate::radians_per_second(
direction * 2.0 * self.attitude_gain_per_second[0] * error.x(),
direction * 2.0 * self.attitude_gain_per_second[1] * error.y(),
direction * 2.0 * self.attitude_gain_per_second[2] * error.z(),
);
let torque = self
.rate
.update(
target_rate,
BodyRate::radians_per_second(
state.angular_rate.vector().x(),
state.angular_rate.vector().y(),
state.angular_rate.vector().z(),
),
dt_seconds,
)
.map_err(|_| ())?;
let allocation = self
.allocator
.allocate_capped(
BodyWrench {
roll: torque.x(),
pitch: torque.y(),
yaw: torque.z(),
collective: (force.vector().norm()
/ self.thrust.maximum_collective_thrust_newtons)
.min(collective_cap),
},
dt_seconds,
collective_cap * 4.,
)
.map_err(|_| ())?;
if limited
|| allocation.saturated
|| force.vector().norm() / self.thrust.maximum_collective_thrust_newtons
> collective_cap
{
self.rate.reset_integrators();
}
Ok(allocation.commands)
}
pub fn reset(&mut self) {
self.rate.reset();
self.allocator.reset_commands();
}
pub fn capture_heading(&mut self, attitude: UnitQuaternion<Frd, Ned>) -> Result<(), ()> {
let forward = attitude.body_to_ned(Vector3::new(1., 0., 0.));
self.heading = Vector3::new(forward.x(), forward.y(), 0.)
.normalized()
.ok_or(())?;
Ok(())
}
pub fn heading_aligned(&self, attitude: UnitQuaternion<Frd, Ned>) -> bool {
let forward = attitude.body_to_ned(Vector3::new(1., 0., 0.));
Vector3::new(forward.x(), forward.y(), 0.)
.normalized()
.is_some_and(|v| v.dot(self.heading) >= 0.996_194_7)
}
}
firmware/libs/kaptan-flight-runtime/src/execution.rs
+2147
View File
File diff suppressed because it is too large. Load diff
firmware/libs/kaptan-flight-runtime/src/lib.rs
+339 -134
View File
@@ -1,148 +1,353 @@
//! Central, transport-neutral flight-runtime composition.
//! Deterministic sensor-driven flight endpoint. The caller owns the clock and
//! borrows its bounded output sink for each effectful call. Local publication
//! never proves remote application. Consumers enforce their own absolute expiry.
//!
//! This crate owns the one configuration manager and the bounded queue for the
//! 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,
})
}
/// 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);
}
/// 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)
}
}
#[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)
}
fn persist(
&mut self,
_: &kaptan_flight_config::CandidateProfile,
) -> Result<(), Self::Error> {
Ok(())
impl RuntimeEpoch {
pub const fn from_parts(session: TransportSessionId, generation: u64) -> Self {
Self {
session,
generation,
}
}
#[test]
fn runtime_uses_the_active_mavlink_policy_and_owns_one_sink_queue() {
let _guard = LOG_TEST.lock().unwrap();
while kaptan_log::drain_one(|_| ()).is_some() {}
let mut runtime = FlightRuntime::recover(
kaptan_flight_config::CandidateProfile::factory(),
kaptan_flight_config::VehicleCapabilities::quad_x(),
Store,
)
.expect("factory configuration validates");
assert_eq!(
runtime.configuration().active().mavlink_logging(),
kaptan_log::LevelFilter::Info
);
kaptan_log::info!("runtime ready");
runtime.publish_log_event(kaptan_log::drain_one(|record| *record).unwrap());
assert_eq!(
runtime.submit_next_mavlink(|record| {
assert_eq!(record.level(), kaptan_log::Level::Info);
(TransportAdmission::Accepted, ())
}),
Some(())
);
pub const fn session(self) -> TransportSessionId {
self.session
}
#[test]
fn disabled_mavlink_sink_filters_every_level() {
let _guard = LOG_TEST.lock().unwrap();
while kaptan_log::drain_one(|_| ()).is_some() {}
let mut factory = kaptan_flight_config::CandidateProfile::factory();
let level_filter = kaptan_flight_config::CandidateProfile::descriptors()
.find(|descriptor| descriptor.display_key == "mavlink.logging.level_filter")
.expect("MAVLink log-level descriptor");
factory
.set_field(level_filter.id, ConfigValue::U8(0))
.unwrap();
let mut runtime = FlightRuntime::recover(
factory,
kaptan_flight_config::VehicleCapabilities::quad_x(),
Store,
)
.expect("disabled MAVLink sink is valid");
assert_eq!(
runtime.configuration().active().mavlink_logging(),
kaptan_log::LevelFilter::Off
);
kaptan_log::error!("not routed to MAVLink");
runtime.publish_log_event(kaptan_log::drain_one(|record| *record).unwrap());
assert_eq!(
runtime.submit_next_mavlink(|_| (TransportAdmission::Accepted, ())),
None
);
pub const fn generation(self) -> u64 {
self.generation
}
}
#[derive(Clone, Copy, Debug, Eq, PartialEq, Ord, PartialOrd)]
pub struct RequestSequence(pub u64);
impl RequestSequence {
pub const fn checked_next(self) -> Option<Self> {
match self.0.checked_add(1) {
Some(v) => Some(Self(v)),
None => None,
}
}
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct ArmId(pub u64);
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct RunBinding {
pub session: TransportSessionId,
pub owner: CommandOwner,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct RequestContext {
pub epoch: RuntimeEpoch,
pub owner: CommandOwner,
pub sequence: RequestSequence,
pub issued_at: Timestamp,
pub expires_at: Timestamp,
pub time_uncertainty_microseconds: u32,
pub arm: Option<ArmId>,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum LocalSetpoint {
PositionYaw {
position: PositionNed,
yaw_radians: f32,
},
VelocityYaw {
velocity: VelocityNed,
yaw_radians: f32,
},
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum FlightCommand {
Arm,
GroundDisarm,
Takeoff {
height_above_launch_metres: f32,
},
Hold,
EnterOffboard {
origin: OriginId,
initial: LocalSetpoint,
},
Land,
Terminate,
/// Take off, hold for 30 seconds after settling, then land and inhibit output.
TakeoffHoverLand {
height_above_launch_metres: f32,
},
}
impl FlightCommand {
pub const fn kind(self) -> CommandKind {
match self {
Self::Arm => CommandKind::Arm,
Self::GroundDisarm => CommandKind::GroundDisarm,
Self::Takeoff { .. } => CommandKind::Takeoff,
Self::Hold => CommandKind::Hold,
Self::EnterOffboard { .. } => CommandKind::EnterOffboard,
Self::Land => CommandKind::Land,
Self::Terminate => CommandKind::Terminate,
Self::TakeoffHoverLand { .. } => CommandKind::TakeoffHoverLand,
}
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct CommandRequest {
pub context: RequestContext,
pub command: FlightCommand,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct SetpointRequest {
pub epoch: RuntimeEpoch,
pub owner: CommandOwner,
pub arm: ArmId,
pub origin: OriginId,
pub offboard: RequestSequence,
pub sequence: RequestSequence,
pub issued_at: Timestamp,
pub time_uncertainty_microseconds: u32,
pub setpoint: LocalSetpoint,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
#[repr(u8)]
pub enum CommandKind {
Arm = 1,
GroundDisarm = 2,
Takeoff = 3,
Hold = 4,
EnterOffboard = 5,
Land = 6,
Terminate = 7,
TakeoffHoverLand = 8,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
#[repr(u8)]
pub enum CommandLifecycle {
Rejected,
Accepted,
Completed,
Cancelled,
Failed,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
#[repr(u16)]
pub enum CommandReason {
None = 0,
WrongEpoch = 1,
WrongOwner = 2,
Replay = 3,
SequenceExhausted = 4,
InvalidTime = 5,
Expired = 6,
InvalidValue = 7,
NotReady = 8,
WrongPhase = 9,
WrongArm = 10,
WrongOrigin = 11,
WrongOffboard = 12,
MissingCapability = 13,
FallbackLatched = 14,
AlreadyActive = 15,
Superseded = 16,
PhaseDeadline = 17,
Terminated = 18,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct CommandStatus {
pub epoch: RuntimeEpoch,
pub owner: CommandOwner,
pub sequence: RequestSequence,
pub kind: CommandKind,
pub lifecycle: CommandLifecycle,
pub at: Timestamp,
pub reason: CommandReason,
pub arm: Option<ArmId>,
}
/// A committed terminal transition from the most recent effectful call.
/// The adapter must reconcile these records even when that call returned Err.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct CommandOutcome {
pub status: CommandStatus,
pub published: bool,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct SetpointStatus {
pub offboard: RequestSequence,
pub sequence: RequestSequence,
pub accepted: bool,
pub reason: CommandReason,
pub expires_at: Option<Timestamp>,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
#[repr(u8)]
pub enum VehiclePhase {
Disarmed,
PreArm,
ArmedIdle,
Takeoff,
Hold,
Offboard,
FallbackHold,
Land,
TouchdownConfirm,
Terminated,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
#[repr(u8)]
pub enum GroundState {
Unknown,
Airborne,
ContactCandidate,
Grounded,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
#[repr(u16)]
pub enum FallbackReason {
OffboardExpired = 1,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
#[repr(u16)]
pub enum TerminalReason {
Requested = 1,
ClockRegression = 2,
SequenceExhausted = 3,
ImuLost = 4,
NavigationLost = 5,
Numerical = 6,
ControlDeadline = 7,
OutputFault = 8,
LandingDeadline = 9,
FlightDeadline = 10,
IngressOverrun = 11,
}
/// Output evidence issued by the runtime. Consumers cannot grant permission
/// by changing the record's private fields.
/// ```compile_fail,E0616
/// use kaptan_flight_runtime::OutputRecord;
/// fn grant_permission(mut record: OutputRecord) {
/// record.permitted = true;
/// }
/// ```
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct OutputRecord {
epoch: RuntimeEpoch,
arm: Option<ArmId>,
sequence: u64,
produced_at: Timestamp,
valid_until: Timestamp,
permitted: bool,
motors: [f32; 4],
}
impl OutputRecord {
pub const fn epoch(self) -> RuntimeEpoch {
self.epoch
}
pub const fn arm(self) -> Option<ArmId> {
self.arm
}
pub const fn sequence(self) -> u64 {
self.sequence
}
pub const fn produced_at(self) -> Timestamp {
self.produced_at
}
pub const fn valid_until(self) -> Timestamp {
self.valid_until
}
pub const fn permitted(self) -> bool {
self.permitted
}
pub const fn motors(self) -> [f32; 4] {
self.motors
}
}
pub trait FlightOutput {
type Error;
fn actuator(&mut self, record: OutputRecord) -> Result<(), Self::Error>;
fn command_status(&mut self, record: CommandStatus) -> Result<(), Self::Error>;
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct FlightStatus {
pub epoch: RuntimeEpoch,
pub owner: CommandOwner,
pub at: Timestamp,
pub arm: Option<ArmId>,
pub phase: VehiclePhase,
pub phase_since: Timestamp,
pub phase_deadline: Option<Timestamp>,
pub flight_deadline: Option<Timestamp>,
pub fallback_latched: bool,
pub fallback_reason: Option<FallbackReason>,
pub terminal_reason: Option<TerminalReason>,
pub offboard: Option<RequestSequence>,
pub setpoint_sequence: Option<RequestSequence>,
pub setpoint_expires_at: Option<Timestamp>,
pub command_highwater: Option<RequestSequence>,
pub active_command: Option<CommandStatus>,
pub last_terminal_command: Option<CommandStatus>,
pub estimate: Option<FlightEstimate>,
pub ground_state: GroundState,
pub local_output_healthy: bool,
pub last_output_sequence: Option<u64>,
pub last_local_commit_at: Option<Timestamp>,
pub source_status: [SensorStatus; 5],
}
#[derive(Debug, PartialEq)]
pub struct EndpointError<E> {
pub primary: EndpointFailure<E>,
pub inhibition: Option<E>,
}
#[derive(Debug, PartialEq)]
pub enum EndpointFailure<E> {
ClockRegression,
EpochExhausted,
OutputSequenceExhausted,
NotDisarmed,
ControlDeadline,
Navigation(NavigationFailure),
Control,
Output(E),
Notification(E),
}
impl<E> From<EndpointFailure<E>> for EndpointError<E> {
fn from(primary: EndpointFailure<E>) -> Self {
Self {
primary,
inhibition: None,
}
}
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum RuntimeConfigError {
Navigation,
Control,
Timing,
Safety,
}
firmware/libs/kaptan-flight-runtime/tests/lifecycle.rs
+586
View File
@@ -0,0 +1,586 @@
//! Direct scripted sensor trajectories exercise the core flight phases.
mod support;
use kaptan_flight_runtime::*;
use kaptan_navigation::{AccelerationFrd, GRAVITY_MPS2, PositionNed, SensorRecord, VelocityNed};
use support::*;
fn climb(t: u64, start: u64, duration: f64) -> (f64, f64, f64) {
let s = (t.saturating_sub(start) as f64 * 1e-6 / duration).clamp(0., 1.);
(
2. * (10. * s.powi(3) - 15. * s.powi(4) + 6. * s.powi(5)),
2. / duration * (30. * s * s - 60. * s.powi(3) + 30. * s.powi(4)),
2. / (duration * duration) * (60. * s - 180. * s * s + 120. * s.powi(3)),
)
}
fn fly(ep: &mut FlightEndpoint, out: &mut Recording, t: u64, start: u64, land: Option<u64>) {
fly_with_bounce(ep, out, t, start, land, None);
}
fn fly_with_bounce(
ep: &mut FlightEndpoint,
out: &mut Recording,
t: u64,
start: u64,
land: Option<u64>,
bounce: Option<u64>,
) {
let motion = |measured: u64| {
let (mut h, mut v, _) = climb(measured, start, 5.);
if let Some(land) = land
&& measured >= land
{
let (down, dv, _) = climb(measured, land, 6.);
h = 2. - down;
v = -dv;
}
if let Some(bounce) = bounce
&& measured >= bounce
&& measured <= bounce + 1_000_000
{
let phase = (measured - bounce) as f64 * 1e-6 * core::f64::consts::TAU;
h += 0.05 * (1.0 - phase.cos());
v += 0.05 * core::f64::consts::TAU * phase.sin();
}
(h, v)
};
for mut record in records(t) {
let measured = record.context().measurement_time.as_microseconds();
let (h, v) = motion(measured);
match &mut record {
SensorRecord::Imu(r) => {
// Complete declared window, not endpoint acceleration.
let duration = u64::from(r.acquisition_duration_us);
let a = (v - motion(measured - duration).1) / (duration as f64 * 1e-6);
r.acceleration =
AccelerationFrd::metres_per_second_squared(0., 0., -GRAVITY_MPS2 - a as f32)
}
SensorRecord::LocalPositionVelocity(r) => {
r.position = PositionNed::metres(12., -7., 4. - h as f32);
r.velocity = VelocityNed::metres_per_second(0., 0., -v as f32);
}
SensorRecord::Pressure(r) => {
r.pascals =
(101325. * (-h / (287.05 * 288.15 / f64::from(GRAVITY_MPS2))).exp()) as f32
}
SensorRecord::Range(r) => r.metres = 0.04 + h as f32,
SensorRecord::Magnetic(_) => {}
}
assert_eq!(
ep.observe(ep.status().epoch, record, time(t), out).unwrap(),
ObservationAdmission::Queued
);
}
ep.tick(time(t), out).unwrap();
}
#[test]
fn onboard_hover_lands_and_stops() {
let (mut ep, mut out, start) = armed();
let mission = request(
&ep,
2,
FlightCommand::TakeoffHoverLand {
height_above_launch_metres: 2.,
},
start,
);
assert_eq!(
ep.command(mission, time(start), &mut out)
.unwrap()
.lifecycle,
CommandLifecycle::Accepted
);
let flight_deadline = ep.status().flight_deadline;
for t in (start + 10_000..=start + 8_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
let hold = ep.status();
assert_eq!(hold.phase, VehiclePhase::Hold);
let land = hold.phase_since.as_microseconds() + 30_000_000;
assert_eq!(hold.phase_deadline, Some(time(land)));
assert_eq!(hold.active_command.unwrap().sequence, RequestSequence(2));
for t in (start + 8_010_000..land).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
assert_eq!(ep.status().phase_since, hold.phase_since);
assert_eq!(ep.status().phase_deadline, Some(time(land)));
assert!(!out.commands.iter().any(|command| {
command.sequence == RequestSequence(2) && command.lifecycle == CommandLifecycle::Completed
}));
let output = *out.outputs.last().unwrap();
ep.supervise(time(land), &mut out).unwrap();
assert_eq!(ep.status().phase, VehiclePhase::Land);
assert_eq!(ep.status().flight_deadline, flight_deadline);
assert_eq!(
ep.status().phase_deadline,
Some(time(land + LANDING_BUDGET_US))
);
assert_eq!(*out.outputs.last().unwrap(), output);
fly(&mut ep, &mut out, land, start, Some(land));
for t in (land + 10_000..=land + 9_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, Some(land));
}
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert_eq!(ep.status().arm, None);
assert_eq!(ep.status().active_command, None);
assert_eq!(ep.status().flight_deadline, None);
assert!(!out.outputs.last().unwrap().permitted());
let completed = ep.status().last_terminal_command.unwrap();
assert_eq!(completed.kind, CommandKind::TakeoffHoverLand);
assert_eq!(completed.lifecycle, CommandLifecycle::Completed);
assert_eq!(completed.arm, None);
assert!(completed.at > time(land));
}
#[test]
fn takeoff_hold_offboard_loss_latches_lands_and_requires_new_arm() {
let (mut ep, mut out, start) = armed();
ep.command(
request(
&ep,
2,
FlightCommand::Takeoff {
height_above_launch_metres: 2.,
},
start,
),
time(start),
&mut out,
)
.unwrap();
for t in (start + 10_000..=start + 8_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
assert_eq!(ep.status().phase, VehiclePhase::Hold);
assert_eq!(ep.status().phase_deadline, None);
let held_since = ep.status().phase_since;
// Autonomous phase outlives the finite command TTL and settling deadline.
for t in (start + 8_010_000..=start + 38_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
assert_eq!(ep.status().phase_since, held_since);
assert!(ep.status().flight_deadline.is_some());
let enter = start + 38_000_000;
let origin = ep.status().estimate.unwrap().origin.id;
let initial = LocalSetpoint::PositionYaw {
position: PositionNed::metres(0., 0., -2.),
yaw_radians: 0.,
};
let mut entry = request(
&ep,
3,
FlightCommand::EnterOffboard { origin, initial },
enter - 3_000,
);
entry.context.time_uncertainty_microseconds = 2_000;
assert_eq!(
ep.command(entry, time(enter), &mut out).unwrap().lifecycle,
CommandLifecycle::Completed
);
let sp = SetpointRequest {
epoch: ep.status().epoch,
owner: ep.status().owner,
arm: ep.status().arm.unwrap(),
origin,
offboard: RequestSequence(3),
sequence: RequestSequence(0),
issued_at: time(enter - 3_000),
time_uncertainty_microseconds: 2_000,
setpoint: initial,
};
assert_eq!(
ep.setpoint(sp, time(enter), &mut out).unwrap().reason,
CommandReason::Replay
);
assert!(
ep.setpoint(
SetpointRequest {
sequence: RequestSequence(1),
..sp
},
time(enter),
&mut out
)
.unwrap()
.accepted
);
for t in (enter + 10_000..=enter + 300_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
if t == enter + 290_000 {
assert!(out.outputs.last().unwrap().valid_until() > time(enter + 300_000));
}
}
assert_eq!(ep.status().phase, VehiclePhase::FallbackHold);
assert_eq!(ep.status().phase_deadline, Some(time(enter + 1_295_000)));
let retry = ep
.command(
request(
&ep,
4,
FlightCommand::EnterOffboard { origin, initial },
enter + 300_000,
),
time(enter + 300_000),
&mut out,
)
.unwrap();
assert_eq!(retry.reason, CommandReason::FallbackLatched);
assert!(
!ep.setpoint(
SetpointRequest {
sequence: RequestSequence(2),
issued_at: time(enter + 298_000),
..sp
},
time(enter + 300_000),
&mut out
)
.unwrap()
.accepted
);
let land = enter + 1_300_000;
for t in (enter + 310_000..=land).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
if t == land - 10_000 {
assert!(out.outputs.last().unwrap().valid_until() > time(land));
}
}
assert_eq!(ep.status().phase, VehiclePhase::Land);
let deadline = ep.status().phase_deadline;
assert_eq!(deadline, Some(time(enter + 21_295_000)));
for sequence in [5, 6] {
let repeated = ep
.command(
request(&ep, sequence, FlightCommand::Land, land),
time(land),
&mut out,
)
.unwrap();
assert_eq!(repeated.reason, CommandReason::AlreadyActive);
assert_eq!(repeated.lifecycle, CommandLifecycle::Rejected);
assert_eq!(ep.status().phase_deadline, deadline);
}
for t in (land + 10_000..=land + 9_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, Some(land));
}
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert!(ep.status().fallback_latched);
assert_eq!(ep.status().ground_state, GroundState::Grounded);
let inhibit = out.outputs.last().unwrap();
assert!(!inhibit.permitted());
assert_eq!(inhibit.arm(), None);
let now = land + 9_000_000;
let s = ep
.command(
request(&ep, 7, FlightCommand::Arm, now),
time(now),
&mut out,
)
.unwrap();
assert_eq!(s.arm, Some(ArmId(7)));
assert!(ep.status().fallback_latched);
for t in (now + 10_000..=now + 600_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, Some(land));
}
assert_eq!(ep.status().phase, VehiclePhase::ArmedIdle);
assert!(!ep.status().fallback_latched);
}
#[test]
fn failed_inhibition_and_terminal_notification_are_both_retained() {
let (mut ep, mut out, now) = armed();
out.fail_inhibit = true;
out.fail_notification = true;
let err = ep
.command(
request(&ep, 2, FlightCommand::Terminate, now),
time(now),
&mut out,
)
.unwrap_err();
assert_eq!(err.primary, EndpointFailure::Notification("notification"));
assert_eq!(err.inhibition, Some("inhibit"));
assert_eq!(ep.status().phase, VehiclePhase::Terminated);
assert!(!out.outputs.last().unwrap().permitted());
assert_eq!(out.commands.last().unwrap().kind, CommandKind::Terminate);
}
#[test]
fn supervision_latches_stream_expiry_and_land_without_consuming_control_release() {
let (mut ep, mut out, start) = armed();
ep.command(
request(
&ep,
2,
FlightCommand::Takeoff {
height_above_launch_metres: 2.0,
},
start,
),
time(start),
&mut out,
)
.unwrap();
for t in (start + 10_000..=start + 8_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
assert_eq!(ep.status().phase, VehiclePhase::Hold);
let enter = start + 8_000_000;
let origin = ep.status().estimate.unwrap().origin.id;
ep.command(
request(
&ep,
3,
FlightCommand::EnterOffboard {
origin,
initial: LocalSetpoint::PositionYaw {
position: PositionNed::metres(0.0, 0.0, -2.0),
yaw_radians: 0.0,
},
},
enter,
),
time(enter),
&mut out,
)
.unwrap();
let expiry = enter + 300_000;
for t in (enter + 10_000..expiry).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
let output = *out.outputs.last().unwrap();
let cached = ep.status().estimate;
ep.supervise(time(expiry), &mut out).unwrap();
assert_eq!(ep.status().phase, VehiclePhase::FallbackHold);
assert!(ep.status().fallback_latched);
assert_eq!(ep.status().phase_deadline, Some(time(expiry + 1_000_000)));
assert_eq!(ep.status().estimate, cached);
assert_eq!(*out.outputs.last().unwrap(), output);
ep.supervise(time(expiry), &mut out).unwrap();
assert_eq!(*out.outputs.last().unwrap(), output);
// The post-ingress tick must still release on the due grid point.
fly(&mut ep, &mut out, expiry, start, None);
assert_eq!(
out.outputs.last().unwrap().sequence(),
output.sequence() + 1
);
assert!(out.outputs.last().unwrap().permitted());
for t in (expiry + 10_000..expiry + 1_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
let output = *out.outputs.last().unwrap();
let cached = ep.status().estimate;
ep.supervise(time(expiry + 1_000_000), &mut out).unwrap();
assert_eq!(ep.status().phase, VehiclePhase::Land);
assert_eq!(ep.status().phase_deadline, Some(time(expiry + 21_000_000)));
assert_eq!(ep.status().estimate, cached);
assert_eq!(*out.outputs.last().unwrap(), output);
}
#[test]
fn unsuccessful_hold_enters_land_without_resetting_flight_deadline() {
let (mut ep, mut out, start) = armed();
ep.command(
request(
&ep,
2,
FlightCommand::Takeoff {
height_above_launch_metres: 2.,
},
start,
),
time(start),
&mut out,
)
.unwrap();
for t in (start + 10_000..=start + 2_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
let at = start + 2_000_000;
ep.command(request(&ep, 3, FlightCommand::Hold, at), time(at), &mut out)
.unwrap();
let flight_deadline = ep.status().flight_deadline;
for t in (at + 10_000..=at + 10_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
assert_eq!(ep.status().phase, VehiclePhase::Land);
assert_eq!(ep.status().flight_deadline, flight_deadline);
let result = ep.status().last_terminal_command.unwrap();
assert_eq!(result.kind, CommandKind::Hold);
assert_eq!(result.lifecycle, CommandLifecycle::Failed);
assert_eq!(result.reason, CommandReason::PhaseDeadline);
}
#[test]
fn streamed_velocity_and_yaw_reach_the_actual_controller() {
let (mut ep, mut out, start) = armed();
ep.command(
request(
&ep,
2,
FlightCommand::Takeoff {
height_above_launch_metres: 2.0,
},
start,
),
time(start),
&mut out,
)
.unwrap();
for t in (start + 10_000..=start + 8_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
let now = start + 8_000_000;
let status = ep.status();
let origin = status.estimate.unwrap().origin.id;
let initial = LocalSetpoint::VelocityYaw {
velocity: VelocityNed::metres_per_second(0.3, 0.0, 0.0),
yaw_radians: 0.5,
};
ep.command(
request(
&ep,
3,
FlightCommand::EnterOffboard { origin, initial },
now,
),
time(now),
&mut out,
)
.unwrap();
for t in (now + 10_000..=now + 50_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
let u = out.outputs.last().unwrap().motors();
assert!(u[0] - u[1] + u[2] - u[3] > 0.0001); // positive yaw, mixer reaction signs
assert!(u[0] - u[1] - u[2] + u[3] < -0.001); // nose-down pitch for north velocity
let sp = SetpointRequest {
epoch: status.epoch,
owner: status.owner,
arm: status.arm.unwrap(),
origin,
offboard: RequestSequence(3),
sequence: RequestSequence(1),
issued_at: time(now + 50_000),
time_uncertainty_microseconds: 0,
setpoint: LocalSetpoint::VelocityYaw {
velocity: VelocityNed::metres_per_second(0.3, 0.0, 0.0),
yaw_radians: -0.5,
},
};
assert!(
ep.setpoint(sp, time(now + 50_000), &mut out)
.unwrap()
.accepted
);
let expires = ep.status().setpoint_expires_at;
assert_eq!(
ep.setpoint(sp, time(now + 50_000), &mut out)
.unwrap()
.reason,
CommandReason::Replay
);
assert_eq!(ep.status().setpoint_expires_at, expires);
for t in (now + 60_000..=now + 150_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
let u = out.outputs.last().unwrap().motors();
assert!(u[0] - u[1] + u[2] - u[3] < -0.0001);
}
#[test]
fn sensed_bounce_restarts_contact_dwell_without_extending_landing() {
let (mut ep, mut out, start) = armed();
ep.command(
request(
&ep,
2,
FlightCommand::Takeoff {
height_above_launch_metres: 2.0,
},
start,
),
time(start),
&mut out,
)
.unwrap();
for t in (start + 10_000..=start + 8_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
let land = start + 8_000_000;
ep.command(
request(&ep, 3, FlightCommand::Land, land),
time(land),
&mut out,
)
.unwrap();
let deadline = ep.status().phase_deadline;
let mut bounce = None;
let mut observed_motion = false;
for t in (land + 10_000..=land + 11_000_000).step_by(10_000) {
fly_with_bounce(&mut ep, &mut out, t, start, Some(land), bounce);
if bounce.is_none() && ep.status().phase == VehiclePhase::TouchdownConfirm {
bounce = Some(t + 100_000);
}
if let Some(at) = bounce {
if t >= at + 200_000 && t <= at + 800_000 {
assert_ne!(ep.status().ground_state, GroundState::Grounded);
assert_eq!(ep.status().phase_deadline, deadline);
observed_motion = true;
}
if t >= at && t < at + 2_000_000 {
assert_ne!(ep.status().phase, VehiclePhase::Disarmed);
}
}
}
assert!(observed_motion);
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert_eq!(
ep.status().last_terminal_command.unwrap().kind,
CommandKind::Land
);
}
#[test]
fn flight_reserve_fails_pending_hold_before_autonomous_landing() {
let (mut ep, mut out, start) = armed();
ep.command(
request(
&ep,
2,
FlightCommand::Takeoff {
height_above_launch_metres: 2.0,
},
start,
),
time(start),
&mut out,
)
.unwrap();
for t in (start + 10_000..=start + 99_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
let hold = start + 99_000_000;
ep.command(
request(&ep, 3, FlightCommand::Hold, hold),
time(hold),
&mut out,
)
.unwrap();
let land = start + 100_000_000;
for t in (hold + 10_000..=land).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
assert_eq!(ep.status().phase, VehiclePhase::Land);
assert_eq!(ep.status().active_command, None);
let failure = ep.command_outcomes()[0].unwrap().status;
assert_eq!(failure.sequence, RequestSequence(3));
assert_eq!(failure.lifecycle, CommandLifecycle::Failed);
assert_eq!(failure.reason, CommandReason::PhaseDeadline);
for t in (land + 10_000..=land + 9_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, Some(land));
}
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert!(!out.commands.iter().any(|c|c.sequence==RequestSequence(3) && c.lifecycle==CommandLifecycle::Completed));
assert_eq!(ep.status().last_terminal_command, Some(failure));
}
firmware/libs/kaptan-flight-runtime/tests/navigation.rs
+164
View File
@@ -0,0 +1,164 @@
mod support;
use kaptan_config::ConfigModel;
use kaptan_flight_runtime::*;
use kaptan_math::Vector3;
use kaptan_navigation::{
AccelerationFrd, AngularRateFrd, NavigationFailure, PositionNed, SensorRecord, VelocityNed,
};
use support::*;
#[test]
fn actual_navigation_fixed_reference_origin_and_delayed_aiding() {
let (ep, out, now) = ready();
let estimate = ep.status().estimate.unwrap();
assert!(
estimate.capabilities.flight_ready(),
"{:?}",
estimate.capabilities
);
assert!(estimate.position.vector().norm() < 0.01);
assert!((estimate.origin.launch_in_reference.vector().x() - 12.).abs() < 0.001);
assert!(estimate.fused_through.as_microseconds() <= now - 100_000);
assert_eq!(estimate.at, time(now));
assert!(
estimate
.ground_clearance
.unwrap()
.contact_clearance_metres
.abs()
< 0.002
);
assert!(out.outputs.iter().all(|v| !v.permitted()));
}
#[test]
fn navigation_failures_retain_their_reason() {
for (delay, observe, failure, reason) in [
(
30_000,
true,
NavigationFailure::ImuGap,
TerminalReason::ImuLost,
),
(
100_000,
false,
NavigationFailure::WorkBudget,
TerminalReason::NavigationLost,
),
] {
let (mut ep, mut out, now) = ready();
let now = now + delay;
let error = if observe {
ep.observe(
ep.status().epoch,
SensorRecord::Imu(imu(now)),
time(now),
&mut out,
)
.unwrap_err()
} else {
ep.tick(time(now), &mut out).unwrap_err()
};
assert_eq!(error.primary, EndpointFailure::Navigation(failure));
assert_eq!(ep.status().terminal_reason, Some(reason));
assert_eq!(ep.status().phase, VehiclePhase::Terminated);
assert!(!out.outputs.last().unwrap().permitted());
}
}
#[test]
fn healthy_armed_idle_expires_without_rearming() {
let (mut ep, mut out, now) = armed();
for t in (now + 10_000..=now + 10_500_000).step_by(10_000) {
feed(&mut ep, &mut out, t);
}
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert_eq!(ep.status().arm, None);
assert!(out.outputs.iter().all(|v| !v.permitted()));
}
#[test]
fn declared_noise_realizations_initialize_and_remain_grounded() {
// Box-Muller Gaussian draws, independently implemented at the sensor boundary.
// q/T gives IMU per-sample variance; no packet carries an ImuNoise value.
let mut seed = 0xaca5_8113_78ab_1234_u64;
let mut uniform = || {
seed ^= seed << 13;
seed ^= seed >> 7;
seed ^= seed << 17;
((seed >> 11) as f64 + 0.5) / 9_007_199_254_740_992.
};
let mut normal = || (-2. * uniform().ln()).sqrt() * (core::f64::consts::TAU * uniform()).cos();
let profile = profile_fixture::candidate()
.validate(&profile_fixture::capabilities())
.unwrap();
let mut ep = FlightEndpoint::new(
profile,
RunBinding {
session: TransportSessionId(7),
owner: CommandOwner(1),
},
)
.unwrap();
let mut out = Recording::default();
for m in (10_000..=6_000_000).step_by(10_000) {
let t = m + 1_000;
for mut record in records(t) {
match &mut record {
SensorRecord::Imu(r) => {
let s = (2.5e-5_f64 / 0.01).sqrt();
r.acceleration = AccelerationFrd::metres_per_second_squared(
(normal() * s) as f32,
(normal() * s) as f32,
(-f64::from(kaptan_navigation::GRAVITY_MPS2) + normal() * s) as f32,
);
let s = (2.5e-7_f64 / 0.01).sqrt();
r.angular_rate = AngularRateFrd::radians_per_second(
(normal() * s) as f32,
(normal() * s) as f32,
(normal() * s) as f32,
);
}
SensorRecord::LocalPositionVelocity(r) => {
r.position = PositionNed::metres(
(12. + normal() * 0.1) as f32,
(-7. + normal() * 0.1) as f32,
(4. + normal() * 0.1) as f32,
);
r.velocity = VelocityNed::metres_per_second(
(normal() * 0.05) as f32,
(normal() * 0.05) as f32,
(normal() * 0.05) as f32,
);
}
SensorRecord::Magnetic(r) => {
r.microtesla_frd = Vector3::new(
(20. + normal() * 0.02) as f32,
(normal() * 0.02) as f32,
(45. + normal() * 0.02) as f32,
)
}
SensorRecord::Pressure(r) => r.pascals = (101_325. + normal() * 0.3) as f32,
SensorRecord::Range(r) => {
r.metres = (((0.04 + normal() * 0.002) / 0.001).round() * 0.001) as f32
}
}
let admitted = ep
.observe(ep.status().epoch, record, time(t), &mut out)
.unwrap();
assert!(
admitted == ObservationAdmission::Queued
|| admitted
== ObservationAdmission::Rejected(
ObservationRejection::BehindFusionHorizon
)
);
}
ep.tick(time(t), &mut out).unwrap();
}
let e = ep.status().estimate.unwrap();
assert!(e.capabilities.flight_ready(), "{:?}", e.capabilities);
assert!(e.position.vector().norm() < 0.1, "{:?}", e.position);
assert_eq!(ep.status().ground_state, GroundState::Grounded);
assert!(out.outputs.iter().all(|r| !r.permitted()));
}
firmware/libs/kaptan-flight-runtime/tests/replay.rs
+413
View File
@@ -0,0 +1,413 @@
//! Sensor-driven lifecycle and output boundary regressions.
mod support;
use kaptan_flight_runtime::*;
use support::*;
#[test]
fn sensor_startup_arm_binding_and_ground_disarm() {
let (mut ep, mut out, now) = ready();
assert_eq!(ep.status().ground_state, GroundState::Grounded);
let s = ep
.command(
request(&ep, 1, FlightCommand::Arm, now),
time(now),
&mut out,
)
.unwrap();
assert_eq!(s.lifecycle, CommandLifecycle::Accepted);
assert_eq!(s.arm, Some(ArmId(1)));
assert_eq!(ep.status().phase, VehiclePhase::PreArm);
let binding = out.outputs.last().unwrap();
assert_eq!(binding.arm(), s.arm);
assert!(!binding.permitted());
let s = ep
.command(
request(&ep, 2, FlightCommand::GroundDisarm, now),
time(now),
&mut out,
)
.unwrap();
assert_eq!(s.lifecycle, CommandLifecycle::Completed);
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert_eq!(out.outputs.last().unwrap().arm(), None);
}
#[test]
fn command_ttl_replay_and_epoch_do_not_renew_authority() {
let (mut ep, mut out, now) = ready();
let zero = request(&ep, 0, FlightCommand::Arm, now);
assert_eq!(
ep.command(zero, time(now), &mut out).unwrap().reason,
CommandReason::Replay
);
let mut wrong = request(&ep, 50, FlightCommand::Arm, now);
wrong.context.owner = CommandOwner(999);
assert_eq!(
ep.command(wrong, time(now), &mut out).unwrap().reason,
CommandReason::WrongOwner
);
assert_eq!(ep.status().command_highwater, None);
let mut expired = request(&ep, 1, FlightCommand::Arm, now);
expired.context.expires_at = time(now);
assert_eq!(
ep.command(expired, time(now), &mut out).unwrap().lifecycle,
CommandLifecycle::Rejected
);
assert_eq!(ep.status().command_highwater, Some(RequestSequence(1)));
assert_eq!(
ep.command(
request(&ep, 1, FlightCommand::Arm, now),
time(now),
&mut out
)
.unwrap()
.reason,
CommandReason::Replay
);
ep.command(
request(&ep, 2, FlightCommand::Arm, now),
time(now),
&mut out,
)
.unwrap();
for t in (now + 10_000..=now + 600_000).step_by(10_000) {
feed(&mut ep, &mut out, t);
}
assert_eq!(ep.status().phase, VehiclePhase::ArmedIdle); // TTL expired, arm completed.
ep.command(
request(&ep, 3, FlightCommand::GroundDisarm, now + 600_000),
time(now + 600_000),
&mut out,
)
.unwrap();
let old = ep.status().epoch;
let seq = out.outputs.last().unwrap().sequence();
let epoch = ep
.begin_epoch(CommandOwner(8), time(now + 600_000), &mut out)
.unwrap();
assert_ne!(old, epoch);
assert_eq!(out.outputs.last().unwrap().sequence(), seq + 1);
assert_eq!(ep.status().at, time(now + 600_000));
}
#[test]
fn failed_sink_consumes_sequence_and_attempts_new_inhibition() {
let (mut ep, mut out, now) = armed();
ep.command(
request(
&ep,
2,
FlightCommand::Takeoff {
height_above_launch_metres: 2.,
},
now,
),
time(now),
&mut out,
)
.unwrap();
out.fail_enabled = true;
out.fail_inhibit = true;
let attempts = out.outputs.len();
let err = feed_result(&mut ep, &mut out, now + 10_000).unwrap_err();
assert_eq!(out.outputs.len(), attempts + 2);
assert_eq!(err.primary, EndpointFailure::Output("enabled"));
assert_eq!(err.inhibition, Some("inhibit"));
assert_eq!(ep.status().phase, VehiclePhase::Terminated);
let n = out.outputs.len();
assert!(out.outputs[n - 2].permitted());
assert!(!out.outputs[n - 1].permitted());
assert_eq!(
out.outputs[n - 1].sequence(),
out.outputs[n - 2].sequence() + 1
);
assert!(
ep.begin_epoch(CommandOwner(1), time(now + 10_000), &mut out)
.is_err()
);
}
#[test]
fn failed_arm_binding_publication_retains_the_failed_transaction() {
let (mut ep, mut out, now) = ready();
out.fail_inhibit = true;
let error = ep
.command(
request(&ep, 1, FlightCommand::Arm, now),
time(now),
&mut out,
)
.unwrap_err();
assert_eq!(error.primary, EndpointFailure::Output("inhibit"));
assert_eq!(ep.status().phase, VehiclePhase::Terminated);
assert_eq!(ep.status().arm, None);
let failed = ep.status().last_terminal_command.unwrap();
assert_eq!(failed.sequence, RequestSequence(1));
assert_eq!(failed.lifecycle, CommandLifecycle::Failed);
assert_eq!(failed.arm, Some(ArmId(1)));
assert!(!out.outputs.last().unwrap().permitted());
}
#[test]
fn asynchronous_tick_deadline_and_clock_fault_inhibit() {
let (mut ep, mut out, now) = armed();
ep.command(
request(
&ep,
2,
FlightCommand::Takeoff {
height_above_launch_metres: 2.,
},
now,
),
time(now),
&mut out,
)
.unwrap();
feed(&mut ep, &mut out, now + 10_000);
let record = *out.outputs.last().unwrap();
assert!(record.permitted());
assert!(
record.valid_until().as_microseconds() <= record.produced_at().as_microseconds() + 20_000
);
ep.tick(time(now + 10_000), &mut out).unwrap();
assert_eq!(*out.outputs.last().unwrap(), record);
let error = ep.tick(time(now + 30_001), &mut out).unwrap_err();
assert_eq!(error.primary, EndpointFailure::ControlDeadline);
assert!(!out.outputs.last().unwrap().permitted());
let error = ep.tick(time(now), &mut out).unwrap_err();
assert_eq!(error.primary, EndpointFailure::ClockRegression);
assert_eq!(ep.status().phase, VehiclePhase::Terminated);
}
#[test]
fn ordinary_prearm_health_failure_clears_arm_without_terminal_fault() {
let (mut ep, mut out, now) = ready();
ep.command(
request(&ep, 1, FlightCommand::Arm, now),
time(now),
&mut out,
)
.unwrap();
ep.tick(time(now + 25_000), &mut out).unwrap();
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert_eq!(ep.status().arm, None);
assert_eq!(
ep.status().last_terminal_command.unwrap().lifecycle,
CommandLifecycle::Failed
);
assert!(!out.outputs.last().unwrap().permitted());
}
#[test]
fn typed_bad_quality_consumes_sensor_watermark_not_health() {
let (mut ep, mut out, now) = ready();
// The previous complete acquisition ended 1 ms before the service tick.
let mut bad = imu(now + 9_000);
bad.context.quality = 0;
assert_eq!(
ep.observe(
ep.status().epoch,
SensorRecord::Imu(bad),
time(now + 10_000),
&mut out
)
.unwrap(),
ObservationAdmission::Rejected(ObservationRejection::InvalidQuality)
);
bad.context.quality = 255;
assert_eq!(
ep.observe(
ep.status().epoch,
SensorRecord::Imu(bad),
time(now + 10_000),
&mut out
)
.unwrap(),
ObservationAdmission::Rejected(ObservationRejection::Replay)
);
}
#[test]
fn fresh_terminate_does_not_require_current_navigation() {
let (mut ep, mut out, now) = armed();
let now = now + 50_000;
let result = ep
.command(
request(&ep, 2, FlightCommand::Terminate, now),
time(now),
&mut out,
)
.unwrap();
assert_eq!(result.lifecycle, CommandLifecycle::Completed);
assert_eq!(ep.status().phase, VehiclePhase::Terminated);
assert_eq!(ep.status().terminal_reason, Some(TerminalReason::Requested));
assert!(!out.outputs.last().unwrap().permitted());
let sources = ep.status().source_status;
for t in (now + 10_000..=now + 500_000).step_by(10_000) {
for record in records(t) {
assert_eq!(
ep.observe(ep.status().epoch, record, time(t), &mut out)
.unwrap(),
ObservationAdmission::Rejected(ObservationRejection::Terminated)
);
}
ep.tick(time(t), &mut out).unwrap();
}
assert_eq!(ep.status().source_status, sources);
assert_eq!(ep.status().terminal_reason, Some(TerminalReason::Requested));
}
#[test]
fn new_source_isolation_is_seen_before_the_next_control_tick() {
let (mut ep, mut out, now) = ready();
for step in 1..=15 {
let t = now + step * 10_000;
for mut record in records(t) {
let bad = if let SensorRecord::Magnetic(ref mut magnetic) = record {
magnetic.context.quality = 0;
true
} else {
false
};
let outcome = ep
.observe(ep.status().epoch, record, time(t), &mut out)
.unwrap();
assert_eq!(
outcome,
if bad {
ObservationAdmission::Rejected(ObservationRejection::InvalidQuality)
} else {
ObservationAdmission::Queued
}
);
}
if step == 15 {
let arm = ep
.command(request(&ep, 1, FlightCommand::Arm, t), time(t), &mut out)
.unwrap();
assert_eq!(arm.reason, CommandReason::NotReady);
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
}
ep.tick(time(t), &mut out).unwrap();
}
for t in (now + 160_000..=now + 1_400_000).step_by(10_000) {
feed(&mut ep, &mut out, t);
}
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert_eq!(ep.status().ground_state, GroundState::Grounded);
assert!(ep.status().estimate.unwrap().capabilities.heading.available);
let t = now + 1_400_000;
assert_eq!(
ep.command(request(&ep, 2, FlightCommand::Arm, t), time(t), &mut out)
.unwrap()
.lifecycle,
CommandLifecycle::Accepted
);
}
#[test]
fn sensed_motion_during_prearm_consumes_the_arm_transaction() {
let (mut ep, mut out, now) = ready();
ep.command(
request(&ep, 1, FlightCommand::Arm, now),
time(now),
&mut out,
)
.unwrap();
for step in 1..=3 {
let t = now + step * 10_000;
for mut record in records(t) {
if let SensorRecord::Imu(ref mut imu) = record {
imu.acceleration = kaptan_navigation::AccelerationFrd::metres_per_second_squared(
0.0,
0.0,
-kaptan_navigation::GRAVITY_MPS2 - 5.0,
);
}
ep.observe(ep.status().epoch, record, time(t), &mut out)
.unwrap();
}
ep.tick(time(t), &mut out).unwrap();
}
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert_eq!(ep.status().arm, None);
assert_eq!(ep.status().terminal_reason, None);
assert_eq!(
ep.status().last_terminal_command.unwrap().lifecycle,
CommandLifecycle::Failed
);
assert!(out.outputs.iter().all(|record| !record.permitted()));
}
#[test]
fn full_notification_sink_observes_both_cancelled_arm_and_ground_disarm() {
let (mut ep, mut out, now) = ready();
ep.command(
request(&ep, 1, FlightCommand::Arm, now),
time(now),
&mut out,
)
.unwrap();
out.fail_notification = true;
let error = ep
.command(
request(&ep, 2, FlightCommand::GroundDisarm, now),
time(now),
&mut out,
)
.unwrap_err();
assert_eq!(error.primary, EndpointFailure::Notification("notification"));
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert!(!out.outputs.last().unwrap().permitted());
let outcomes = ep.command_outcomes();
let cancelled = outcomes[0].unwrap();
let disarmed = outcomes[1].unwrap();
assert_eq!(cancelled.status.sequence, RequestSequence(1));
assert_eq!(cancelled.status.lifecycle, CommandLifecycle::Cancelled);
assert_eq!(disarmed.status.sequence, RequestSequence(2));
assert_eq!(disarmed.status.lifecycle, CommandLifecycle::Completed);
assert!(!cancelled.published);
assert!(!disarmed.published);
assert_eq!(
&out.commands[out.commands.len() - 2..],
&[cancelled.status, disarmed.status]
);
assert_eq!(ep.status().last_terminal_command, Some(disarmed.status));
}
#[test]
fn asynchronous_tick_and_observe_faults_deliver_failed_command_outcomes() {
for observe_fault in [false, true] {
let (mut ep, mut out, now) = armed();
ep.command(
request(
&ep,
2,
FlightCommand::Takeoff {
height_above_launch_metres: 2.0,
},
now,
),
time(now),
&mut out,
)
.unwrap();
out.fail_notification = true;
let error = if observe_fault {
let sample = imu(now + 30_000);
ep.observe(
ep.status().epoch,
SensorRecord::Imu(sample),
time(now + 30_000),
&mut out,
)
.unwrap_err()
} else {
ep.tick(time(now + 30_000), &mut out).unwrap_err()
};
assert!(!matches!(error.primary, EndpointFailure::Notification(_)));
let outcome = ep.command_outcomes()[0].unwrap();
assert_eq!(outcome.status.sequence, RequestSequence(2));
assert_eq!(outcome.status.lifecycle, CommandLifecycle::Failed);
assert!(!outcome.published);
assert_eq!(out.commands.last(), Some(&outcome.status));
assert_eq!(ep.status().phase, VehiclePhase::Terminated);
assert!(!out.outputs.last().unwrap().permitted());
}
}
firmware/libs/kaptan-flight-runtime/tests/support/mod.rs
+173
View File
@@ -0,0 +1,173 @@
#[path = "../../../kaptan-flight-config/tests/support/mod.rs"]
pub mod profile_fixture;
use kaptan_config::ConfigModel;
use kaptan_flight_runtime::*;
use kaptan_math::Vector3;
use kaptan_navigation::*;
use kaptan_time::Timestamp;
pub fn time(v: u64) -> Timestamp {
Timestamp::from_microseconds(v)
}
pub fn context(t: u64, receipt: u64, kind: u8, period: u64, frame: FrameId) -> ObservationContext {
ObservationContext {
measurement_time: time(t),
receipt_time: time(receipt),
source: SourceId::new(kind),
frame,
calibration: CalibrationRevision::new(1),
quality: 255,
validity: Validity::Valid,
sequence: ObservationSequence::new((t / period) as u32),
time_uncertainty_microseconds: 0,
}
}
pub fn imu(t: u64) -> FlightImuObservation {
FlightImuObservation {
acquisition_duration_us: 10_000,
context: context(t, t, 1, 10_000, FrameId::BODY_FRD),
acceleration: AccelerationFrd::metres_per_second_squared(0., 0., -GRAVITY_MPS2),
angular_rate: AngularRateFrd::radians_per_second(0., 0., 0.),
}
}
pub fn records(t: u64) -> Vec<SensorRecord> {
let mut result = vec![SensorRecord::Imu(imu(t - 1_000))];
let m = t - 1_000;
if m % 50_000 == 30_000 {
result.push(SensorRecord::LocalPositionVelocity(
LocalPositionVelocityObservation {
context: context(m - 30_000, t, 2, 50_000, FrameId::LOCAL_NED),
position: PositionNed::metres(12., -7., 4.),
velocity: VelocityNed::metres_per_second(0., 0., 0.),
position_variance: [ScalarVariance::new(0.01).unwrap(); 3],
velocity_variance: [ScalarVariance::new(0.0025).unwrap(); 3],
reference: SensorReferenceId(1),
},
));
}
if m.is_multiple_of(50_000) {
result.push(SensorRecord::Magnetic(MagneticObservation {
context: context(m, t, 3, 50_000, FrameId::BODY_FRD),
microtesla_frd: Vector3::new(20., 0., 45.),
variance_microtesla_squared: [ScalarVariance::new(0.0004).unwrap(); 3],
}));
result.push(SensorRecord::Pressure(PressureObservation {
context: context(m, t, 4, 50_000, FrameId::SCALAR),
pascals: 101_325.,
variance_pascals_squared: ScalarVariance::new(0.09).unwrap(),
}));
}
if m.is_multiple_of(20_000) {
result.push(SensorRecord::Range(RangeObservation {
context: context(m, t, 5, 20_000, FrameId::new(3)),
metres: 0.04,
variance: ScalarVariance::new(4e-6).unwrap(),
}));
}
result
}
#[derive(Default)]
pub struct Recording {
pub outputs: Vec<OutputRecord>,
pub commands: Vec<CommandStatus>,
pub fail_enabled: bool,
pub fail_inhibit: bool,
pub fail_notification: bool,
}
impl FlightOutput for Recording {
type Error = &'static str;
fn actuator(&mut self, r: OutputRecord) -> Result<(), Self::Error> {
self.outputs.push(r);
if r.permitted() && self.fail_enabled {
Err("enabled")
} else if !r.permitted() && self.fail_inhibit {
Err("inhibit")
} else {
Ok(())
}
}
fn command_status(&mut self, s: CommandStatus) -> Result<(), Self::Error> {
self.commands.push(s);
if self.fail_notification {
Err("notification")
} else {
Ok(())
}
}
}
pub fn feed_result(
ep: &mut FlightEndpoint,
out: &mut Recording,
t: u64,
) -> Result<(), EndpointError<&'static str>> {
for r in records(t) {
let result = ep.observe(ep.status().epoch, r, time(t), out)?;
if result == ObservationAdmission::Rejected(ObservationRejection::BehindFusionHorizon) {
// The first delayed GNSS delivery after initialization can predate
// t0. It must be refused rather than fused backwards or retimed.
assert!(ep.status().estimate.is_none());
assert_eq!(r.kind(), 2);
assert!(r.context().measurement_time.as_microseconds() <= 2_020_000);
} else {
assert_eq!(result, ObservationAdmission::Queued);
}
}
ep.tick(time(t), out)
}
pub fn feed(ep: &mut FlightEndpoint, out: &mut Recording, t: u64) {
feed_result(ep, out, t).unwrap();
}
pub fn request(ep: &FlightEndpoint, seq: u64, command: FlightCommand, t: u64) -> CommandRequest {
CommandRequest {
context: RequestContext {
epoch: ep.status().epoch,
owner: ep.status().owner,
sequence: RequestSequence(seq),
issued_at: time(t),
expires_at: time(t + 250_000),
time_uncertainty_microseconds: 0,
arm: if command == FlightCommand::Arm {
None
} else {
ep.status().arm
},
},
command,
}
}
pub fn ready() -> (FlightEndpoint, Recording, u64) {
let profile = profile_fixture::candidate()
.validate(&profile_fixture::capabilities())
.unwrap();
let mut ep = FlightEndpoint::new(
profile,
RunBinding {
session: TransportSessionId(123),
owner: CommandOwner(1),
},
)
.unwrap();
let mut out = Recording::default();
for m in (10_000..=4_000_000).step_by(10_000) {
feed(&mut ep, &mut out, m + 1_000);
}
assert!(ep.status().estimate.is_some(), "missing initialization");
(ep, out, 4_001_000)
}
pub fn armed() -> (FlightEndpoint, Recording, u64) {
let (mut ep, mut out, now) = ready();
let result = ep
.command(
request(&ep, 1, FlightCommand::Arm, now),
time(now),
&mut out,
)
.unwrap();
assert_eq!(result.lifecycle, CommandLifecycle::Accepted, "{result:?}");
for t in (now + 10_000..=now + 600_000).step_by(10_000) {
feed(&mut ep, &mut out, t);
}
assert_eq!(ep.status().phase, VehiclePhase::ArmedIdle);
(ep, out, now + 600_000)
}
firmware/libs/kaptan-i2c/Cargo.toml
+3
View File
@@ -7,3 +7,6 @@ version.workspace = true
[lints]
workspace = true
[dependencies]
kaptan-time = { path = "../kaptan-time" }
firmware/libs/kaptan-i2c/src/lib.rs
+32 -6
View File
@@ -6,6 +6,8 @@
#![no_std]
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>;
}
firmware/libs/kaptan-math/src/kani_proofs.rs
+26
View File
@@ -0,0 +1,26 @@
//! Classification of the production square root, not an accuracy proof.
use super::sqrt;
#[kani::proof]
#[kani::unwind(5)]
fn sqrt_preserves_domain_and_zero_sign() {
let value = f32::from_bits(kani::any());
let root = sqrt(value);
if !value.is_finite() || value < 0.0 {
assert!(root.is_nan());
} else if value == 0.0 {
assert_eq!(root.to_bits(), value.to_bits());
} else {
assert!(root.is_finite());
assert!(root > 0.0);
}
kani::cover!(value.is_nan());
kani::cover!(value == f32::INFINITY);
kani::cover!(value < 0.0 && value.is_finite());
kani::cover!(value.to_bits() == (-0.0_f32).to_bits());
kani::cover!(value > 0.0 && value < f32::MIN_POSITIVE);
kani::cover!(value == f32::MAX);
}
firmware/libs/kaptan-math/src/lib.rs
+93 -164
View File
@@ -1,19 +1,18 @@
//! Minimal `f32` math shared by Kaptan navigation and control.
//!
//! 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 [
firmware/libs/kaptan-math/tests/so3_reference.rs
+122
View File
@@ -0,0 +1,122 @@
use kaptan_math::{Frd, UnitQuaternion, Vector3, left_jacobian_apply, right_jacobian_apply};
fn exp(v: [f64; 3]) -> [f64; 4] {
let norm = v.iter().map(|x| x * x).sum::<f64>().sqrt();
let scale = if norm == 0.0 {
0.5
} else {
(norm / 2.0).sin() / norm
};
[(norm / 2.0).cos(), v[0] * scale, v[1] * scale, v[2] * scale]
}
fn multiply(a: [f64; 4], b: [f64; 4]) -> [f64; 4] {
[
a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3],
a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2],
a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1],
a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0],
]
}
#[test]
fn bounded_exponential_and_jacobians_match_f64_group_differentials() {
// Axial and oblique directions across the admitted domain. Central
// differences of Exp(phi + d) Exp(-phi) independently fix the left sign.
for direction in [
[1.0_f64, 0.0, 0.0],
[0.0, 0.0, -1.0],
[2.0 / 3.0, -1.0 / 3.0, 2.0 / 3.0],
] {
for magnitude in [0.0_f64, 1.0e-7, 0.001, 0.2, 1.0, 1.5, 1.999, 2.0] {
let phi = direction.map(|x| (x * magnitude) as f32);
let phi64 = phi.map(f64::from);
let expected = exp(phi64);
let rotation = Vector3::<Frd>::new(phi[0], phi[1], phi[2]);
let actual = UnitQuaternion::from_small_rotation_vector(rotation).unwrap();
for (actual, expected) in [actual.w(), actual.x(), actual.y(), actual.z()]
.into_iter()
.zip(expected)
{
// Below two f32 ulps, including the polynomial endpoint error.
assert!((f64::from(actual) - expected).abs() < 1.5e-7, "{phi:?}");
}
let inverse = [expected[0], -expected[1], -expected[2], -expected[3]];
for axis in 0..3 {
let mut basis = [0.0; 3];
basis[axis] = 1.0;
let basis = Vector3::new(basis[0], basis[1], basis[2]);
let mut plus = phi64;
let mut minus = phi64;
const STEP: f64 = 1.0e-6;
plus[axis] += STEP;
minus[axis] -= STEP;
for (actual, positive, negative) in [
(
left_jacobian_apply(rotation, basis).unwrap(),
multiply(exp(plus), inverse),
multiply(exp(minus), inverse),
),
(
right_jacobian_apply(rotation, basis).unwrap(),
multiply(inverse, exp(plus)),
multiply(inverse, exp(minus)),
),
] {
for (row, value) in [actual.x(), actual.y(), actual.z()].into_iter().enumerate()
{
// The infinitesimal rotation is twice quaternion xyz.
let reference = (positive[row + 1] - negative[row + 1]) / STEP;
assert!(
(f64::from(value) - reference).abs() < 2.5e-6,
"{phi:?}, axis {axis}, row {row}"
);
}
}
}
}
}
}
#[test]
fn normalization_is_scale_invariant_across_f32_exponents() {
for scale in [
f32::from_bits(1),
f32::MIN_POSITIVE,
1.0e-30,
1.0e-9,
1.0,
1.0e30,
f32::MAX,
] {
let vector = Vector3::<Frd>::new(scale, -scale, scale);
let normalized = vector.normalized().unwrap();
for component in [normalized.x(), -normalized.y(), normalized.z()] {
assert!((f64::from(component) - 1.0 / 3.0_f64.sqrt()).abs() < 1.0e-7);
}
let quaternion =
UnitQuaternion::<Frd, Frd>::from_components(scale, scale, -scale, -scale).unwrap();
assert_eq!(
[
quaternion.w(),
quaternion.x(),
quaternion.y(),
quaternion.z()
],
[0.5, 0.5, -0.5, -0.5]
);
assert_eq!(Vector3::<Frd>::new(scale, 0.0, 0.0).norm(), scale);
}
}
#[test]
fn norm_distinguishes_zero_invalid_input_and_unrepresentable_magnitude() {
assert_eq!(Vector3::<Frd>::zero().norm(), 0.0);
for invalid in [f32::NAN, f32::INFINITY, f32::NEG_INFINITY] {
assert!(Vector3::<Frd>::new(1.0, invalid, 2.0).norm().is_nan());
}
let large = Vector3::<Frd>::new(f32::MAX, f32::MAX, f32::MAX);
assert_eq!(large.norm(), f32::INFINITY);
// The direction is still representable when the magnitude is not.
assert!((f64::from(large.normalized().unwrap().norm()) - 1.0).abs() < 2.0e-7);
}
firmware/libs/kaptan-mavlink/Cargo.toml
-4
View File
@@ -9,14 +9,10 @@ version.workspace = true
workspace = true
[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",
] }
firmware/libs/kaptan-mavlink/build.rs
+2 -6
View File
@@ -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\
firmware/libs/kaptan-mavlink/definitions/kaptan.xml
+2 -2
View File
@@ -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>
firmware/libs/kaptan-mavlink/definitions/publish/error.xml
+1 -1
View File
@@ -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>
firmware/libs/kaptan-mavlink/definitions/publish/heartbeat.xml
+1 -1
View File
@@ -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>
firmware/libs/kaptan-mavlink/definitions/publish/imu.xml
+2 -2
View File
@@ -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>
firmware/libs/kaptan-mavlink/definitions/publish/log.xml
+1 -1
View File
@@ -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>
firmware/libs/kaptan-mavlink/definitions/publish/range.xml
+1 -1
View File
@@ -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>
firmware/libs/kaptan-mavlink/src/encoder.rs
+41 -7
View File
@@ -15,7 +15,7 @@ use crate::{
generated::kaptan::{HEARTBEAT_DATA, HeartbeatState, MavMessage},
};
/// Serializes FC-domain data with MAVLink sender identity and sequence state.
/// Encodes Kaptan messages with one endpoint's sender identity and packet sequence.
pub struct MavlinkEncoder {
system_id: u8,
component_id: u8,
@@ -46,8 +46,7 @@ impl MavlinkEncoder {
header
}
/// Encodes one concrete generated Kaptan record through this endpoint's
/// sole header-sequence owner.
/// Encodes a Kaptan record using this endpoint's packet sequence.
pub fn encode<M: MessageData<Message = MavMessage>>(&mut self, data: &M) -> MavlinkFrame {
let mut raw = MavlinkFrame::new();
raw.serialize_message_data(self.header(), data);
@@ -155,7 +154,7 @@ mod tests {
let snapshot = ImuSnapshot::new(
Timestamp::from_microseconds(123),
[1, 2, 3],
[4, 5, 6],
[-34_871, 34_870, -8_731],
2_500,
);
@@ -166,7 +165,40 @@ mod tests {
Some(MavMessage::IMU(data))
if data.time_us == 123
&& data.acceleration_milligravity == [1, 2, 3]
&& data.angular_rate_milliradians_per_second == [4, 5, 6]
&& data.angular_rate_milliradians_per_second == [-34_871, 34_870, -8_731]
&& data.temperature_centidegrees_celsius == 2_500
));
}
#[cfg(feature = "sensor-telemetry")]
#[test]
fn imu_wire_layout_preserves_the_full_signed_gyro_representation() {
let mut encoder = MavlinkEncoder::new(1, 1);
let snapshot = ImuSnapshot::new(
Timestamp::from_microseconds(123),
[1, -2, 3],
[i32::MIN, i32::MAX, -34_871],
2_500,
);
let frame = encoder.imu_snapshot(&snapshot);
assert_eq!(frame.raw_bytes()[1], 28);
// MAVLink orders fields by scalar width. Widening gyro moves it ahead
// of acceleration. Literal little-endian bytes pin that new contract.
assert_eq!(
&frame.raw_bytes()[10..38],
&[
123, 0, 0, 0, 0, 0, 0, 0, // time_us: u64
0, 0, 0, 0x80, 0xff, 0xff, 0xff, 0x7f, 0xc9, 0x77, 0xff, 0xff, // gyro: i32[3]
1, 0, 0xfe, 0xff, 3, 0, // acceleration: i16[3]
0xc4, 0x09, // temperature: i16
]
);
assert!(matches!(
parse_test_message(&frame),
Some(MavMessage::IMU(data))
if data.time_us == 123
&& data.angular_rate_milliradians_per_second == [i32::MIN, i32::MAX, -34_871]
&& data.acceleration_milligravity == [1, -2, 3]
&& data.temperature_centidegrees_celsius == 2_500
));
}
@@ -219,8 +251,10 @@ mod tests {
assert_eq!(HEARTBEAT_DATA::ENCODED_LEN, 1);
assert_eq!(IMU_DATA::NAME, "IMU");
assert_eq!(IMU_DATA::ID, 51_028);
assert_eq!(IMU_DATA::EXTRA_CRC, 194);
assert_eq!(IMU_DATA::ENCODED_LEN, 22);
// Independently calculated X.25 extra CRC over IMU's wire-ordered
// field types/names/array lengths: u64, i32[3], i16[3], i16.
assert_eq!(IMU_DATA::EXTRA_CRC, 22);
assert_eq!(IMU_DATA::ENCODED_LEN, 28);
assert_eq!(RANGE_DATA::NAME, "RANGE");
assert_eq!(RANGE_DATA::ID, 51_029);
assert_eq!(RANGE_DATA::EXTRA_CRC, 44);
firmware/libs/kaptan-mavlink/src/lib.rs
+6 -5
View File
@@ -2,16 +2,17 @@
#![no_std]
/// MAVLink frame encoding API for boards.
/// MAVLink frame encoding API.
pub mod encoder;
/// Serial-byte MAVLink receive API for boards.
/// Incremental MAVLink byte-stream receive API.
pub mod receiver;
/// UART DMA MAVLink frame transmission API for boards.
pub mod transmitter;
/// An owned MAVLink 2 frame suitable for UART transmission.
/// An owned MAVLink 2 frame.
pub type MavlinkFrame = mavlink_core::MAVLinkV2MessageRaw;
/// Generated-message metadata and serialization contract.
pub use mavlink_core::MessageData;
/// Generated wire types for Kaptan's sole MAVLink dialect.
pub mod generated {
include!(concat!(env!("OUT_DIR"), "/kaptan_module.rs"));
firmware/libs/kaptan-mavlink/src/transmitter.rs
-313
View File
@@ -1,313 +0,0 @@
//! MAVLink frame encoding and UART transmission API.
use kaptan_dma::{DmaTxBufferFactory, DmaTxEngine, TxCompletion};
use kaptan_err_reportable::KaptanError;
use kaptan_tx::{PendingFrame, TransmitQueue};
use crate::MavlinkFrame;
/// Delivery importance used for bounded MAVLink transmission admission.
pub use kaptan_tx::{EnqueueResult, FrameClass};
/// Failure while preparing or starting a MAVLink frame transfer.
#[derive(Debug, KaptanError)]
pub enum MavlinkTransmitFault<MemoryError, EngineError> {
/// DMA-visible frame preparation failed.
#[kaptan_error(id = 10001)]
Memory(MemoryError),
/// The UART DMA route rejected a transfer start.
#[kaptan_error(id = 10002)]
Engine(EngineError),
}
/// Combined bounded-admission and DMA-start outcome.
///
/// The outcomes stay distinct because admission decides whether the source may
/// acknowledge its frame, while a failed start keeps reserved work for retry.
#[must_use]
pub struct AdmissionStart<MemoryError, EngineError> {
/// Queue admission for the source frame.
pub admission: EnqueueResult,
/// One bounded attempt to start pending DMA work.
pub start: Result<(), MavlinkTransmitFault<MemoryError, EngineError>>,
}
/// Owns MAVLink frame admission and UART DMA lifecycle.
///
/// Exactly one worker invokes this type. It accepts local encoded frames and
/// received frames alike; the caller owns the policy that decides which frames
/// are transmitted.
pub struct MavlinkTransmitter<Memory, Engine, const N: usize> {
transmit: TransmitQueue<MavlinkFrame, N>,
memory: Memory,
engine: Engine,
}
impl<Memory, Engine, const N: usize> MavlinkTransmitter<Memory, Engine, N> {
/// Creates an empty MAVLink transmitter.
pub const fn new(memory: Memory, engine: Engine) -> Self {
Self {
transmit: TransmitQueue::new(),
memory,
engine,
}
}
}
impl<Memory, Engine, const N: usize> MavlinkTransmitter<Memory, Engine, N>
where
Memory: DmaTxBufferFactory,
Engine: DmaTxEngine,
{
/// Admits one frame and attempts one bounded DMA start while preserving
/// both outcomes. A start failure retains the selected frame in the queue
/// reservation for a later service-turn retry; it does not change whether
/// the newly submitted source frame was admitted.
pub fn admit_and_start(
&mut self,
class: FrameClass,
frame: MavlinkFrame,
) -> AdmissionStart<Memory::Error, Engine::StartError> {
let admission = self.transmit.enqueue(PendingFrame { class, frame });
let start = self.start_next();
AdmissionStart { admission, start }
}
/// Releases a terminal DMA frame and opportunistically starts the next.
///
/// The returned class is the frame that reached a terminal DMA result,
/// not a class selected from newly pending work. A policy can therefore
/// distinguish completion classes without inferring them from newly
/// pending work.
pub fn on_dma_complete(
&mut self,
completion: TxCompletion,
) -> Result<Option<FrameClass>, MavlinkTransmitFault<Memory::Error, Engine::StartError>> {
let completed = self.complete_dma(completion)?;
if completion == TxCompletion::Complete {
self.start_next()?;
}
Ok(completed)
}
fn complete_dma(
&mut self,
completion: TxCompletion,
) -> Result<Option<FrameClass>, MavlinkTransmitFault<Memory::Error, Engine::StartError>> {
match completion {
TxCompletion::NotPending => Ok(None),
TxCompletion::Complete => Ok(self.transmit.complete().map(|frame| frame.class)),
TxCompletion::TransferFault | TxCompletion::Halted => {
Ok(self.transmit.fail().map(|frame| frame.class))
}
}
}
fn start_next(
&mut self,
) -> Result<(), MavlinkTransmitFault<Memory::Error, Engine::StartError>> {
let (transmit, memory, engine) = (&mut self.transmit, &mut self.memory, &mut self.engine);
transmit
.start_with(|frame| {
let buffer = memory
.prepare(frame.raw_bytes())
.map_err(MavlinkTransmitFault::Memory)?;
engine.start(buffer).map_err(MavlinkTransmitFault::Engine)
})
.map(|_| ())
}
}
#[cfg(test)]
mod tests {
use super::{
EnqueueResult, FrameClass, MavlinkTransmitFault, MavlinkTransmitter, PendingFrame,
};
use crate::encoder::MavlinkEncoder;
use kaptan_dma::{DmaReadableBuffer, DmaTxBufferFactory, DmaTxEngine, TxCompletion};
struct Memory;
impl DmaTxBufferFactory for Memory {
type Error = core::convert::Infallible;
fn prepare<'a>(&mut self, bytes: &'a [u8]) -> Result<DmaReadableBuffer<'a>, Self::Error> {
// SAFETY: this host-only test keeps the queue's active frame alive.
Ok(unsafe { DmaReadableBuffer::new_unchecked(bytes) })
}
}
struct Engine;
impl DmaTxEngine for Engine {
type StartError = core::convert::Infallible;
fn start(&mut self, _: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> {
Ok(())
}
}
struct RecordingEngine {
started_sequences: [u8; 4],
starts: usize,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
enum RetryError {
Busy,
}
struct BusyOnceEngine {
busy_once: bool,
starts: usize,
}
impl DmaTxEngine for BusyOnceEngine {
type StartError = RetryError;
fn start(&mut self, _: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> {
if self.busy_once {
self.busy_once = false;
return Err(RetryError::Busy);
}
self.starts += 1;
Ok(())
}
}
impl DmaTxEngine for RecordingEngine {
type StartError = core::convert::Infallible;
fn start(&mut self, buffer: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> {
self.started_sequences[self.starts] = buffer.bytes()[4];
self.starts += 1;
Ok(())
}
}
#[test]
fn transfer_fault_drops_only_the_active_frame() {
let mut encoder = MavlinkEncoder::new(1, 1);
let mut transmitter = MavlinkTransmitter::<Memory, Engine, 2>::new(Memory, Engine);
assert!(
transmitter
.admit_and_start(FrameClass::BestEffort, encoder.heartbeat())
.start
.is_ok()
);
assert!(
transmitter
.admit_and_start(FrameClass::BestEffort, encoder.heartbeat())
.start
.is_ok()
);
assert!(transmitter.transmit.is_active());
transmitter
.on_dma_complete(TxCompletion::TransferFault)
.unwrap();
assert!(!transmitter.transmit.is_active());
}
#[test]
fn reports_critical_queue_saturation_to_the_policy_owner() {
let mut encoder = MavlinkEncoder::new(1, 1);
let mut transmitter = MavlinkTransmitter::<Memory, Engine, 1>::new(Memory, Engine);
assert_eq!(
transmitter
.admit_and_start(FrameClass::Critical, encoder.heartbeat())
.admission,
EnqueueResult::Enqueued
);
assert_eq!(
transmitter
.admit_and_start(FrameClass::Critical, encoder.heartbeat())
.admission,
EnqueueResult::Enqueued
);
assert_eq!(
transmitter
.admit_and_start(FrameClass::Critical, encoder.heartbeat())
.admission,
EnqueueResult::CriticalQueueFull
);
}
#[test]
fn terminal_completion_can_defer_a_transient_engine_busy_without_losing_frame_order() {
let mut encoder = MavlinkEncoder::new(1, 1);
let mut transmitter = MavlinkTransmitter::<Memory, BusyOnceEngine, 2>::new(
Memory,
BusyOnceEngine {
busy_once: false,
starts: 0,
},
);
transmitter.transmit.enqueue(PendingFrame {
class: FrameClass::Critical,
frame: encoder.heartbeat(),
});
transmitter.start_next().unwrap();
assert_eq!(transmitter.engine.starts, 1);
transmitter.transmit.enqueue(PendingFrame {
class: FrameClass::Critical,
frame: encoder.heartbeat(),
});
// Model the STM32 case: TCIF is acknowledged but CR.EN still reports
// busy for the first attempt to start the successor. `complete_dma`
// deliberately does not start it in the terminal IRQ path.
assert_eq!(
transmitter.complete_dma(TxCompletion::Complete).unwrap(),
Some(FrameClass::Critical)
);
assert_eq!(transmitter.engine.starts, 1);
transmitter.engine.busy_once = true;
assert!(matches!(
transmitter.start_next(),
Err(MavlinkTransmitFault::Engine(RetryError::Busy))
));
assert!(!transmitter.transmit.is_active());
transmitter.start_next().unwrap();
assert!(transmitter.transmit.is_active());
assert_eq!(transmitter.engine.starts, 2);
}
#[test]
fn completion_retry_starts_pending_best_effort_before_later_critical_frame() {
let mut encoder = MavlinkEncoder::new(1, 1);
let mut transmitter = MavlinkTransmitter::<Memory, RecordingEngine, 3>::new(
Memory,
RecordingEngine {
started_sequences: [0; 4],
starts: 0,
},
);
// A critical frame is active. A best-effort sample arrives behind it
// before DMA completion.
transmitter.transmit.enqueue(PendingFrame {
class: FrameClass::Critical,
frame: encoder.heartbeat(),
});
transmitter.start_next().unwrap();
transmitter.transmit.enqueue(PendingFrame {
class: FrameClass::BestEffort,
frame: encoder.heartbeat(),
});
// The completion worker performs its bounded retry before the next
// periodic sender can enqueue fresh critical traffic. The existing
// best-effort frame therefore cannot starve behind later traffic.
transmitter.complete_dma(TxCompletion::Complete).unwrap();
transmitter.start_next().unwrap();
transmitter.transmit.enqueue(PendingFrame {
class: FrameClass::Critical,
frame: encoder.heartbeat(),
});
assert_eq!(transmitter.engine.starts, 2);
assert_eq!(&transmitter.engine.started_sequences[..2], &[0, 1]);
}
}
firmware/libs/kaptan-navigation/Cargo.toml
+3 -4
View File
@@ -10,11 +10,10 @@ workspace = true
[features]
default = []
# Enables the provisional raw-rate estimator used only by the internal SITL
# executable. It is not part of a board build.
sitl = ["dep:kaptan-sensor"]
# Driver snapshot conversion into navigation observations. The estimator is
# available independently of physical sensor adapters.
sensor-snapshots = ["dep:kaptan-sensor"]
[dependencies]
kaptan-err-reportable = { path = "../kaptan-err-reportable" }
kaptan-math = { path = "../kaptan-math" }
kaptan-sensor = { path = "../kaptan-sensor", optional = true }
kaptan-time = { path = "../kaptan-time" }
firmware/libs/kaptan-navigation/src/eskf.rs
-4216
View File
File diff suppressed because it is too large. Load diff
firmware/libs/kaptan-navigation/src/filter.rs
+1140
View File
File diff suppressed because it is too large. Load diff
firmware/libs/kaptan-navigation/src/flight.rs
+868
View File
@@ -0,0 +1,868 @@
//! Flight observation, uncertainty and bounded execution contracts.
use crate::*;
use kaptan_math::{Frd, Ned, UnitQuaternion, Vector3};
use kaptan_time::Timestamp;
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct OriginId(pub u32);
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct SensorReferenceId(pub u32);
/// Mean calibrated force/rate over the declared preceding acquisition window.
/// Interior propagation reconstructs that window with its constant average.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct FlightImuObservation {
pub context: ObservationContext,
pub acceleration: AccelerationFrd,
pub angular_rate: AngularRateFrd,
/// Positive duration in the endpoint clock, bounded by the configured IMU
/// interval. Both window endpoints must share the same timing model.
pub acquisition_duration_us: u32,
}
impl FlightImuObservation {
pub(crate) fn acquisition_start(self) -> Option<u64> {
if self.acquisition_duration_us == 0 {
return None;
}
self.context
.measurement_time
.as_microseconds()
.checked_sub(u64::from(self.acquisition_duration_us))
}
}
/// Timing of the latest valid admitted IMU window, possibly not yet propagated.
/// This historical snapshot does not establish freshness, estimator health or
/// output authority. It may remain available after a numerical failure.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct ImuCoverage {
pub end: Timestamp,
pub acquisition_duration_us: u32,
pub time_uncertainty_microseconds: u32,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct MagneticObservation {
pub context: ObservationContext,
pub microtesla_frd: Vector3<Frd>,
pub variance_microtesla_squared: [ScalarVariance; 3],
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct PressureObservation {
pub context: ObservationContext,
pub pascals: f32,
pub variance_pascals_squared: ScalarVariance,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum SensorRecord {
Imu(FlightImuObservation),
LocalPositionVelocity(LocalPositionVelocityObservation),
Magnetic(MagneticObservation),
Pressure(PressureObservation),
Range(RangeObservation),
}
impl SensorRecord {
pub const fn context(self) -> ObservationContext {
match self {
Self::Imu(v) => v.context,
Self::LocalPositionVelocity(v) => v.context,
Self::Magnetic(v) => v.context,
Self::Pressure(v) => v.context,
Self::Range(v) => v.context,
}
}
pub const fn kind(self) -> u8 {
match self {
Self::Imu(_) => 1,
Self::LocalPositionVelocity(_) => 2,
Self::Magnetic(_) => 3,
Self::Pressure(_) => 4,
Self::Range(_) => 5,
}
}
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
#[repr(u16)]
pub enum ObservationRejection {
WrongEpoch = 1,
InvalidTime = 2,
TooOld = 3,
Replay = 4,
SourceMismatch = 5,
FrameMismatch = 6,
CalibrationMismatch = 7,
InvalidValue = 8,
InvalidQuality = 9,
QueueFull = 10,
RateExceeded = 11,
BehindFusionHorizon = 12,
ReferenceMismatch = 13,
Terminated = 14,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum ObservationAdmission {
Queued,
Rejected(ObservationRejection),
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum NavigationFailure {
ImuGap,
QueueOverrun,
WorkBudget,
Numerical,
RequiredCapabilityLost,
}
impl From<kaptan_math::MathError> for NavigationFailure {
fn from(_: kaptan_math::MathError) -> Self {
Self::Numerical
}
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum SourceStatus {
Unknown,
Healthy,
Stale,
Isolated,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct SensorStatus {
pub kind: u8,
pub source: SourceId,
pub integrity: SourceStatus,
pub last_received_measurement: Option<Timestamp>,
pub last_fused_measurement: Option<Timestamp>,
pub queued: u16,
pub rejected: u32,
pub fused: u32,
pub dropped: u32,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct LocalOrigin {
pub id: OriginId,
pub established_at: Timestamp,
pub reference: SensorReferenceId,
pub launch_in_reference: PositionNed,
pub launch_variance_m2: [f32; 3],
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct CapabilityEvidence {
pub available: bool,
pub last_measurement_at: Option<Timestamp>,
pub valid_until: Option<Timestamp>,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct FlightCapabilities {
pub tilt: CapabilityEvidence,
pub heading: CapabilityEvidence,
pub horizontal_position: CapabilityEvidence,
pub horizontal_velocity: CapabilityEvidence,
pub height: CapabilityEvidence,
pub vertical_velocity: CapabilityEvidence,
pub ground_clearance: CapabilityEvidence,
}
impl FlightCapabilities {
pub fn flight_ready(self) -> bool {
self.tilt.available
&& self.heading.available
&& self.horizontal_position.available
&& self.horizontal_velocity.available
&& self.height.available
&& self.vertical_velocity.available
&& self.ground_clearance.available
}
pub fn valid_until(self) -> Option<Timestamp> {
if !self.flight_ready() {
return None;
}
[
self.tilt,
self.heading,
self.horizontal_position,
self.horizontal_velocity,
self.height,
self.vertical_velocity,
self.ground_clearance,
]
.into_iter()
.map(|v| v.valid_until)
.try_fold(Timestamp::from_microseconds(u64::MAX), |a, b| {
Some(a.min(b?))
})
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct GroundClearance {
pub measurement_at: Timestamp,
pub valid_until: Timestamp,
pub source: SourceId,
pub body_origin_height_metres: f32,
pub contact_clearance_metres: f32,
pub variance_m2: f32,
pub measurement_time_uncertainty_microseconds: u32,
pub upper_error_metres: f32,
}
impl GroundClearance {
/// Bound current clearance using current foot orientation and velocity
/// confidence. `upper_error_metres` bounds measured COM height, not a foot
/// projection frozen at the range timestamp.
pub fn upper_at(
self,
now: Timestamp,
estimate: &FlightEstimate,
contact: ContactGeometry,
) -> Option<f32> {
let speed_bound =
estimate.velocity_error_bound().norm() + estimate.velocity.vector().norm();
let angle_bound = estimate.attitude_error_bound().norm();
if now >= self.valid_until
|| estimate.at != now
|| !speed_bound.is_finite()
|| !angle_bound.is_finite()
|| !contact.validate()
{
return None;
}
let earliest = self
.measurement_at
.as_microseconds()
.checked_sub(u64::from(self.measurement_time_uncertainty_microseconds))?;
let age = now.as_microseconds().checked_sub(earliest)? as f32 * 1e-6;
// Configuration checks the vehicle's free-flight acceleration bound.
let feet = contact.depth(estimate.attitude);
let upper = self.body_origin_height_metres
+ self.upper_error_metres
+ speed_bound * age
+ 0.5 * MAXIMUM_FREE_ACCELERATION_MPS2 * age * age
- feet
+ contact.rotation_radius_bound() * angle_bound;
upper.is_finite().then_some(upper)
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct FlightEstimate {
pub at: Timestamp,
pub fused_through: Timestamp,
/// Latest complete IMU acquisition-window end, not its start or receipt.
pub last_imu_at: Timestamp,
pub origin: LocalOrigin,
pub attitude: UnitQuaternion<Frd, Ned>,
pub position: PositionNed,
pub velocity: VelocityNed,
/// Bias-corrected average of the latest complete declared window. This is not
/// an instantaneous angular-rate measurement at `at`.
pub angular_rate: AngularRateFrd,
pub position_variance_m2: [f32; 3],
pub velocity_variance_m2ps2: [f32; 3],
pub attitude_variance_rad2: [f32; 3],
pub timing_error_bound: TimingErrorBounds,
pub capabilities: FlightCapabilities,
pub ground_clearance: Option<GroundClearance>,
}
/// Deterministic absolute error radii from timing, separate from Gaussian P.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct TimingErrorBounds {
pub position_metres: [f32; 3],
pub velocity_mps: [f32; 3],
pub attitude_radians: [f32; 3],
}
impl TimingErrorBounds {
pub const ZERO: Self = Self {
position_metres: [0.0; 3],
velocity_mps: [0.0; 3],
attitude_radians: [0.0; 3],
};
}
impl FlightEstimate {
pub fn velocity_error_bound(&self) -> Vector3<Ned> {
let e: [f32; 3] = core::array::from_fn(|i| {
3.0 * kaptan_math::sqrt(self.velocity_variance_m2ps2[i])
+ self.timing_error_bound.velocity_mps[i]
});
Vector3::new(e[0], e[1], e[2])
}
pub fn attitude_error_bound(&self) -> Vector3<Ned> {
let e: [f32; 3] = core::array::from_fn(|i| {
3.0 * kaptan_math::sqrt(self.attitude_variance_rad2[i])
+ self.timing_error_bound.attitude_radians[i]
});
Vector3::new(e[0], e[1], e[2])
}
}
/// Four equal contact spheres at the corners of a body-FRD rectangle.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct ContactGeometry {
pub half_span_x_metres: f32,
pub half_span_y_metres: f32,
pub centre_z_metres: f32,
pub radius_metres: f32,
}
impl ContactGeometry {
pub(crate) fn validate(self) -> bool {
self.half_span_x_metres.is_finite()
&& self.half_span_x_metres > 0.0
&& self.half_span_y_metres.is_finite()
&& self.half_span_y_metres > 0.0
&& self.centre_z_metres.is_finite()
&& self.radius_metres.is_finite()
&& self.radius_metres >= 0.0
&& (self.rotation_radius_bound() + self.radius_metres).is_finite()
}
fn rotation_radius_bound(self) -> f32 {
l1_bound(Vector3::new(
self.half_span_x_metres,
self.half_span_y_metres,
self.centre_z_metres,
))
}
pub(crate) fn depth(self, q: UnitQuaternion<Frd, Ned>) -> f32 {
let mut depth = f32::NEG_INFINITY;
for x in [-self.half_span_x_metres, self.half_span_x_metres] {
for y in [-self.half_span_y_metres, self.half_span_y_metres] {
let foot = q.body_to_ned(Vector3::new(x, y, self.centre_z_metres)).z()
+ self.radius_metres;
if !foot.is_finite() {
return f32::NAN;
}
depth = depth.max(foot);
}
}
depth
}
}
/// Selected source for each supported observation kind.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct NavigationSources {
pub imu: SourceId,
pub local_position_velocity: SourceId,
pub magnetic: SourceId,
pub pressure: SourceId,
pub range: SourceId,
}
impl NavigationSources {
pub(crate) const fn ordered(self) -> [SourceId; 5] {
[
self.imu,
self.local_position_velocity,
self.magnetic,
self.pressure,
self.range,
]
}
}
/// Range-sensor extrinsics and deterministic quantization error.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct RangeInstallation {
pub frame: FrameId,
pub offset_frd_metres: Vector3<Frd>,
/// Finite nonzero beam direction, normalized once by navigation construction.
pub direction_frd: Vector3<Frd>,
pub quantization_error_metres: f32,
}
impl RangeInstallation {
fn validate(self) -> bool {
self.offset_frd_metres.is_finite()
&& l1_bound(self.offset_frd_metres).is_finite()
&& self.direction_frd.normalized().is_some()
&& self.quantization_error_metres.is_finite()
&& self.quantization_error_metres >= 0.0
}
pub(crate) fn lever_bound(self) -> f32 {
l1_bound(self.offset_frd_metres)
}
}
// The L1 norm bounds Euclidean radius. Round both additions outward.
fn l1_bound(v: Vector3<Frd>) -> f32 {
((v.x().abs() + v.y().abs()).next_up() + v.z().abs()).next_up()
}
/// Local isothermal pressure reference. The conversion accepts 0.95..1.05 of it.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct BarometricReference {
pub pressure_pascals: f32,
pub temperature_kelvin: f32,
}
impl BarometricReference {
pub(crate) fn height_scale(self) -> f32 {
287.05 * self.temperature_kelvin / GRAVITY_MPS2
}
fn validate(self) -> bool {
let lower = 0.95 * self.pressure_pascals;
let upper = 1.05 * self.pressure_pascals;
let scale = self.height_scale();
self.pressure_pascals.is_finite()
&& self.pressure_pascals > 0.0
&& self.temperature_kelvin.is_finite()
&& self.temperature_kelvin > 0.0
&& (self.pressure_pascals + upper).is_finite()
&& scale.is_finite()
&& (scale / lower).is_finite()
&& scale / upper > 0.0
}
}
/// Required compiled installation. Calibration identifies one coherent set of
/// mounting, calibration and uncertainty data, changed only by reinitialization.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct NavigationInstallation {
pub sources: NavigationSources,
pub range: RangeInstallation,
pub contact: ContactGeometry,
pub atmosphere: BarometricReference,
pub magnetic_field_ned_microtesla: Vector3<Ned>,
pub calibration: CalibrationRevision,
pub reference: SensorReferenceId,
}
impl NavigationInstallation {
fn validate(self) -> bool {
self.range.validate()
&& self.contact.validate()
&& self.atmosphere.validate()
&& self.magnetic_field_ned_microtesla.is_finite()
&& Vector3::<Ned>::new(
self.magnetic_field_ned_microtesla.x(),
self.magnetic_field_ned_microtesla.y(),
0.0,
)
.norm()
> 10.0
&& self.magnetic_field_ned_microtesla.norm() < 100.0
&& self.reference.0 != 0
}
}
/// Continuous process-noise densities in SI units.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct NoiseDensities {
pub gyro: f32, // rad²/s
pub acceleration: f32, // m²/s³
pub gyro_bias: f32, // rad²/s³
pub acceleration_bias: f32, // m²/s⁵
pub barometric_datum: f32, // m²/s
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct NavigationConfig {
pub noise: NoiseDensities,
/// Acquisition-window, numerical-step and current-extrapolation ceiling.
pub maximum_imu_interval_microseconds: u64,
pub imu_freshness_microseconds: u64,
pub aiding_freshness_microseconds: u64,
pub maximum_time_uncertainty_microseconds: u32,
pub lag_microseconds: u64,
pub installation: NavigationInstallation,
}
impl NavigationConfig {
#[cfg(test)]
pub const fn engineering_fixture() -> Self {
Self {
noise: NoiseDensities {
gyro: 2.5e-7,
acceleration: 2.5e-5,
gyro_bias: 1e-10,
acceleration_bias: 1e-8,
barometric_datum: 1e-5,
},
maximum_imu_interval_microseconds: 20_000,
imu_freshness_microseconds: 20_000,
aiding_freshness_microseconds: 250_000,
maximum_time_uncertainty_microseconds: 2_000,
lag_microseconds: 100_000,
installation: NavigationInstallation {
sources: NavigationSources {
imu: SourceId::new(1),
local_position_velocity: SourceId::new(2),
magnetic: SourceId::new(3),
pressure: SourceId::new(4),
range: SourceId::new(5),
},
range: RangeInstallation {
frame: FrameId::new(3),
offset_frd_metres: Vector3::new(0.0, 0.0, 0.04),
direction_frd: Vector3::new(0.0, 0.0, 1.0),
quantization_error_metres: 0.0005,
},
contact: ContactGeometry {
half_span_x_metres: 0.12,
half_span_y_metres: 0.12,
centre_z_metres: 0.065,
radius_metres: 0.015,
},
atmosphere: BarometricReference {
pressure_pascals: 101_325.0,
temperature_kelvin: 288.15,
},
magnetic_field_ned_microtesla: Vector3::new(20.0, 0.0, 45.0),
calibration: CalibrationRevision::new(1),
reference: SensorReferenceId(1),
},
}
}
pub fn validate(self) -> bool {
let n = self.noise;
[
n.gyro,
n.acceleration,
n.gyro_bias,
n.acceleration_bias,
n.barometric_datum,
]
.iter()
.all(|v| v.is_finite() && *v > 0.0 && *v <= 1.0)
&& (10_000..=20_000).contains(&self.maximum_imu_interval_microseconds)
&& self.imu_freshness_microseconds >= self.maximum_imu_interval_microseconds
&& self.imu_freshness_microseconds <= 20_000
&& self.lag_microseconds == 100_000
&& (200_000..=500_000).contains(&self.aiding_freshness_microseconds)
&& self.maximum_time_uncertainty_microseconds <= 2_000
&& self.installation.validate()
}
}
pub(crate) fn earliest(c: ObservationContext) -> Option<u64> {
c.measurement_time
.as_microseconds()
.checked_sub(u64::from(c.time_uncertainty_microseconds))
}
pub(crate) fn expiry(c: ObservationContext, age: u64) -> Option<Timestamp> {
Some(Timestamp::from_microseconds(earliest(c)?.checked_add(age)?))
}
#[cfg(test)]
mod tests {
use super::*;
fn contact() -> ContactGeometry {
NavigationConfig::engineering_fixture().installation.contact
}
fn time(v: u64) -> Timestamp {
Timestamp::from_microseconds(v)
}
fn estimate(at: Timestamp) -> FlightEstimate {
let capability = CapabilityEvidence {
available: true,
last_measurement_at: Some(at),
valid_until: Some(time(u64::MAX)),
};
FlightEstimate {
at,
fused_through: at,
last_imu_at: at,
origin: LocalOrigin {
id: OriginId(1),
established_at: at,
reference: SensorReferenceId(1),
launch_in_reference: PositionNed::metres(0.0, 0.0, 0.0),
launch_variance_m2: [0.0; 3],
},
attitude: UnitQuaternion::identity(),
position: PositionNed::metres(0.0, 0.0, 0.0),
velocity: VelocityNed::metres_per_second(0.1, 0.0, 0.0),
angular_rate: AngularRateFrd::radians_per_second(0.0, 0.0, 0.0),
position_variance_m2: [0.0; 3],
velocity_variance_m2ps2: [0.0; 3],
attitude_variance_rad2: [0.0; 3],
timing_error_bound: TimingErrorBounds::ZERO,
capabilities: FlightCapabilities {
tilt: capability,
heading: capability,
horizontal_position: capability,
horizontal_velocity: capability,
height: capability,
vertical_velocity: capability,
ground_clearance: capability,
},
ground_clearance: None,
}
}
#[test]
fn capability_expiry_requires_every_requested_capability() {
let evidence = CapabilityEvidence {
available: true,
last_measurement_at: Some(time(10)),
valid_until: Some(time(100)),
};
let healthy = FlightCapabilities {
tilt: evidence,
heading: evidence,
horizontal_position: evidence,
horizontal_velocity: evidence,
height: evidence,
vertical_velocity: evidence,
ground_clearance: evidence,
};
assert_eq!(healthy.valid_until(), Some(time(100)));
for index in 0..7 {
let mut c = healthy;
[
&mut c.tilt,
&mut c.heading,
&mut c.horizontal_position,
&mut c.horizontal_velocity,
&mut c.height,
&mut c.vertical_velocity,
&mut c.ground_clearance,
]
.into_iter()
.nth(index)
.unwrap()
.available = false;
assert!(!c.flight_ready());
assert_eq!(c.valid_until(), None);
}
let mut c = healthy;
c.ground_clearance.valid_until = None;
assert_eq!(c.valid_until(), None);
c = healthy;
c.heading.valid_until = Some(time(50));
assert_eq!(c.valid_until(), Some(time(50)));
}
#[test]
fn clearance_bound_uses_earliest_capture_and_rejects_invalid_arithmetic() {
let c = GroundClearance {
measurement_at: time(10_000),
valid_until: time(60_000),
source: SourceId::new(5),
body_origin_height_metres: 0.08,
contact_clearance_metres: 0.0,
variance_m2: 4e-6,
measurement_time_uncertainty_microseconds: 1_000,
upper_error_metres: 0.01,
};
let e = estimate(time(20_000));
assert!((c.upper_at(time(20_000), &e, contact()).unwrap() - 0.012_612_5).abs() < 1e-7);
assert_eq!(
c.upper_at(time(60_000), &estimate(time(60_000)), contact()),
None
);
for speed in [f32::NAN, f32::INFINITY] {
let mut bad = e;
bad.velocity = VelocityNed::metres_per_second(speed, 0.0, 0.0);
assert_eq!(c.upper_at(time(20_000), &bad, contact()), None);
}
assert_eq!(
c.upper_at(time(8_000), &estimate(time(8_000)), contact()),
None
);
assert_eq!(c.upper_at(time(20_001), &e, contact()), None);
let mut bad_angle = e;
bad_angle.attitude_variance_rad2[0] = f32::NAN;
assert_eq!(c.upper_at(time(20_000), &bad_angle, contact()), None);
let mut invalid_contact = contact();
invalid_contact.radius_metres = -0.01;
assert_eq!(c.upper_at(time(20_000), &e, invalid_contact), None);
assert_eq!(
GroundClearance {
measurement_time_uncertainty_microseconds: 20_000,
..c
}
.upper_at(time(20_000), &e, contact()),
None
);
assert_eq!(
GroundClearance {
valid_until: time(3_000_000),
..c
}
.upper_at(
time(2_000_000),
&FlightEstimate {
at: time(2_000_000),
velocity: VelocityNed::metres_per_second(f32::MAX, 0.0, 0.0),
..e
},
contact(),
),
None
);
}
#[test]
fn current_foot_geometry_and_velocity_confidence_bound_landing_clearance() {
// Independent rigid geometry: fixed COM 0.132 m; foot-sphere centres
// (+/-0.12,+/-0.12,0.065), radius 0.015. Roll changes 0.2 -> 0.
let old_depth = 0.12_f64 * 0.2_f64.sin() + 0.065 * 0.2_f64.cos() + 0.015;
let measured_contact = 0.132 - old_depth;
let old_bound = measured_contact + 0.0075 + 0.2 * 0.020 + 0.5 * 25.0 * 0.020_f64.powi(2);
assert!(old_bound < 0.05);
let actual_now = 0.132 - (0.065 + 0.015);
let c = GroundClearance {
measurement_at: time(10_000),
valid_until: time(70_000),
source: SourceId::new(5),
body_origin_height_metres: 0.132,
contact_clearance_metres: measured_contact as f32,
variance_m2: 4e-6,
measurement_time_uncertainty_microseconds: 0,
upper_error_metres: 0.0075,
};
let mut e = estimate(time(30_000));
e.velocity = VelocityNed::metres_per_second(0.0, 0.0, 0.0);
let upper = c.upper_at(e.at, &e, contact()).unwrap();
assert!(f64::from(upper) >= actual_now);
assert!(upper > 0.05); // neither cap entry nor contact dwell may use the old bound
e.velocity_variance_m2ps2 = [0.04; 3];
let uncertain = c.upper_at(e.at, &e, contact()).unwrap();
assert!(uncertain > upper + 0.020); // 3*sqrt(trace(Pv))*20 ms, not fixed +0.2 m/s
}
#[test]
fn contact_geometry() {
for geometry in [
contact(),
ContactGeometry {
half_span_x_metres: 0.3,
half_span_y_metres: 0.2,
centre_z_metres: 0.15,
radius_metres: 0.025,
},
] {
let x = f64::from(geometry.half_span_x_metres);
let y = f64::from(geometry.half_span_y_metres);
let z = f64::from(geometry.centre_z_metres);
assert!(f64::from(geometry.rotation_radius_bound()) >= (x * x + y * y + z * z).sqrt());
for (roll, pitch) in [(-0.5_f64, 0.2_f64), (0.0, 0.0), (0.4, -0.3)] {
let (sr, cr) = ((roll / 2.0).sin(), (roll / 2.0).cos());
let (sp, cp) = ((pitch / 2.0).sin(), (pitch / 2.0).cos());
let q = UnitQuaternion::from_components(
(cp * cr) as f32,
(cp * sr) as f32,
(sp * cr) as f32,
(-sp * sr) as f32,
)
.unwrap();
let expected = x * pitch.sin().abs()
+ y * (pitch.cos() * roll.sin()).abs()
+ z * pitch.cos() * roll.cos()
+ f64::from(geometry.radius_metres);
assert!((f64::from(geometry.depth(q)) - expected).abs() < 1e-7);
let e = FlightEstimate {
attitude: q,
..estimate(time(10_000))
};
let c = GroundClearance {
measurement_at: e.at,
valid_until: time(70_000),
source: SourceId::new(90),
body_origin_height_metres: 1.0,
contact_clearance_metres: 0.0,
variance_m2: 1e-6,
measurement_time_uncertainty_microseconds: 0,
upper_error_metres: 0.001,
};
assert!(f64::from(c.upper_at(e.at, &e, geometry).unwrap()) >= 1.0 - expected);
}
}
}
#[test]
fn installation_validation() {
let fixture = NavigationConfig::engineering_fixture();
for field in 0..9 {
for bad in [f32::NAN, f32::INFINITY, -1.0] {
let mut config = fixture;
let i = &mut config.installation;
match field {
0 => i.contact.half_span_x_metres = bad,
1 => i.contact.half_span_y_metres = bad,
2 => i.contact.radius_metres = bad,
3 => i.range.quantization_error_metres = bad,
4 => i.atmosphere.pressure_pascals = bad,
5 => i.atmosphere.temperature_kelvin = bad,
6 if !bad.is_finite() => i.contact.centre_z_metres = bad,
7 if !bad.is_finite() => {
i.range.offset_frd_metres = Vector3::new(bad, 0.0, 0.0)
}
8 if !bad.is_finite() => i.range.direction_frd = Vector3::new(0.0, 0.0, bad),
_ => continue,
}
assert!(Navigation::new(config).is_err());
}
}
for field in 0..6 {
let mut config = fixture;
let i = &mut config.installation;
match field {
0 => i.range.direction_frd = Vector3::zero(),
1 => i.contact.half_span_x_metres = 0.0,
2 => i.contact.half_span_y_metres = 0.0,
3 => i.atmosphere.pressure_pascals = 0.0,
4 => i.atmosphere.temperature_kelvin = 0.0,
_ => i.atmosphere.pressure_pascals = f32::MAX,
}
assert!(Navigation::new(config).is_err());
}
let mut config = fixture;
config.installation.contact.half_span_x_metres = f32::MAX;
config.installation.contact.half_span_y_metres = f32::MAX;
assert!(Navigation::new(config).is_err());
config = fixture;
config.installation.range.offset_frd_metres = Vector3::new(f32::MAX, f32::MAX, 0.0);
assert!(Navigation::new(config).is_err());
for (pressure_pascals, temperature_kelvin) in [
(101_325.0, f32::MAX),
(f32::MIN_POSITIVE, 288.15),
(1e20, f32::from_bits(1)),
] {
config = fixture;
config.installation.atmosphere = BarometricReference {
pressure_pascals,
temperature_kelvin,
};
assert!(Navigation::new(config).is_err());
}
}
#[test]
fn navigation_configuration_rejects_each_invalid_domain() {
let factory = NavigationConfig::engineering_fixture();
assert!(factory.validate());
for index in 0..5 {
for bad in [f32::NAN, 0.0, -1.0, 1.01] {
let mut c = factory;
*[
&mut c.noise.gyro,
&mut c.noise.acceleration,
&mut c.noise.gyro_bias,
&mut c.noise.acceleration_bias,
&mut c.noise.barometric_datum,
]
.into_iter()
.nth(index)
.unwrap() = bad;
assert!(!c.validate());
assert!(crate::Navigation::new(c).is_err());
}
}
let mut c = factory;
c.maximum_imu_interval_microseconds = 9_999;
assert!(!c.validate());
c = factory;
c.imu_freshness_microseconds = 19_999;
assert!(!c.validate());
c = factory;
c.imu_freshness_microseconds = 20_001;
assert!(!c.validate());
c = factory;
c.lag_microseconds = 0;
assert!(!c.validate());
c = factory;
c.aiding_freshness_microseconds = 100_000;
assert!(!c.validate());
c = factory;
c.maximum_time_uncertainty_microseconds = 2_001;
assert!(!c.validate());
c = factory;
c.installation.magnetic_field_ned_microtesla = Vector3::new(f32::NAN, 0.0, 45.0);
assert!(!c.validate());
c = factory;
c.installation.magnetic_field_ned_microtesla = Vector3::new(10.0, 0.0, 45.0);
assert!(!c.validate());
c = factory;
c.installation.magnetic_field_ned_microtesla = Vector3::new(20.0, f32::MAX, 45.0);
assert!(!c.validate());
c = factory;
c.installation.reference = SensorReferenceId(0);
assert!(!c.validate());
c = factory;
c.installation.magnetic_field_ned_microtesla = Vector3::new(0.0, 20.0, 45.0);
assert!(c.validate());
}
}
firmware/libs/kaptan-navigation/src/lib.rs
+17 -31
View File
@@ -1,41 +1,27 @@
//! Navigation observation contracts and a SITL-only raw-rate ESKF.
//!
//! Navigation owns the conversion from driver integer observations into SI
//! `f32` values. Board and platform crates must not manufacture navigation
//! observations or duplicate their frame/calibration metadata.
//! Allocation-free sensor-driven navigation shared by host and MCU endpoints.
#![no_std]
#![forbid(unsafe_code)]
#[cfg(all(feature = "sitl", target_os = "none"))]
compile_error!(
"kaptan-navigation's raw-rate ESKF is restricted to the SITL executable; \
it is not available to a board target"
);
#[cfg(any(test, feature = "sitl"))]
mod eskf;
mod filter;
mod flight;
mod navigation;
mod observation;
mod timing;
pub use flight::*;
pub use navigation::Navigation;
pub use observation::{
AccelerationFrd, AngularRateFrd, CalibrationRevision, Covariance2, FrameId,
LocalPositionVelocityObservation, ObservationContext, ObservationSequence,
OpticalFlowObservation, PositionFrd, PositionNed, RangeObservation, ScalarVariance,
SensorExtrinsics, SourceId, Validity, VelocityNed,
AccelerationFrd, AngularRateFrd, CalibrationRevision, FrameId,
LocalPositionVelocityObservation, ObservationContext, ObservationSequence, PositionNed,
RangeObservation, ScalarVariance, SourceId, Validity, VelocityNed,
};
/// Internal-SITL API for the provisional raw-rate ESKF.
///
/// It supports the host simulator only. It cannot compile for a bare-metal
/// target and must not be used as a flight-estimator API.
#[cfg(any(test, feature = "sitl"))]
pub mod sitl {
pub use crate::eskf::{
Eskf, EstimatorCapabilities, EstimatorConfig, EstimatorHealth, FusionError, FusionResult,
InertialEskf, IntegrityConfig, LocalTerrainEskf, NominalState, PropagationError,
SourceStatus, StationaryInitializer,
};
pub use crate::observation::{ImuNoise, ImuObservation};
}
/// Standard gravity in metres per second squared.
pub const GRAVITY_MPS2: f32 = 9.806_65;
/// Supported free-flight acceleration bound in m/s², including gravity
/// and maximum collective thrust. Flight configuration checks T_max/m + g
/// against this bound. It does not bound contact impulses.
pub const MAXIMUM_FREE_ACCELERATION_MPS2: f32 = 25.0;
firmware/libs/kaptan-navigation/src/navigation.rs
+2771
View File
File diff suppressed because it is too large. Load diff
firmware/libs/kaptan-navigation/src/observation.rs
+45 -282
View File
@@ -1,15 +1,10 @@
//! Typed SI observations and their calibration/frame provenance.
use kaptan_math::{Frd, Ned, Sensor, UnitQuaternion, Vector2, Vector3};
#[cfg(any(test, feature = "sitl"))]
use kaptan_sensor::imu::ImuSnapshot;
#[cfg(any(test, feature = "sitl"))]
use kaptan_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot};
use kaptan_math::{Frd, Ned, Vector3};
#[cfg(any(test, feature = "sensor-snapshots"))]
use kaptan_sensor::range::RangeSnapshot;
use kaptan_time::Timestamp;
#[cfg(any(test, feature = "sitl"))]
use crate::GRAVITY_MPS2;
/// Origin of one observation stream, allocated by a static board configuration.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct SourceId(u8);
@@ -32,6 +27,7 @@ impl FrameId {
pub const BODY_FRD: Self = Self(0);
/// Active launch-local navigation frame with north/east/down axes.
pub const LOCAL_NED: Self = Self(1);
pub const SCALAR: Self = Self(2);
/// Creates a configured sensor frame identifier.
pub const fn new(value: u8) -> Self {
Self(value)
@@ -44,11 +40,9 @@ impl FrameId {
/// Monotonic revision of the calibration/extrinsics used for an observation.
///
/// TODO(configuration): this is a provenance token, not a calibration store.
/// The future configuration service must atomically install sensor mounting,
/// scale/bias calibration, uncertainty, and this revision while navigation is
/// stopped, then require reinitialization before admitting observations from
/// the new revision.
/// This token identifies a coherent mounting, calibration and uncertainty set.
/// Changing the set requires navigation to stop and reinitialize before it
/// accepts observations carrying the new revision.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct CalibrationRevision(u32);
impl CalibrationRevision {
@@ -86,6 +80,13 @@ pub enum Validity {
}
/// Common provenance for every navigation observation.
///
/// The caller must map physical sample time into the controller clock, accounting
/// for sensor delay. A snapshot's request or reception timestamp is insufficient.
/// Without that mapping, withhold the observation or mark it [`Validity::Invalid`].
/// Receipt time must be at or after the snapshot timestamp in the same clock domain.
/// Snapshot conversion preserves these times and changes only units. The estimator
/// checks ordering and freshness on admission.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct ObservationContext {
/// Sensor measurement time synchronized into the controller monotonic domain.
@@ -104,32 +105,8 @@ pub struct ObservationContext {
pub validity: Validity,
/// Source sequence number.
pub sequence: ObservationSequence,
}
impl ObservationContext {
/// Creates provenance shared by a concrete observation value.
#[allow(clippy::too_many_arguments)]
pub const fn new(
measurement_time: Timestamp,
receipt_time: Timestamp,
source: SourceId,
frame: FrameId,
calibration: CalibrationRevision,
quality: u8,
validity: Validity,
sequence: ObservationSequence,
) -> Self {
Self {
measurement_time,
receipt_time,
source,
frame,
calibration,
quality,
validity,
sequence,
}
}
/// Hard absolute clock-mapping error, not a statistical standard deviation.
pub time_uncertainty_microseconds: u32,
}
/// Positive finite scalar measurement variance in squared SI units.
@@ -150,29 +127,13 @@ impl ScalarVariance {
}
}
/// Symmetric two-dimensional covariance in squared SI units.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Covariance2 {
/// xx variance.
pub xx: ScalarVariance,
/// xy covariance.
pub xy: f32,
/// yy variance.
pub yy: ScalarVariance,
}
impl Covariance2 {
/// Validates finite covariance and a positive determinant.
pub fn new(xx: ScalarVariance, xy: f32, yy: ScalarVariance) -> Option<Self> {
let determinant = xx.get() * yy.get() - xy * xy;
if xy.is_finite() && determinant.is_finite() && determinant > 0.0 {
Some(Self { xx, xy, yy })
} else {
None
}
}
}
/// NED position in metres.
///
/// Position and velocity remain distinct even in the same coordinate frame.
/// ```compile_fail,E0308
/// use kaptan_navigation::{PositionNed, VelocityNed};
/// let position: PositionNed = VelocityNed::metres_per_second(0.0, 0.0, 0.0);
/// ```
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct PositionNed(Vector3<Ned>);
impl PositionNed {
@@ -203,8 +164,7 @@ impl VelocityNed {
/// Complete position and velocity aiding in the active local-NED frame.
///
/// Position and velocity share one source timestamp and sequence because the
/// producer measures the same vehicle state. Their diagonal variances remain
/// explicit rather than being hidden in a simulator or transport adapter.
/// producer measures the same vehicle state. Variances use squared SI units.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct LocalPositionVelocityObservation {
/// Required source/time/frame provenance.
@@ -214,23 +174,11 @@ pub struct LocalPositionVelocityObservation {
/// Absolute local-NED velocity in metres per second.
pub velocity: VelocityNed,
/// Per-axis position measurement variance in m².
pub position_variance: ScalarVariance,
pub position_variance: [ScalarVariance; 3],
/// Per-axis velocity measurement variance in (m/s)².
pub velocity_variance: ScalarVariance,
}
/// Body FRD position offset in metres.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct PositionFrd(Vector3<Frd>);
impl PositionFrd {
/// Creates a body-frame offset in metres.
pub const fn metres(forward: f32, right: f32, down: f32) -> Self {
Self(Vector3::new(forward, right, down))
}
/// Returns its body vector.
pub const fn vector(self) -> Vector3<Frd> {
self.0
}
pub velocity_variance: [ScalarVariance; 3],
/// Fixed sensor-reference coordinates. Navigation alone subtracts launch.
pub reference: crate::SensorReferenceId,
}
/// Body FRD specific force in metres per second squared.
@@ -261,57 +209,6 @@ impl AngularRateFrd {
}
}
/// Configured IMU white-noise variances in SI squared units.
#[derive(Clone, Copy, Debug, PartialEq)]
#[cfg(any(test, feature = "sitl"))]
pub struct ImuNoise {
/// Gyro rate variance, (rad/s)^2.
pub gyro_rate: ScalarVariance,
/// Accelerometer specific-force variance, (m/s²)^2.
pub acceleration: ScalarVariance,
}
/// IMU observation expressed in the calibrated aircraft FRD frame.
#[derive(Clone, Copy, Debug, PartialEq)]
#[cfg(any(test, feature = "sitl"))]
pub struct ImuObservation {
/// Required observation provenance.
pub context: ObservationContext,
/// Specific force in m/s².
pub acceleration: AccelerationFrd,
/// Angular rate in rad/s.
pub angular_rate: AngularRateFrd,
/// Explicit configured uncertainty.
pub noise: ImuNoise,
}
#[cfg(any(test, feature = "sitl"))]
impl ImuObservation {
/// Converts the existing integer IMU contract to SI exactly at the
/// navigation boundary. The board has already aligned this snapshot to FRD.
pub fn from_snapshot(
snapshot: ImuSnapshot,
mut context: ObservationContext,
noise: ImuNoise,
) -> Self {
context.measurement_time = snapshot.timestamp;
Self {
context,
acceleration: AccelerationFrd::metres_per_second_squared(
f32::from(snapshot.acceleration_milligravity[0]) * GRAVITY_MPS2 / 1_000.0,
f32::from(snapshot.acceleration_milligravity[1]) * GRAVITY_MPS2 / 1_000.0,
f32::from(snapshot.acceleration_milligravity[2]) * GRAVITY_MPS2 / 1_000.0,
),
angular_rate: AngularRateFrd::radians_per_second(
f32::from(snapshot.angular_rate_milliradians_per_second[0]) / 1_000.0,
f32::from(snapshot.angular_rate_milliradians_per_second[1]) / 1_000.0,
f32::from(snapshot.angular_rate_milliradians_per_second[2]) / 1_000.0,
),
noise,
}
}
}
/// Range observation in metres along the sensor's positive z axis.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct RangeObservation {
@@ -325,13 +222,14 @@ pub struct RangeObservation {
impl RangeObservation {
/// Converts a driver range value to metres at the navigation boundary.
#[cfg(any(test, feature = "sitl"))]
///
/// Uses the caller's timestamps under the [`ObservationContext`] timing contract.
#[cfg(any(test, feature = "sensor-snapshots"))]
pub fn from_snapshot(
snapshot: RangeSnapshot,
mut context: ObservationContext,
context: ObservationContext,
variance: ScalarVariance,
) -> Self {
context.measurement_time = snapshot.timestamp;
Self {
context,
metres: snapshot.distance_millimetres as f32 / 1_000.0,
@@ -340,115 +238,23 @@ impl RangeObservation {
}
}
/// Ground-referenced XY velocity reported in a named sensor frame.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct OpticalFlowObservation {
/// Required observation provenance.
pub context: ObservationContext,
/// Sensor-frame x/y velocity in m/s.
pub velocity: Vector2<Sensor>,
/// Ground distance in metres; retained for plausibility/diagnostics.
pub ground_distance_metres: f32,
/// Explicit XY measurement covariance in (m/s)².
pub covariance: Covariance2,
}
impl OpticalFlowObservation {
/// Converts the existing MTF-01-independent driver value into SI.
#[cfg(any(test, feature = "sitl"))]
pub fn from_snapshot(
snapshot: OpticalFlowSnapshot,
mut context: ObservationContext,
covariance: Covariance2,
) -> Self {
context.measurement_time = snapshot.timestamp;
context.quality = snapshot.quality;
Self {
context,
velocity: Vector2::new(
snapshot.velocity_millimetres_per_second[0] as f32 / 1_000.0,
snapshot.velocity_millimetres_per_second[1] as f32 / 1_000.0,
),
ground_distance_metres: snapshot.ground_distance_millimetres as f32 / 1_000.0,
covariance,
}
}
}
/// Static calibrated transform from a sensor frame into aircraft FRD.
///
/// TODO(configuration): this remains a build-time value until the
/// configuration service can stage the full sensor calibration bundle. Do not
/// expose setters for individual fields: source identity, frame, calibration
/// revision, rotation, and lever arm must change together and only while the
/// estimator and actuators are inhibited.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct SensorExtrinsics {
/// Statically configured source expected for this physical device.
pub source: SourceId,
/// Frame ID expected in every observation from this device.
pub sensor_frame: FrameId,
/// Calibration/extrinsics revision represented by this transform.
pub calibration: CalibrationRevision,
/// Rotation mapping sensor vectors into FRD.
pub sensor_to_body: UnitQuaternion<Sensor, Frd>,
/// Sensor origin relative to aircraft origin, in FRD metres.
pub position_body: PositionFrd,
}
impl SensorExtrinsics {
/// Maps a sensor vector into FRD at the sole sensor-calibration boundary.
pub fn sensor_to_body_vector(self, value: Vector3<Sensor>) -> Vector3<Frd> {
self.sensor_to_body.rotate(value)
}
}
#[cfg(test)]
mod tests {
use super::*;
use kaptan_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot};
use kaptan_sensor::range::RangeSnapshot;
fn context(frame: FrameId) -> ObservationContext {
ObservationContext::new(
Timestamp::from_microseconds(10),
Timestamp::from_microseconds(12),
SourceId::new(2),
ObservationContext {
measurement_time: Timestamp::from_microseconds(95),
receipt_time: Timestamp::from_microseconds(120),
source: SourceId::new(2),
frame,
CalibrationRevision::new(3),
250,
Validity::Valid,
ObservationSequence::new(9),
)
}
#[test]
fn imu_snapshot_is_si() {
let noise = ImuNoise {
gyro_rate: ScalarVariance::new(0.01).unwrap(),
acceleration: ScalarVariance::new(0.02).unwrap(),
};
let sample = ImuObservation::from_snapshot(
ImuSnapshot::new(
Timestamp::from_microseconds(10),
[1_000, 0, -1_000],
[1_000, 0, 0],
0,
),
context(FrameId::BODY_FRD),
noise,
);
assert!((sample.acceleration.vector().x() - GRAVITY_MPS2).abs() < 1.0e-5);
assert!((sample.angular_rate.vector().x() - 1.0).abs() < 1.0e-6);
assert_eq!(sample.context.calibration, CalibrationRevision::new(3));
}
#[test]
fn covariance_validation() {
let variance = ScalarVariance::new(1.0).unwrap();
assert!(Covariance2::new(variance, 1.0, variance).is_none());
let large = ScalarVariance::new(f32::MAX).unwrap();
assert!(Covariance2::new(large, 0.0, large).is_none());
assert!(Covariance2::new(variance, f32::NAN, variance).is_none());
calibration: CalibrationRevision::new(3),
quality: 250,
validity: Validity::Valid,
sequence: ObservationSequence::new(9),
time_uncertainty_microseconds: 0,
}
}
#[test]
@@ -463,55 +269,24 @@ mod tests {
}
#[test]
fn snapshots_convert_at_boundary() {
fn range_conversion_preserves_established_times() {
let variance = ScalarVariance::new(0.25).unwrap();
let range = RangeObservation::from_snapshot(
RangeSnapshot::new(Timestamp::from_microseconds(20), 1_234),
RangeSnapshot::new(Timestamp::from_microseconds(100), 1_234),
context(FrameId::new(4)),
variance,
);
assert_eq!(
range.context.measurement_time,
Timestamp::from_microseconds(20)
);
assert_eq!(range.context, context(FrameId::new(4)));
assert!((range.metres - 1.234).abs() < 1.0e-6);
let covariance = Covariance2::new(variance, 0.01, variance).unwrap();
let flow = OpticalFlowObservation::from_snapshot(
OpticalFlowSnapshot::new(Timestamp::from_microseconds(21), [-500, 250], 2_500, 42),
context(FrameId::new(5)),
covariance,
);
assert_eq!(
flow.context.measurement_time,
Timestamp::from_microseconds(21)
);
assert_eq!(flow.context.quality, 42);
assert_eq!((flow.velocity.x(), flow.velocity.y()), (-0.5, 0.25));
assert_eq!(flow.ground_distance_metres, 2.5);
}
#[test]
fn observation_frames() {
let noise = ImuNoise {
gyro_rate: ScalarVariance::new(0.01).unwrap(),
acceleration: ScalarVariance::new(0.02).unwrap(),
};
let sample = ImuObservation::from_snapshot(
ImuSnapshot::new(Timestamp::from_microseconds(10), [0; 3], [0; 3], 0),
context(FrameId::BODY_FRD),
noise,
);
assert_eq!(
sample.context.measurement_time,
Timestamp::from_microseconds(10)
);
assert_eq!(PositionNed::metres(1.0, 2.0, 3.0).vector().z(), 3.0);
assert_eq!(
VelocityNed::metres_per_second(1.0, 2.0, 3.0).vector().y(),
2.0
);
assert_eq!(PositionFrd::metres(1.0, 2.0, 3.0).vector().x(), 1.0);
assert_eq!(
AccelerationFrd::metres_per_second_squared(1.0, 2.0, 3.0)
.vector()
@@ -524,17 +299,5 @@ mod tests {
.z(),
3.0
);
let extrinsics = SensorExtrinsics {
source: SourceId::new(2),
sensor_frame: FrameId::new(5),
calibration: CalibrationRevision::new(3),
sensor_to_body: UnitQuaternion::identity(),
position_body: PositionFrd::metres(0.0, 0.0, 0.0),
};
assert_eq!(
extrinsics.sensor_to_body_vector(Vector3::new(1.0, 2.0, 3.0)),
Vector3::new(1.0, 2.0, 3.0)
);
}
}
firmware/libs/kaptan-navigation/src/timing.rs
+335
View File
@@ -0,0 +1,335 @@
//! Correlated, bounded timing-model error, separate from Gaussian covariance.
//!
//! `shape` encloses deterministic error in an ellipsoid. Linear transformations
//! preserve state correlations. Minkowski addition covers arbitrary correlation
//! between old and new bounded errors, including one persistent clock offset.
//! It is NOT independent measurement noise and is never added to Gaussian P.
use crate::{
NavigationFailure,
filter::{Matrix, N, ZERO, sandwich},
};
use kaptan_math::sqrt;
#[derive(Clone, Copy)]
pub(crate) struct TimingEnvelope {
shape: Matrix,
}
impl TimingEnvelope {
pub const ZERO: Self = Self { shape: ZERO };
pub fn valid(&self) -> bool {
self.shape.iter().flatten().all(|v| v.is_finite())
&& (0..N).all(|i| self.shape[i][i] >= 0.0)
}
pub fn radius(&self, axis: usize) -> f32 {
sqrt(self.shape[axis][axis])
}
pub fn norm_bound(&self, start: usize, count: usize) -> f32 {
// trace bounds the largest eigenvalue, so this bounds the Euclidean
// norm of the requested projection of the ellipsoid, not a sigma.
sqrt((start..start + count).map(|i| self.shape[i][i]).sum())
}
pub fn projected(&self, h: &[f32; N]) -> Result<f32, NavigationFailure> {
let mut squared = 0.0;
for (i, row) in self.shape.iter().enumerate() {
for (j, value) in row.iter().enumerate() {
squared += h[i] * value * h[j];
}
}
if !squared.is_finite() || squared < 0.0 {
return Err(NavigationFailure::Numerical);
}
Ok(sqrt(squared))
}
pub fn transform(&mut self, a: &Matrix) -> Result<(), NavigationFailure> {
if self.shape == ZERO {
return Ok(());
}
// Reject nonfinite shape before skipping zero coefficients, which would
// otherwise hide 0*Inf/NaN failures.
if !self.shape.iter().flatten().all(|v| v.is_finite()) {
return Err(NavigationFailure::Numerical);
}
let mut next = sandwich(a, &self.shape);
let columns: [f32; N] = core::array::from_fn(|k| a.iter().map(|r| r[k].abs()).sum());
for (i, row) in next.iter_mut().enumerate() {
let mut absolute_products = [0.0; N];
for (j, aj) in a[i].iter().enumerate() {
if *aj != 0.0 {
for (k, ap) in absolute_products.iter_mut().enumerate() {
*ap += aj.abs() * self.shape[j][k].abs();
}
}
}
let mut absolute_row = 0.0;
for (ap, column) in absolute_products.iter().zip(&columns) {
absolute_row += ap * column;
}
// Outward arithmetic enclosure, not a repair of Gaussian P. The
// 16-term products need < gamma_64; 256 eps covers the computed
// majorant and diagonal addition too. The absolute term also
// dominates underflow/flush-to-zero at these fixed matrix sizes.
row[i] = (row[i] + absolute_row * (256.0 * f32::EPSILON) + 1e-30).next_up();
}
self.commit(next)
}
pub fn add_generator(&mut self, g: [f32; N]) -> Result<(), NavigationFailure> {
if !g.iter().all(|v| v.is_finite()) {
return Err(NavigationFailure::Numerical);
}
if g.iter().all(|v| *v == 0.0) {
return Ok(());
}
let old_trace: f32 = (0..N).map(|i| self.shape[i][i]).sum();
let new_trace: f32 = g.iter().map(|v| v * v).sum();
if !old_trace.is_finite() || !new_trace.is_finite() {
return Err(NavigationFailure::Numerical);
}
let (old_weight, new_weight) = if old_trace == 0.0 {
(0.0, 1.0)
} else {
// Every positive beta is a valid Minkowski enclosure. This trace
// choice in the declared SI coordinates reduces its size. Limiting
// beta is numerical conditioning, not a limit on admitted timing.
let old_norm = sqrt(old_trace);
let new_norm = sqrt(new_trace.max(1e-30));
let beta = if new_norm > old_norm * 1e6 {
1e6
} else if new_norm < old_norm * 1e-6 {
1e-6
} else {
new_norm / old_norm
};
(1.0 + beta, 1.0 + 1.0 / beta)
};
let mut next = ZERO;
for (i, row) in next.iter_mut().enumerate() {
let mut magnitude = 0.0;
for (j, value) in row.iter_mut().enumerate() {
let left = old_weight * self.shape[i][j];
let right = new_weight * g[i] * g[j];
*value = left + right;
magnitude += left.abs() + right.abs();
}
row[i] = (row[i] + magnitude * (32.0 * f32::EPSILON) + 1e-30).next_up();
}
self.commit(next)
}
fn commit(&mut self, mut next: Matrix) -> Result<(), NavigationFailure> {
for i in 0..N {
for j in 0..i {
let value = next[i][j] * 0.5 + next[j][i] * 0.5;
next[i][j] = value;
next[j][i] = value;
}
}
let candidate = Self { shape: next };
if !candidate.valid() {
return Err(NavigationFailure::Numerical);
}
*self = candidate;
Ok(())
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::filter::identity;
impl TimingEnvelope {
fn former_transform(&mut self, a: &Matrix) -> Result<(), NavigationFailure> {
if self.shape == ZERO {
return Ok(());
}
let mut next = sandwich(a, &self.shape);
let columns: [f32; N] = core::array::from_fn(|k| a.iter().map(|r| r[k].abs()).sum());
for (i, row) in next.iter_mut().enumerate() {
let mut absolute_row = 0.0;
for (k, column) in columns.iter().enumerate() {
let mut ap = 0.0;
for (j, aj) in a[i].iter().enumerate() {
ap += aj.abs() * self.shape[j][k].abs();
}
absolute_row += ap * column;
}
// Outward arithmetic enclosure, not a repair of Gaussian P. The
// 16-term products need < gamma_64; 256 eps covers the computed
// majorant and diagonal addition too. The absolute term also
// dominates underflow/flush-to-zero at these fixed matrix sizes.
row[i] = (row[i] + absolute_row * (256.0 * f32::EPSILON) + 1e-30).next_up();
}
self.commit(next)
}
}
#[test]
fn sparse_absolute_products_preserve_the_dense_enclosure_and_refusal() {
fn compare(shape: Matrix, a: Matrix) {
let mut previous = TimingEnvelope { shape };
let mut current = previous;
assert_eq!(current.transform(&a), previous.former_transform(&a));
// Includes the retained input on refusal. Check signs of zero and
// original nonfinite payloads as well as every finite result bit.
for (actual, expected) in current
.shape
.iter()
.flatten()
.zip(previous.shape.iter().flatten())
{
assert_eq!(actual.to_bits(), expected.to_bits());
}
}
let mut sparse = identity();
sparse[0][3] = -0.02;
sparse[3][7] = 0.196133;
sparse[6][9] = -0.02;
let dense = core::array::from_fn(|i| {
core::array::from_fn(|j| ((i * 7 + j * 3) as f32 - 47.0) * 0.03125)
});
for scale in [
-0.0,
f32::from_bits(1),
f32::MIN_POSITIVE,
1e-20,
0.25,
1e20,
f32::MAX,
] {
let shape = core::array::from_fn(|i| {
core::array::from_fn(|j| if i == j { scale } else { scale * 0.01 })
});
for a in [ZERO, identity(), sparse, dense] {
compare(shape, a);
}
}
for bad in [f32::NAN, f32::INFINITY, f32::NEG_INFINITY] {
for (row, column) in [(0, 0), (12, 4)] {
let mut shape = identity();
shape[row][column] = bad;
for a in [ZERO, identity(), sparse, dense] {
compare(shape, a);
}
}
let mut a = identity();
a[0][1] = bad;
compare(ZERO, a); // The existing zero-shape early return stays first.
compare(identity(), a);
}
let mut huge = identity();
huge[0][0] = f32::MAX;
compare(identity(), huge);
}
#[test]
fn correlated_generators_and_linear_motion_enclose_independent_supports() {
// The exact set is a sum of line segments. Its support in any direction
// is sum(|h dot g|), regardless of whether clock offsets are correlated.
let generators: [[f32; N]; 3] = core::array::from_fn(|j| {
core::array::from_fn(|i| ((i * 7 + j * 11) as f32 - 50.0) * 0.0001)
});
let mut envelope = TimingEnvelope::ZERO;
for g in generators {
envelope.add_generator(g).unwrap();
}
let mut a = identity();
a[0][3] = 0.02;
a[3][7] = -0.196133;
a[6][9] = -0.02;
a[15][2] = 1.0;
envelope.transform(&a).unwrap();
for trial in 0..128 {
let h: [f32; N] =
core::array::from_fn(|i| (((trial + 1) * (i + 3) * 17 % 103) as f32 - 51.0) / 51.0);
let exact: f64 = generators
.iter()
.map(|g| {
(0..N)
.map(|i| {
f64::from(h[i])
* (0..N)
.map(|j| f64::from(a[i][j]) * f64::from(g[j]))
.sum::<f64>()
})
.sum::<f64>()
.abs()
})
.sum();
assert!(f64::from(envelope.projected(&h).unwrap()) >= exact);
}
assert!(envelope.norm_bound(0, 3) >= envelope.radius(0));
// A common position/datum offset cancels in the pressure observation.
// Diagonal independent-error treatment would lose this information.
let mut correlated = TimingEnvelope::ZERO;
let mut g = [0.0; N];
g[2] = 0.01;
g[15] = 0.01;
correlated.add_generator(g).unwrap();
let mut h = [0.0; N];
h[2] = -1.0;
h[15] = 1.0;
assert!(correlated.projected(&h).unwrap() < 0.0001);
}
#[test]
fn disparate_and_subnormal_generators_remain_outward() {
for (first, second) in [(1e-8, 1.0), (1.0, 1e-8), (1e-40, 1e-40)] {
let mut envelope = TimingEnvelope::ZERO;
let mut g = [0.0; N];
g[0] = first;
envelope.add_generator(g).unwrap();
g[0] = second;
envelope.add_generator(g).unwrap();
assert!(f64::from(envelope.radius(0)) >= f64::from(first) + f64::from(second));
}
let mut zero = TimingEnvelope::ZERO;
zero.add_generator([0.0; N]).unwrap();
zero.transform(&identity()).unwrap();
assert_eq!(zero.shape, ZERO);
}
#[test]
fn numerical_failures_preserve_the_timing_enclosure() {
let mut envelope = TimingEnvelope::ZERO;
let mut g = [0.0; N];
g[0] = 0.002;
envelope.add_generator(g).unwrap();
let before = envelope.shape;
for bad in [f32::NAN, f32::INFINITY, f32::MAX] {
g[0] = bad;
assert_eq!(envelope.add_generator(g), Err(NavigationFailure::Numerical));
assert_eq!(envelope.shape, before);
}
let mut a = identity();
a[0][0] = f32::MAX;
assert_eq!(envelope.transform(&a), Err(NavigationFailure::Numerical));
assert_eq!(envelope.shape, before);
let mut h = [0.0; N];
h[0] = f32::MAX;
assert_eq!(envelope.projected(&h), Err(NavigationFailure::Numerical));
// Exercise the finite screen independently of callers' Gaussian checks.
let mut invalid = TimingEnvelope { shape: identity() };
invalid.shape[0][0] = -1.0;
assert!(!invalid.valid());
h[0] = 1.0;
assert_eq!(invalid.projected(&h), Err(NavigationFailure::Numerical));
invalid.shape[0][0] = f32::INFINITY;
assert!(!invalid.valid());
invalid.shape = identity();
for (i, row) in invalid.shape.iter_mut().enumerate() {
row[i] = f32::MAX;
}
g[0] = 0.002;
assert_eq!(invalid.add_generator(g), Err(NavigationFailure::Numerical));
}
}
Loaded 100 of 211 files, more files were not shown because too many files have changed in this diff. Show more