From 13e4040ee3c53b6c7f534a321a3a63429d0c7fe3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Onur=20=C3=96zkan?= Date: Sun, 9 Aug 2026 22:08:13 +0300 Subject: [PATCH] feat(sitl): compose persisted configuration runtime --- .github/workflows/sitl-smoke.yml | 13 ++ tools/sitl/Cargo.toml | 12 ++ tools/sitl/src/config/logging.rs | 37 ++++ tools/sitl/src/config/mod.rs | 18 ++ tools/sitl/src/config/runtime.rs | 103 +++++++++ tools/sitl/src/config/store.rs | 176 +++++++++++++++ tools/sitl/src/config/tests.rs | 355 +++++++++++++++++++++++++++++++ tools/sitl/src/main.rs | 291 ++++++++++++++++++------- tools/sitl/src/protocol.rs | 3 +- 9 files changed, 930 insertions(+), 78 deletions(-) create mode 100644 tools/sitl/src/config/logging.rs create mode 100644 tools/sitl/src/config/mod.rs create mode 100644 tools/sitl/src/config/runtime.rs create mode 100644 tools/sitl/src/config/store.rs create mode 100644 tools/sitl/src/config/tests.rs diff --git a/.github/workflows/sitl-smoke.yml b/.github/workflows/sitl-smoke.yml index cddb5bc..019dcc9 100644 --- a/.github/workflows/sitl-smoke.yml +++ b/.github/workflows/sitl-smoke.yml @@ -21,6 +21,19 @@ jobs: run: | python3 -m unittest tools/sitl/test_kaptan_sim.py python3 tools/sitl/world-structure-test.py + cargo check --locked -p kaptan-mavlink --no-default-features + cargo check --locked -p kaptan-log --no-default-features + cargo check --locked -p kaptan-log-mavlink --no-default-features + cargo test --locked -p kaptan-log --features facade + cargo test --locked -p kaptan-log-derive -p kaptan-log-mavlink + if rg -n '(^use log::|\blog::)' platforms/stm32f7/src/f722; then + echo 'the standard log facade is forbidden in F722 IRQ/panic paths' + exit 1 + fi + if rg -n 'max_level_(off|error|warn|info|debug|trace)' --glob 'Cargo.toml' --glob '*.rs' .; then + echo 'log/max_level_* removes records before Kaptan can observe them' + exit 1 + fi cargo test --locked -p kaptan-sitl - name: Run Docker closed-loop smoke diff --git a/tools/sitl/Cargo.toml b/tools/sitl/Cargo.toml index 88a250b..ccbc27c 100644 --- a/tools/sitl/Cargo.toml +++ b/tools/sitl/Cargo.toml @@ -11,7 +11,19 @@ workspace = true [dependencies] kaptan-control = { path = "../../libs/kaptan-control" } +kaptan-config = { path = "../../libs/kaptan-config" } +kaptan-config-mavlink = { path = "../../libs/kaptan-config-mavlink", features = ["host-authority"] } +kaptan-flight-config = { path = "../../libs/kaptan-flight-config" } +kaptan-flight-runtime = { path = "../../libs/kaptan-flight-runtime" } +kaptan-log = { path = "../../libs/kaptan-log", features = ["facade"] } +kaptan-log-mavlink = { path = "../../libs/kaptan-log-mavlink" } +kaptan-mavlink = { path = "../../libs/kaptan-mavlink" } kaptan-math = { path = "../../libs/kaptan-math" } kaptan-navigation = { path = "../../libs/kaptan-navigation", features = ["sitl"] } kaptan-safety = { path = "../../libs/kaptan-safety" } kaptan-time = { path = "../../libs/kaptan-time" } +minicbor = "0.25" +log = { version = "0.4.33", default-features = false } + +[dev-dependencies] +mavlink = { version = "0.18", default-features = false, features = ["embedded"] } diff --git a/tools/sitl/src/config/logging.rs b/tools/sitl/src/config/logging.rs new file mode 100644 index 0000000..54b1833 --- /dev/null +++ b/tools/sitl/src/config/logging.rs @@ -0,0 +1,37 @@ +//! Host-only standard-log facade composition. + +use std::sync::Once; + +use kaptan_flight_runtime::RuntimeLogHub; +use kaptan_log::{FacadeIngress, GlobalLogger, LogFilter, Severity}; + +const SITL_LOG_INGRESS_CAPACITY: usize = 16; +static SITL_LOG_INGRESS: FacadeIngress = + FacadeIngress::new(LogFilter::new(Severity::Info)); +static SITL_LOGGER: GlobalLogger = + GlobalLogger::new(&SITL_LOG_INGRESS, sitl_forbidden_log_context); +static INSTALL_SITL_LOGGER: Once = Once::new(); + +const fn sitl_forbidden_log_context() -> bool { + false +} + +/// Installs the host-only standard logging facade once for the SITL process. +/// +/// SITL has no ISR, panic, or hard-fault context. Flight targets deliberately +/// do not enable this host-only feature. +pub fn install_log_facade() { + INSTALL_SITL_LOGGER.call_once(|| { + SITL_LOGGER + .install() + .expect("SITL owns the process-global log facade") + }); +} + +pub(super) fn set_filter(filter: LogFilter) { + SITL_LOG_INGRESS.set_filter(filter); +} + +pub(super) fn drain_one_into(hub: &mut RuntimeLogHub) { + let _ = SITL_LOG_INGRESS.drain_one_into(hub); +} diff --git a/tools/sitl/src/config/mod.rs b/tools/sitl/src/config/mod.rs new file mode 100644 index 0000000..d461ae7 --- /dev/null +++ b/tools/sitl/src/config/mod.rs @@ -0,0 +1,18 @@ +//! Host-only configuration composition for SITL. +//! +//! This module is deliberately not a firmware storage backend. Flight targets +//! need a reviewed A/B journal and authority path; SITL uses a same-directory +//! file replacement to make startup and configuration tests reproducible. + +mod logging; +mod runtime; +mod store; + +#[cfg(test)] +mod tests; + +pub use logging::install_log_facade; +pub use runtime::{CAPABILITIES, SitlConfigRuntime, SitlSnapshotImporter, configuration_service}; +pub use store::{ + FileProfileStore, FileStoreError, default_profile, load_profile, write_default_profile, +}; diff --git a/tools/sitl/src/config/runtime.rs b/tools/sitl/src/config/runtime.rs new file mode 100644 index 0000000..b692cae --- /dev/null +++ b/tools/sitl/src/config/runtime.rs @@ -0,0 +1,103 @@ +//! Central SITL configuration and protocol composition. + +use kaptan_config::ConfigManager; +use kaptan_config_mavlink::{ + Authorized, ConfigCommand, ConfigProtocol, ProtocolError, ServiceResponse, SnapshotImporter, + Target, +}; +use kaptan_flight_config::{ + CandidateProfile, CborError, FORMAT_VERSION, VehicleCapabilities, VehicleProfile, + decode_candidate, +}; +use kaptan_flight_runtime::FlightRuntime; +use kaptan_log::TransportAdmission; +use kaptan_log_mavlink::{DiagnosticPayload, encode_diagnostic}; + +use super::{FileProfileStore, logging}; + +/// SITL's immutable compiled capability contract. +pub const CAPABILITIES: VehicleCapabilities = VehicleCapabilities::quad_x(); + +/// Host integration adapter from the schema's one bounded CBOR profile blob to +/// the generic custom-MAVLink transaction service. +#[derive(Clone, Copy, Debug, Default)] +pub struct SitlSnapshotImporter; + +impl SnapshotImporter for SitlSnapshotImporter { + const IMPORT_SUPPORTED: bool = true; + const PROFILE_FORMAT: u16 = FORMAT_VERSION; + + fn decode(&self, bytes: &[u8]) -> Result { + decode_candidate(bytes, &CAPABILITIES).map_err(|error| match error { + CborError::TooLarge => ProtocolError::CborTooLarge, + CborError::Malformed | CborError::TrailingData => ProtocolError::CborMalformed, + CborError::InvalidProfile(violation) => ProtocolError::Violation(violation), + }) + } +} + +/// SITL composition root retaining the one profile manager and one protocol +/// lease state machine. The MAVLink adapter never owns either resource. +pub struct SitlConfigRuntime { + runtime: FlightRuntime, + protocol: ConfigProtocol, +} + +impl SitlConfigRuntime { + /// Dispatches one host-authorized transaction command. Real target ingress + /// remains disabled until signing/replay authority is implemented. + pub fn dispatch(&mut self, command: Authorized) -> ServiceResponse { + let (manager, _) = self.runtime.configuration_and_logs_mut(); + self.protocol.dispatch(manager, command) + } + + /// Enqueues the high-level error companion only after a composition owner + /// has retained the authoritative `KAPTAN_CONFIG_RESULT`. There is no + /// request correlation in `KAPTAN_ERROR`; its purpose is human-facing + /// observability, never replacement of the structured result. + pub fn after_config_result(&mut self, response: ServiceResponse) { + if let ServiceResponse::Result(reply) = response { + let (_, logs) = self.runtime.configuration_and_logs_mut(); + let _ = reply.publish_rejection(logs); + } + } + + /// Returns the central manager for SITL state inspection only. + pub const fn manager(&self) -> &ConfigManager { + self.runtime.configuration() + } + + /// Returns the immutable profile selected by the central manager at boot. + pub const fn active_profile(&self) -> VehicleProfile { + *self.runtime.configuration().active() + } + + /// Indicates custom CBOR import support to the local GCS bridge. + pub const fn capability_flags(&self) -> u8 { + self.protocol.capability_flags_for::() + } + + /// Drains one standard `log` facade record and exposes the generated + /// diagnostic payload for the host's sole MAVLink link owner. This + /// host-only harness accepts the bounded handoff synchronously. + pub fn pending_diagnostic_payload(&mut self) -> Option { + let (_, logs) = self.runtime.configuration_and_logs_mut(); + logging::drain_one_into(logs); + self.runtime + .submit_next(|record| (TransportAdmission::Accepted, encode_diagnostic(record))) + } +} + +/// Builds the host-only central configuration composition used by forthcoming +/// SITL/GCS integration. Flight targets must supply authenticated authority +/// and a reviewed journal/activation coordinator before constructing one. +pub fn configuration_service(store: FileProfileStore) -> Result { + logging::install_log_facade(); + let runtime = FlightRuntime::recover(CAPABILITIES, store) + .map_err(|error| format!("configuration recovery failed: {error:?}"))?; + logging::set_filter(runtime.log_filter()); + Ok(SitlConfigRuntime { + runtime, + protocol: ConfigProtocol::with_importer(Target::new(1, 1), SitlSnapshotImporter), + }) +} diff --git a/tools/sitl/src/config/store.rs b/tools/sitl/src/config/store.rs new file mode 100644 index 0000000..fe05a0d --- /dev/null +++ b/tools/sitl/src/config/store.rs @@ -0,0 +1,176 @@ +//! Host-only persistence for a validated SITL profile. + +use std::{ + fs::{self, File}, + io::{Read, Write}, + path::Path, +}; + +use kaptan_config::{Generation, ProfileStore}; +use kaptan_flight_config::{ + CandidateProfile, CborError, MAXIMUM_CANDIDATE_BYTES, VehicleProfile, decode_candidate, + encode_candidate, +}; +use kaptan_log::KaptanError; + +use super::{CAPABILITIES, configuration_service}; + +/// Host file-store error. It intentionally makes no flash power-loss claim. +#[derive(Debug, KaptanError)] +pub enum FileStoreError { + /// Host filesystem operation failed. + Io(std::io::Error), + /// The bounded file record did not decode and validate. + Cbor(CborError), + /// The fixed snapshot buffer could not encode the complete candidate. + Encode, + /// The host generation counter cannot advance. + GenerationExhausted, + /// Factory-only startup deliberately has no durable configuration path. + FactoryOnly, + /// The private host envelope was malformed or did not carry a valid + /// nonzero committed generation. + Envelope, +} + +/// Same-directory atomic-replacement store for SITL only. +pub struct FileProfileStore { + path: Option, + generation: Option, +} + +impl FileProfileStore { + /// Creates a host-only store at a fixed profile path. + pub fn new(path: impl Into) -> Self { + Self { + path: Some(path.into()), + generation: None, + } + } + + /// Creates a factory-only store for `kaptan-sitl serve` without + /// `--config`. Commits are rejected rather than silently discarded. + pub const fn factory_only() -> Self { + Self { + path: None, + generation: None, + } + } +} + +impl ProfileStore for FileProfileStore { + type Error = FileStoreError; + + fn load(&mut self) -> Result, Self::Error> { + let Some(path) = self.path.as_deref() else { + return Ok(None); + }; + let file = match File::open(path) { + Ok(file) => file, + Err(error) if error.kind() == std::io::ErrorKind::NotFound => return Ok(None), + Err(error) => return Err(FileStoreError::Io(error)), + }; + const MAXIMUM_FILE_BYTES: u64 = (MAXIMUM_CANDIDATE_BYTES + 64) as u64; + if file.metadata().map_err(FileStoreError::Io)?.len() > MAXIMUM_FILE_BYTES { + return Err(FileStoreError::Cbor(CborError::TooLarge)); + } + let mut bytes = Vec::new(); + file.take(MAXIMUM_FILE_BYTES + 1) + .read_to_end(&mut bytes) + .map_err(FileStoreError::Io)?; + if bytes.len() as u64 > MAXIMUM_FILE_BYTES { + return Err(FileStoreError::Cbor(CborError::TooLarge)); + } + let (generation, candidate) = decode_envelope(&bytes)?; + self.generation = Some(generation); + Ok(Some((generation, candidate))) + } + + fn persist(&mut self, candidate: &CandidateProfile) -> Result { + let path = self.path.clone().ok_or(FileStoreError::FactoryOnly)?; + if self.generation.is_none() { + let _ = self.load()?; + } + let generation = match self.generation { + Some(generation) => Generation::try_new( + generation + .get() + .checked_add(1) + .ok_or(FileStoreError::GenerationExhausted)?, + ) + .ok_or(FileStoreError::GenerationExhausted)?, + None => Generation::new(1), + }; + write_candidate(&path, *candidate, generation)?; + self.generation = Some(generation); + Ok(generation) + } +} + +/// Returns the fully validated factory-active profile. +pub fn default_profile() -> Result { + Ok(configuration_service(FileProfileStore::factory_only())?.active_profile()) +} + +/// Loads one bounded, complete CBOR candidate and returns its active view. +pub fn load_profile(path: impl AsRef) -> Result { + if !path.as_ref().is_file() { + return Err("configuration file does not exist".to_owned()); + } + Ok(configuration_service(FileProfileStore::new(path.as_ref()))?.active_profile()) +} + +/// Writes one validated factory candidate using a same-directory replacement. +pub fn write_default_profile(path: impl AsRef) -> Result<(), String> { + let mut store = FileProfileStore::new(path.as_ref()); + store + .persist(&CandidateProfile::default()) + .map(|_| ()) + .map_err(|error| error.to_string()) +} + +fn write_candidate( + path: &Path, + candidate: CandidateProfile, + generation: Generation, +) -> Result<(), FileStoreError> { + candidate + .validate_against(&CAPABILITIES) + .map_err(CborError::InvalidProfile) + .map_err(FileStoreError::Cbor)?; + let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]); + encode_candidate(candidate, &mut writer).map_err(|_| FileStoreError::Encode)?; + let length = writer.position(); + let bytes = writer.into_inner(); + let mut envelope = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES + 64]); + let mut encoder = minicbor::Encoder::new(&mut envelope); + encoder + .array(2) + .and_then(|encoder| encoder.u64(generation.get())) + .and_then(|encoder| encoder.bytes(&bytes[..length])) + .map_err(|_| FileStoreError::Encode)?; + let envelope_length = encoder.writer_mut().position(); + let envelope = envelope.into_inner(); + let temporary = path.with_extension("cbor.tmp"); + let mut file = File::create(&temporary).map_err(FileStoreError::Io)?; + file.write_all(&envelope[..envelope_length]) + .map_err(FileStoreError::Io)?; + file.sync_all().map_err(FileStoreError::Io)?; + fs::rename(&temporary, path).map_err(FileStoreError::Io) +} + +fn decode_envelope(bytes: &[u8]) -> Result<(Generation, CandidateProfile), FileStoreError> { + let mut decoder = minicbor::Decoder::new(bytes); + if decoder.array().map_err(|_| FileStoreError::Envelope)? != Some(2) { + return Err(FileStoreError::Envelope); + } + let generation = Generation::try_new(decoder.u64().map_err(|_| FileStoreError::Envelope)?) + .ok_or(FileStoreError::Envelope)?; + let candidate_bytes = decoder.bytes().map_err(|_| FileStoreError::Envelope)?; + if decoder.position() != bytes.len() { + return Err(FileStoreError::Envelope); + } + let candidate = + decode_candidate(candidate_bytes, &CAPABILITIES).map_err(FileStoreError::Cbor)?; + Ok((generation, candidate)) +} diff --git a/tools/sitl/src/config/tests.rs b/tools/sitl/src/config/tests.rs new file mode 100644 index 0000000..52fcbd9 --- /dev/null +++ b/tools/sitl/src/config/tests.rs @@ -0,0 +1,355 @@ +use std::fs; + +use kaptan_config::{Generation, ProfileStore}; +use kaptan_config_mavlink::{ + Authorized, ConfigCommand, ResultKind, ServiceResponse, SnapshotImporter, +}; +use kaptan_flight_config::{ + CandidateProfile, CborError, MAXIMUM_CANDIDATE_BYTES, SCHEMA_ID as PROFILE_SCHEMA_ID, + decode_candidate, encode_candidate, +}; +use kaptan_log_mavlink::DiagnosticPayload; +use kaptan_mavlink::{ + encoder::MavlinkEncoder, + message::{Message, kaptan::MavMessage}, +}; + +use super::{CAPABILITIES, FileProfileStore, SitlSnapshotImporter, configuration_service}; + +// Independently authored current-format CBOR map. It is intentionally not +// produced by `encode_candidate`: this is the GCS/upload compatibility vector +// for schema KAPTANPROFILE-01, with a non-default 1.7 kg mass. +const EXTERNAL_PROFILE_CBOR: &[u8] = &[ + 0xb8, 0x1a, // definite map, 26 entries + 0x00, 0x01, // format key/value + 0x19, 0xff, 0xff, 0x50, b'K', b'A', b'P', b'T', b'A', b'N', b'P', b'R', b'O', b'F', b'I', b'L', + b'E', b'-', b'0', b'1', // schema key/value + 0x01, 0x1a, 0x00, 0x01, 0x86, 0xa0, // prearm dwell: 100000 + 0x02, 0x19, 0xc3, 0x50, // IMU interval: 50000 + 0x03, 0x19, 0xc3, 0x50, // IMU freshness: 50000 + 0x04, 0x1a, 0x00, 0x02, 0x49, 0xf0, // local freshness: 150000 + 0x05, 0x1a, 0x00, 0x07, 0xa1, 0x20, // link freshness: 500000 + 0x06, 0xfa, 0x3f, 0x80, 0x00, 0x00, 0x07, 0xfa, 0x3f, 0x80, 0x00, 0x00, 0x08, 0xfa, 0x3f, 0x80, + 0x00, 0x00, 0x09, 0xfa, 0x3f, 0x80, 0x00, 0x00, 0x0a, 0xfa, 0x3f, 0x80, 0x00, 0x00, 0x0b, 0xfa, + 0x3f, 0x80, 0x00, 0x00, 0x0c, 0xfa, 0x3f, 0x80, 0x00, 0x00, 0x0d, 0xfa, 0x3f, 0xd9, 0x99, + 0x9a, // mass: 1.7 + 0x0e, 0xfa, 0x41, 0xa0, 0x00, 0x00, // collective thrust: 20 + 0x0f, 0xfa, 0x3f, 0x80, 0x00, 0x00, 0x10, 0xfa, 0x3f, 0x80, 0x00, 0x00, 0x11, 0xfa, 0x3f, 0x80, + 0x00, 0x00, 0x12, 0xfa, 0x00, 0x00, 0x00, 0x00, 0x13, 0xfa, 0x3f, 0x80, 0x00, 0x00, 0x14, 0xfa, + 0x3f, 0x80, 0x00, 0x00, 0x15, 0xfa, 0x00, 0x00, 0x00, 0x00, 0x16, 0xfa, 0x3f, 0x80, 0x00, 0x00, + 0x17, 0xfa, 0x3f, 0x80, 0x00, 0x00, 0x18, 0x18, 0x06, // maximum log level: INFO +]; + +fn result(response: ServiceResponse) -> kaptan_config_mavlink::ConfigReply { + match response { + ServiceResponse::Result(reply) => reply, + ServiceResponse::Info(_) | ServiceResponse::Descriptors(_) | ServiceResponse::Value(_) => { + panic!("expected a mutating configuration result") + } + } +} + +fn crc32c(bytes: &[u8]) -> u32 { + let mut crc = !0_u32; + for byte in bytes { + crc ^= u32::from(*byte); + for _ in 0..8 { + crc = if crc & 1 == 0 { + crc >> 1 + } else { + (crc >> 1) ^ 0x82f6_3b78 + }; + } + } + !crc +} + +#[test] +fn host_store_round_trips_a_complete_validated_snapshot() { + let path = std::env::temp_dir().join(format!( + "kaptan-config-store-{}-{}.cbor", + std::process::id(), + std::thread::current().name().unwrap_or("test") + )); + let temporary = path.with_extension("cbor.tmp"); + let _ = fs::remove_file(&path); + let _ = fs::remove_file(&temporary); + + let mut store = FileProfileStore::new(&path); + assert!(store.load().unwrap().is_none()); + assert_eq!( + store.persist(&CandidateProfile::default()).unwrap(), + Generation::new(1) + ); + let envelope = fs::read(&path).unwrap(); + assert!(matches!( + decode_candidate(&envelope, &CAPABILITIES), + Err(CborError::Malformed) + )); + let (_, candidate) = store.load().unwrap().expect("persisted candidate"); + assert!(candidate.validate_against(&CAPABILITIES).is_ok()); + + assert_eq!( + store.persist(&CandidateProfile::default()).unwrap(), + Generation::new(2) + ); + let mut reopened = FileProfileStore::new(&path); + assert_eq!( + reopened.load().unwrap().map(|(generation, _)| generation), + Some(Generation::new(2)) + ); + let mut fresh = FileProfileStore::new(&path); + assert_eq!( + fresh.persist(&CandidateProfile::default()).unwrap(), + Generation::new(3) + ); + + fs::remove_file(&path).unwrap(); +} + +#[test] +fn sitl_snapshot_importer_uses_the_real_schema_and_capabilities() { + let candidate = CandidateProfile::default(); + let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]); + encode_candidate(candidate, &mut writer).unwrap(); + let length = writer.position(); + let bytes = writer.into_inner(); + let imported = SitlSnapshotImporter + .decode(&bytes[..length]) + .expect("valid current candidate import"); + assert!(imported.validate_against(&CAPABILITIES).is_ok()); +} + +#[test] +fn sitl_service_imports_a_real_cbor_candidate_only_into_its_transaction() { + let path = std::env::temp_dir().join(format!( + "kaptan-config-import-{}-{}.cbor", + std::process::id(), + std::thread::current().name().unwrap_or("test") + )); + let _ = fs::remove_file(&path); + let mut service = configuration_service(FileProfileStore::new(&path)).unwrap(); + let begin = result(service.dispatch(Authorized::host( + 9, + ConfigCommand::Begin { + request_id: 1, + expected_generation: 0, + target: kaptan_config_mavlink::Target::new(1, 1), + }, + ))); + let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]); + encode_candidate(CandidateProfile::default(), &mut writer).unwrap(); + let length = writer.position(); + assert!(length <= 192); + let bytes = writer.into_inner(); + let import_begin = result(service.dispatch(Authorized::host( + 9, + ConfigCommand::ImportBegin { + request_id: 2, + session_id: begin.session_id, + expected_revision: 0, + length: length as u16, + schema_id: PROFILE_SCHEMA_ID, + crc32c: crc32c(&bytes[..length]), + target: kaptan_config_mavlink::Target::new(1, 1), + }, + ))); + assert_eq!(import_begin.result, ResultKind::Accepted); + let mut chunk = [0; 192]; + chunk[..length].copy_from_slice(&bytes[..length]); + assert_eq!( + result(service.dispatch(Authorized::host( + 9, + ConfigCommand::ImportChunk { + request_id: 3, + session_id: begin.session_id, + transfer_id: import_begin.transfer_id, + offset: 0, + length: length as u8, + data: chunk, + target: kaptan_config_mavlink::Target::new(1, 1), + }, + ))) + .result, + ResultKind::Accepted + ); + assert_eq!( + result(service.dispatch(Authorized::host( + 9, + ConfigCommand::ImportFinish { + request_id: 4, + session_id: begin.session_id, + transfer_id: import_begin.transfer_id, + expected_revision: 0, + target: kaptan_config_mavlink::Target::new(1, 1), + }, + ))) + .result, + ResultKind::Accepted + ); + assert!( + service + .manager() + .transaction_value(kaptan_config::FieldId::new(1)) + .is_some() + ); + assert!( + service + .manager() + .active_value(kaptan_config::FieldId::new(1)) + .is_some() + ); + let _ = fs::remove_file(&path); +} + +#[test] +fn external_gcs_vector_imports_commits_and_reopens_as_the_boot_profile() { + let path = std::env::temp_dir().join(format!( + "kaptan-config-gcs-vector-{}-{}.cbor", + std::process::id(), + std::thread::current().name().unwrap_or("test") + )); + let _ = fs::remove_file(&path); + let mut service = configuration_service(FileProfileStore::new(&path)).unwrap(); + let target = kaptan_config_mavlink::Target::new(1, 1); + let begin = result(service.dispatch(Authorized::host( + 9, + ConfigCommand::Begin { + request_id: 1, + expected_generation: 0, + target, + }, + ))); + let import = result(service.dispatch(Authorized::host( + 9, + ConfigCommand::ImportBegin { + request_id: 2, + session_id: begin.session_id, + expected_revision: 0, + length: EXTERNAL_PROFILE_CBOR.len() as u16, + schema_id: PROFILE_SCHEMA_ID, + crc32c: crc32c(EXTERNAL_PROFILE_CBOR), + target, + }, + ))); + let mut data = [0_u8; 192]; + data[..EXTERNAL_PROFILE_CBOR.len()].copy_from_slice(EXTERNAL_PROFILE_CBOR); + assert_eq!( + result(service.dispatch(Authorized::host( + 9, + ConfigCommand::ImportChunk { + request_id: 3, + session_id: begin.session_id, + transfer_id: import.transfer_id, + offset: 0, + length: EXTERNAL_PROFILE_CBOR.len() as u8, + data, + target, + }, + ))) + .result, + ResultKind::Accepted + ); + assert_eq!( + result(service.dispatch(Authorized::host( + 9, + ConfigCommand::ImportFinish { + request_id: 4, + session_id: begin.session_id, + transfer_id: import.transfer_id, + expected_revision: 0, + target, + }, + ))) + .revision, + 1 + ); + assert_eq!( + result(service.dispatch(Authorized::host( + 9, + ConfigCommand::Commit { + request_id: 5, + session_id: begin.session_id, + expected_generation: 0, + expected_revision: 1, + target, + }, + ))) + .result, + ResultKind::CommittedRestartRequired + ); + drop(service); + + let reopened = configuration_service(FileProfileStore::new(&path)).unwrap(); + assert_eq!(reopened.active_profile().thrust().mass_kg(), 1.7); + let _ = fs::remove_file(&path); +} + +#[test] +fn standard_log_macro_reaches_the_generated_mavlink_log_payload() { + let path = std::env::temp_dir().join(format!( + "kaptan-sitl-log-{}-{}.cbor", + std::process::id(), + std::thread::current().name().unwrap_or("test") + )); + let _ = fs::remove_file(&path); + let mut service = configuration_service(FileProfileStore::new(&path)).unwrap(); + + log::info!("SITL configuration service ready"); + let Some(DiagnosticPayload::Log(payload)) = service.pending_diagnostic_payload() else { + panic!("info facade record must reach KAPTAN_LOG"); + }; + assert_eq!(payload.level, kaptan_log::Severity::Info as u8); + let message = std::str::from_utf8(&payload.message[..usize::from(payload.message_length)]) + .expect("facade text remains UTF-8"); + assert!(message.contains("config::tests")); + assert!(message.ends_with("SITL configuration service ready")); + let mut encoder = MavlinkEncoder::new(1, 1); + let frame = encoder.custom(&payload); + assert_eq!(frame.message_id(), 51_020); + assert!(matches!( + MavMessage::parse( + mavlink::MavlinkVersion::V2, + frame.message_id(), + frame.payload(), + ), + Ok(MavMessage::KAPTAN_LOG(data)) + if data.level == payload.level + && data.message_length == payload.message_length + && data.message == payload.message + )); + let _ = fs::remove_file(&path); +} + +#[test] +fn rejected_config_result_enqueues_a_separate_high_level_error_payload() { + let path = std::env::temp_dir().join(format!( + "kaptan-sitl-config-error-{}-{}.cbor", + std::process::id(), + std::thread::current().name().unwrap_or("test") + )); + let _ = fs::remove_file(&path); + let mut service = configuration_service(FileProfileStore::new(&path)).unwrap(); + let response = service.dispatch(Authorized::host( + 9, + ConfigCommand::Begin { + request_id: 1, + expected_generation: 1, + target: kaptan_config_mavlink::Target::new(1, 1), + }, + )); + let reply = result(response); + assert_eq!(reply.result, ResultKind::Rejected); + service.after_config_result(response); + + let Some(DiagnosticPayload::Error(payload)) = service.pending_diagnostic_payload() else { + panic!("rejected configuration result must enqueue KAPTAN_ERROR"); + }; + let message = std::str::from_utf8(&payload.message[..usize::from(payload.message_length)]) + .expect("derived error identity remains UTF-8"); + assert_eq!( + message, + "kaptan_config_mavlink::protocol::ProtocolError::GenerationConflict" + ); + let _ = fs::remove_file(&path); +} diff --git a/tools/sitl/src/main.rs b/tools/sitl/src/main.rs index 0611b5b..468b068 100644 --- a/tools/sitl/src/main.rs +++ b/tools/sitl/src/main.rs @@ -5,6 +5,8 @@ //! state reaches the controller; simulator pose/velocity never has a parallel //! controller path. +/// Host-only persisted configuration adapter and custom-service integration. +pub mod config; mod protocol; use std::{ @@ -33,6 +35,13 @@ use kaptan_navigation::{ use kaptan_safety::{ActuatorPermit, SafetyConfig, SafetyInputs, SafetySupervisor}; use kaptan_time::Timestamp; +#[cfg(test)] +use crate::config::default_profile; +use crate::config::{ + FileProfileStore, SitlConfigRuntime, configuration_service, load_profile, write_default_profile, +}; +use kaptan_flight_config::VehicleProfile; + use protocol::{ ACTUATOR_PORT, CLOCK_MAGIC, CONTROL_PORT, GoalKeepalivePacket, GoalLifecycle, GoalPacket, GoalStatusPacket, IMU_MAGIC, LOCAL_STATE_MAGIC, SENSOR_PORT, STATUS_ACTUATOR_TRANSPORT, @@ -42,26 +51,22 @@ use protocol::{ const SOURCE_IMU: SourceId = SourceId::new(1); const SOURCE_LOCAL_STATE: SourceId = SourceId::new(2); const CALIBRATION: CalibrationRevision = CalibrationRevision::new(1); -const IMU_FRESHNESS_MICROSECONDS: u64 = 50_000; -const LOCAL_STATE_FRESHNESS_MICROSECONDS: u64 = 150_000; -const COMMAND_LINK_FRESHNESS_MICROSECONDS: u64 = 500_000; -/// Host-simulation policy only. Flight targets must supply a reviewed vehicle -/// profile rather than reuse this value. -const SITL_PREARM_DWELL_MICROSECONDS: u64 = 100_000; /// Bound either ingress lane so a fast simulator clock cannot indefinitely /// delay command lease processing, and a command flood cannot delay sensor /// ordering and safety supervision. const MAXIMUM_INGRESS_DATAGRAMS_PER_TURN: usize = 32; -const MAXIMUM_IMU_INTERVAL_MICROSECONDS: u64 = 50_000; -// TODO(configuration): bind these values, vehicle mass/inertia, mixer, and -// flight limits to the reviewed vehicle profile once the ground configuration -// protocol exists. They are intentionally confined to this host-only model. -const POSITION_VARIANCE_M2: f32 = 0.04; -const VELOCITY_VARIANCE_M2PS2: f32 = 0.04; fn main() -> ExitCode { - match env::args().nth(1).as_deref() { - Some("serve") => match serve() { + let mut arguments = env::args().skip(1); + match arguments.next().as_deref() { + Some("serve") => match serve_configuration(&mut arguments).and_then(serve) { + Ok(()) => ExitCode::SUCCESS, + Err(error) => { + eprintln!("kaptan-sitl: {error}"); + ExitCode::FAILURE + } + }, + Some("config") => match config_command(&mut arguments) { Ok(()) => ExitCode::SUCCESS, Err(error) => { eprintln!("kaptan-sitl: {error}"); @@ -69,13 +74,44 @@ fn main() -> ExitCode { } }, _ => { - eprintln!("usage: kaptan-sitl serve"); + eprintln!( + "usage: kaptan-sitl serve [--config PROFILE.cbor]\n kaptan-sitl config write-default PROFILE.cbor\n kaptan-sitl config validate PROFILE.cbor" + ); ExitCode::FAILURE } } } -fn serve() -> Result<(), String> { +fn serve_configuration( + arguments: &mut impl Iterator, +) -> Result { + match (arguments.next().as_deref(), arguments.next()) { + (None, None) => configuration_service(FileProfileStore::factory_only()), + (Some("--config"), Some(path)) => configuration_service(FileProfileStore::new(path)), + _ => Err("usage: kaptan-sitl serve [--config PROFILE.cbor]".to_owned()), + } +} + +fn config_command(arguments: &mut impl Iterator) -> Result<(), String> { + match ( + arguments.next().as_deref(), + arguments.next(), + arguments.next(), + ) { + (Some("write-default"), Some(path), None) => write_default_profile(path), + (Some("validate"), Some(path), None) => { + let _profile = load_profile(path)?; + eprintln!("kaptan-sitl: versioned CBOR configuration candidate is valid"); + Ok(()) + } + _ => Err( + "usage: kaptan-sitl config write-default PROFILE.cbor | validate PROFILE.cbor" + .to_owned(), + ), + } +} + +fn serve(configuration: SitlConfigRuntime) -> Result<(), String> { let sensor = bind_loopback(SENSOR_PORT)?; let control = bind_loopback(CONTROL_PORT)?; let actuator = UdpSocket::bind(("127.0.0.1", 0)).map_err(|error| error.to_string())?; @@ -85,9 +121,11 @@ fn serve() -> Result<(), String> { control .set_nonblocking(true) .map_err(|error| error.to_string())?; - let mut runtime = SitlRuntime::new()?; + let mut runtime = SitlRuntime::from_configuration(configuration)?; + let config_generation = runtime.configuration.manager().active_generation(); eprintln!( - "kaptan-sitl: listening for Gazebo sensors on 127.0.0.1:{SENSOR_PORT} and commands on 127.0.0.1:{CONTROL_PORT}" + "kaptan-sitl: configuration generation {} selected at boot; listening for Gazebo sensors on 127.0.0.1:{SENSOR_PORT} and commands on 127.0.0.1:{CONTROL_PORT}", + config_generation.map_or(0, kaptan_config::Generation::get), ); loop { @@ -152,6 +190,10 @@ impl DatagramSink for UdpSocket { } struct SitlRuntime { + /// Retains the sole central manager that selected this boot-active profile. + /// No live apply path exists: a commit is restart-required. + configuration: SitlConfigRuntime, + profile: VehicleProfile, initializer: StationaryInitializer<100>, estimator: Option, controller: QuadController, @@ -180,14 +222,22 @@ enum ClockWork { } impl SitlRuntime { + #[cfg(test)] fn new() -> Result { + Self::from_configuration(configuration_service(FileProfileStore::factory_only())?) + } + + fn from_configuration(configuration: SitlConfigRuntime) -> Result { + let profile = configuration.active_profile(); Ok(Self { + configuration, + profile, initializer: StationaryInitializer::new(), estimator: None, - controller: QuadController::new()?, + controller: QuadController::new(profile)?, trajectory: None, safety: SafetySupervisor::new( - SafetyConfig::new(SITL_PREARM_DWELL_MICROSECONDS) + SafetyConfig::new(profile.safety().prearm_dwell_microseconds()) .map_err(|error| format!("invalid SITL safety policy: {error:?}"))?, ), active_goal: None, @@ -399,7 +449,11 @@ impl SitlRuntime { let step_seconds = if reset_epoch { None } else { - control_step_seconds(self.last_imu_time, packet.time_microseconds) + control_step_seconds( + self.last_imu_time, + packet.time_microseconds, + self.profile.timing().maximum_imu_interval_microseconds(), + ) }; if self.estimator.is_some() && self.last_imu_time.is_some() && step_seconds.is_none() { self.reset_for_simulator_epoch(); @@ -421,7 +475,7 @@ impl SitlRuntime { } else if self.initializer.push(observation) && self.initializer.is_complete() { self.estimator = core::mem::take(&mut self.initializer).finish( observation.context.measurement_time, - estimator_config(), + estimator_config(self.profile.timing().maximum_imu_interval_microseconds()), IntegrityConfig::default(), 0.0, ); @@ -446,10 +500,16 @@ impl SitlRuntime { let Some(estimator) = self.estimator.as_mut() else { return; }; - let Some(position_variance) = ScalarVariance::new(POSITION_VARIANCE_M2) else { + let Some(position_variance) = + ScalarVariance::new(self.profile.aiding().position_variance_m2()) + else { return; }; - let Some(velocity_variance) = ScalarVariance::new(VELOCITY_VARIANCE_M2PS2) else { + let Some(velocity_variance) = ScalarVariance::new( + self.profile + .aiding() + .velocity_variance_m2_per_second_squared(), + ) else { return; }; let observation = LocalPositionVelocityObservation { @@ -624,15 +684,19 @@ impl SitlRuntime { }) && fresh( self.last_local_state_time, now, - LOCAL_STATE_FRESHNESS_MICROSECONDS, + self.profile.timing().local_state_freshness_microseconds(), ); SafetyInputs { estimator_healthy, - imu_fresh: fresh(self.last_imu_time, now, IMU_FRESHNESS_MICROSECONDS), + imu_fresh: fresh( + self.last_imu_time, + now, + self.profile.timing().imu_freshness_microseconds(), + ), link_fresh: fresh( self.last_command_time, now, - COMMAND_LINK_FRESHNESS_MICROSECONDS, + self.profile.timing().command_link_freshness_microseconds(), ), actuator_healthy: true, } @@ -643,7 +707,7 @@ impl SitlRuntime { && !fresh( self.last_command_time, now, - COMMAND_LINK_FRESHNESS_MICROSECONDS, + self.profile.timing().command_link_freshness_microseconds(), ) { self.abort_goal(); @@ -703,13 +767,17 @@ impl SitlRuntime { fn status_flags(&self) -> u16 { let now = self.current_time(); let mut flags = STATUS_ACTUATOR_TRANSPORT; - if fresh(self.last_imu_time, now, IMU_FRESHNESS_MICROSECONDS) { + if fresh( + self.last_imu_time, + now, + self.profile.timing().imu_freshness_microseconds(), + ) { flags |= STATUS_IMU_FRESH; } if fresh( self.last_local_state_time, now, - LOCAL_STATE_FRESHNESS_MICROSECONDS, + self.profile.timing().local_state_freshness_microseconds(), ) { flags |= STATUS_LOCAL_AIDING_FRESH; } @@ -790,55 +858,54 @@ struct QuadController { } impl QuadController { - fn new() -> Result { - // TODO(configuration): replace this one test vehicle's validated - // constants with an immutable, reviewed configuration selected before - // arming. No board target may inherit this host-SITL profile. + fn new(config: VehicleProfile) -> Result { + let control = config.control(); + let thrust_config = config.thrust(); + let rate_pid = config.rate_pid(); + let actuator_limits = config.actuator_limits(); let translational = TranslationalController::new(TranslationalControlConfig { - 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, + 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(|error| format!("invalid translational controller: {error:?}"))?; let thrust = ThrustEnvelope { - mass_kg: 1.2, - maximum_collective_thrust_newtons: 18.0, - // tan(35 degrees), computed offline so the embedded rate path - // requires no transcendental implementation. - maximum_tilt_tangent: 0.700_207_53, + mass_kg: thrust_config.mass_kg(), + maximum_collective_thrust_newtons: thrust_config.maximum_collective_thrust_newtons(), + maximum_tilt_tangent: thrust_config.maximum_tilt_tangent(), }; thrust .validate() .map_err(|error| format!("invalid thrust envelope: {error:?}"))?; let rate_config = PidConfig { - proportional: 0.035, - integral: 0.006, - derivative: 0.0, - output_limit: 0.10, - integral_limit: 0.025, + proportional: rate_pid.proportional(), + integral: rate_pid.integral(), + derivative: rate_pid.derivative(), + output_limit: rate_pid.output_limit(), + integral_limit: rate_pid.integral_limit(), }; let limits = ActuatorLimits { - minimum: 0.0, - maximum: 1.0, - slew_per_second: 8.0, + minimum: actuator_limits.minimum(), + maximum: actuator_limits.maximum(), + slew_per_second: actuator_limits.slew_per_second(), }; let mixer = PrecomputedMixer::new([ MixerOutput { - coefficients: [-1.0, 1.0, 1.0, 1.0], + coefficients: config.mixer()[0], limits, }, MixerOutput { - coefficients: [-1.0, -1.0, -1.0, 1.0], + coefficients: config.mixer()[1], limits, }, MixerOutput { - coefficients: [1.0, -1.0, 1.0, 1.0], + coefficients: config.mixer()[2], limits, }, MixerOutput { - coefficients: [1.0, 1.0, -1.0, 1.0], + coefficients: config.mixer()[3], limits, }, ]); @@ -954,16 +1021,16 @@ fn imu_observation(packet: protocol::ImuPacket) -> Result EstimatorConfig { +fn estimator_config(maximum_imu_interval_microseconds: u64) -> EstimatorConfig { EstimatorConfig { - maximum_imu_interval_microseconds: MAXIMUM_IMU_INTERVAL_MICROSECONDS, + maximum_imu_interval_microseconds, ..EstimatorConfig::default() } } -fn control_step_seconds(previous: Option, current: u64) -> Option { +fn control_step_seconds(previous: Option, current: u64, maximum_interval: u64) -> Option { let elapsed = current.checked_sub(previous?)?; - if elapsed == 0 || elapsed > MAXIMUM_IMU_INTERVAL_MICROSECONDS { + if elapsed == 0 || elapsed > maximum_interval { return None; } Some(elapsed as f32 / 1_000_000.0) @@ -1004,6 +1071,7 @@ fn strictly_after( mod tests { use super::*; use core::cell::RefCell; + use kaptan_config::{ConfigModel, ConfigValue, FieldId, ProfileStore}; #[derive(Default)] struct RecordingSink { @@ -1053,7 +1121,12 @@ mod tests { initializer .finish( Timestamp::from_microseconds(40_000), - estimator_config(), + estimator_config( + default_profile() + .unwrap() + .timing() + .maximum_imu_interval_microseconds(), + ), IntegrityConfig::default(), 0.0, ) @@ -1067,9 +1140,33 @@ mod tests { assert!(!strictly_after(1, 8, Some(2), Some(7))); } + #[test] + fn serve_configuration_retains_the_persisted_profile_that_builds_consumers() { + let path = std::env::temp_dir().join(format!( + "kaptan-sitl-serve-config-{}-{}.cbor", + std::process::id(), + std::thread::current().name().unwrap_or("test") + )); + let _ = std::fs::remove_file(&path); + let mut candidate = kaptan_flight_config::CandidateProfile::default(); + candidate + .set_field(FieldId::new(13), ConfigValue::F32(1.7)) + .unwrap(); + FileProfileStore::new(&path).persist(&candidate).unwrap(); + + let mut arguments = + vec!["--config".to_owned(), path.to_string_lossy().into_owned()].into_iter(); + let configuration = serve_configuration(&mut arguments).unwrap(); + assert_eq!(configuration.active_profile().thrust().mass_kg(), 1.7); + let runtime = SitlRuntime::from_configuration(configuration).unwrap(); + assert_eq!(runtime.profile.thrust().mass_kg(), 1.7); + assert_eq!(runtime.controller.thrust.mass_kg, 1.7); + let _ = std::fs::remove_file(&path); + } + #[test] fn hover_output() { - let mut controller = QuadController::new().unwrap(); + let mut controller = QuadController::new(default_profile().unwrap()).unwrap(); let filter = initialized_estimator(); let outputs = controller .update( @@ -1092,7 +1189,10 @@ mod tests { #[test] fn settled_landing_reports_landed_then_emits_inhibited_zero_output() { - let now = SITL_PREARM_DWELL_MICROSECONDS; + let now = default_profile() + .unwrap() + .safety() + .prearm_dwell_microseconds(); let mut runtime = SitlRuntime::new().unwrap(); runtime.estimator = Some(initialized_estimator()); runtime.accept_local_state(protocol::LocalStatePacket { @@ -1160,12 +1260,22 @@ mod tests { #[test] fn control_step_tracks_imu_time() { - assert!(control_step_seconds(None, 10_000).is_none()); - assert!(control_step_seconds(Some(10_000), 10_000).is_none()); - assert!(control_step_seconds(Some(10_000), 9_999).is_none()); - assert!(control_step_seconds(Some(10_000), 60_001).is_none()); - assert!((control_step_seconds(Some(10_000), 20_000).unwrap() - 0.01).abs() < f32::EPSILON); - assert!((control_step_seconds(Some(10_000), 60_000).unwrap() - 0.05).abs() < f32::EPSILON); + let maximum_interval = default_profile() + .unwrap() + .timing() + .maximum_imu_interval_microseconds(); + assert!(control_step_seconds(None, 10_000, maximum_interval).is_none()); + assert!(control_step_seconds(Some(10_000), 10_000, maximum_interval).is_none()); + assert!(control_step_seconds(Some(10_000), 9_999, maximum_interval).is_none()); + assert!(control_step_seconds(Some(10_000), 60_001, maximum_interval).is_none()); + assert!( + (control_step_seconds(Some(10_000), 20_000, maximum_interval).unwrap() - 0.01).abs() + < f32::EPSILON + ); + assert!( + (control_step_seconds(Some(10_000), 60_000, maximum_interval).unwrap() - 0.05).abs() + < f32::EPSILON + ); } #[test] @@ -1199,14 +1309,31 @@ mod tests { actuator_healthy: true, }; runtime.safety.evaluate( - Timestamp::from_microseconds(SITL_PREARM_DWELL_MICROSECONDS), + Timestamp::from_microseconds( + default_profile() + .unwrap() + .safety() + .prearm_dwell_microseconds(), + ), healthy, ); - runtime.expire_stale_goal(1_000_000 + COMMAND_LINK_FRESHNESS_MICROSECONDS); + runtime.expire_stale_goal( + 1_000_000 + + default_profile() + .unwrap() + .timing() + .command_link_freshness_microseconds(), + ); assert!(runtime.active_goal.is_some()); - runtime.expire_stale_goal(1_000_001 + COMMAND_LINK_FRESHNESS_MICROSECONDS); + runtime.expire_stale_goal( + 1_000_001 + + default_profile() + .unwrap() + .timing() + .command_link_freshness_microseconds(), + ); assert!(runtime.active_goal.is_none()); assert!(runtime.goal_client.is_none()); @@ -1220,10 +1347,14 @@ mod tests { #[test] fn freshness_uses_only_simulation_timestamps() { - assert!(fresh(Some(10_000), 60_000, IMU_FRESHNESS_MICROSECONDS)); - assert!(!fresh(Some(10_000), 60_001, IMU_FRESHNESS_MICROSECONDS)); - assert!(!fresh(Some(60_001), 60_000, IMU_FRESHNESS_MICROSECONDS)); - assert!(!fresh(None, 60_000, IMU_FRESHNESS_MICROSECONDS)); + let freshness = default_profile() + .unwrap() + .timing() + .imu_freshness_microseconds(); + assert!(fresh(Some(10_000), 60_000, freshness)); + assert!(!fresh(Some(10_000), 60_001, freshness)); + assert!(!fresh(Some(60_001), 60_000, freshness)); + assert!(!fresh(None, 60_000, freshness)); } #[test] @@ -1288,7 +1419,13 @@ mod tests { runtime .safety .evaluate( - Timestamp::from_microseconds(1_000 + SITL_PREARM_DWELL_MICROSECONDS), + Timestamp::from_microseconds( + 1_000 + + default_profile() + .unwrap() + .safety() + .prearm_dwell_microseconds(), + ), healthy, ) .state(), diff --git a/tools/sitl/src/protocol.rs b/tools/sitl/src/protocol.rs index d829ef4..d52deaa 100644 --- a/tools/sitl/src/protocol.rs +++ b/tools/sitl/src/protocol.rs @@ -4,6 +4,7 @@ //! dialect. It is confined to one host network namespace and is versioned so //! malformed simulator traffic never becomes estimator or motor authority. +use kaptan_log::KaptanError; use std::net::SocketAddr; pub const SENSOR_PORT: u16 = 14_660; @@ -39,7 +40,7 @@ pub const STATUS_IMU_FRESH: u16 = 1 << 1; pub const STATUS_LOCAL_AIDING_FRESH: u16 = 1 << 2; pub const STATUS_OUTPUT_PERMITTED: u16 = 1 << 3; pub const STATUS_ACTUATOR_TRANSPORT: u16 = 1 << 4; -#[derive(Clone, Copy, Debug, Eq, PartialEq)] +#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)] pub enum PacketError { Length, Version,