refactor: move firmware workspace under firmware

firmware/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<SITL_LOG_INGRESS_CAPACITY> =
+    FacadeIngress::new(LogFilter::new(Severity::Info));
+static SITL_LOGGER: GlobalLogger<SITL_LOG_INGRESS_CAPACITY> =
+    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);
+}

firmware/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,
+};

firmware/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<CandidateProfile> for SitlSnapshotImporter {
+    const IMPORT_SUPPORTED: bool = true;
+    const PROFILE_FORMAT: u16 = FORMAT_VERSION;
+
+    fn decode(&self, bytes: &[u8]) -> Result<CandidateProfile, ProtocolError> {
+        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<FileProfileStore>,
+    protocol: ConfigProtocol<SitlSnapshotImporter>,
+}
+
+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<ConfigCommand>) -> 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<CandidateProfile, FileProfileStore> {
+        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::<CandidateProfile>()
+    }
+
+    /// 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<DiagnosticPayload> {
+        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<SitlConfigRuntime, String> {
+    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),
+    })
+}

firmware/tools/sitl/src/config/store.rs

@@ -0,0 +1,177 @@
+//! 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, CborEncodeError, 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<std::path::PathBuf>,
+    generation: Option<Generation>,
+}
+
+impl FileProfileStore {
+    /// Creates a host-only store at a fixed profile path.
+    pub fn new(path: impl Into<std::path::PathBuf>) -> 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<CandidateProfile> for FileProfileStore {
+    type Error = FileStoreError;
+
+    fn load(&mut self) -> Result<Option<(Generation, CandidateProfile)>, 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<Generation, Self::Error> {
+        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<VehicleProfile, String> {
+    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<Path>) -> Result<VehicleProfile, String> {
+    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<Path>) -> 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> {
+    let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]);
+    encode_candidate(candidate, &CAPABILITIES, &mut writer).map_err(|error| match error {
+        CborEncodeError::InvalidProfile(violation) => {
+            FileStoreError::Cbor(CborError::InvalidProfile(violation))
+        }
+        CborEncodeError::Encode(_) => 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))
+}

firmware/tools/sitl/src/config/tests.rs

@@ -0,0 +1,356 @@
+use std::fs;
+
+use kaptan_config::{ConfigModel, 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(&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, &CAPABILITIES, &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(&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(), &CAPABILITIES, &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 error_path = std::str::from_utf8(&payload.path[..usize::from(payload.path_length)])
+        .expect("derived error path remains UTF-8");
+    assert_eq!(
+        error_path,
+        "kaptan_config_mavlink::protocol::ProtocolError::GenerationConflict"
+    );
+    assert_eq!(payload.additional_information_length, 0);
+    let _ = fs::remove_file(&path);
+}

firmware/tools/sitl/src/kaptan_sitl_quad.cc

@@ -0,0 +1,650 @@
+// Deterministic, deliberately narrow Gazebo-side adapter for the Kaptan
+// quad-X host-SITL transport contract. It is simulator tooling, not flight code.
+
+#include <gz/common/Console.hh>
+#include <gz/math/Pose3.hh>
+#include <gz/math/Vector3.hh>
+#include <gz/msgs/imu.pb.h>
+#include <gz/plugin/Register.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/Link.hh>
+#include <gz/sim/Model.hh>
+#include <gz/sim/System.hh>
+#include <gz/transport/Node.hh>
+
+#include <algorithm>
+#include <array>
+#include <bit>
+#include <chrono>
+#include <cmath>
+#include <cstddef>
+#include <cstdint>
+#include <cstring>
+#include <memory>
+#include <mutex>
+#include <limits>
+#include <optional>
+#include <random>
+#include <string>
+
+#include <arpa/inet.h>
+#include <fcntl.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
+namespace kaptan::sitl
+{
+namespace
+{
+constexpr std::uint8_t kPacketVersion = 1;
+constexpr std::array<std::uint8_t, 4> kImuMagic{'K', 'S', 'I', '1'};
+constexpr std::array<std::uint8_t, 4> kLocalStateMagic{'K', 'S', 'V', '1'};
+constexpr std::array<std::uint8_t, 4> kClockMagic{'K', 'S', 'C', '1'};
+constexpr std::array<std::uint8_t, 4> kTruthMagic{'K', 'S', 'T', '2'};
+constexpr std::array<std::uint8_t, 4> kOutputMagic{'K', 'S', 'O', '1'};
+constexpr std::size_t kImuPacketSize = 44;
+constexpr std::size_t kClockPacketSize = 16;
+constexpr std::size_t kTruthPacketSize = 76;
+constexpr std::size_t kOutputPacketSize = 40;
+constexpr std::size_t kMaximumPacketsPerStep = 4;
+constexpr std::size_t kQuadMotorCount = 4;
+
+struct QuadMotorGeometry
+{
+  gz::math::Vector3d offset_flu;
+  double reaction_torque_sign;
+};
+
+// This test model is FLU / ENU. It is not an electrical Kakute timer map:
+// M1 front-right CW, M2 rear-right CCW, M3 rear-left CW, M4 front-left CCW.
+constexpr std::array<QuadMotorGeometry, kQuadMotorCount> kQuadXGeometry{{
+    {{0.17, -0.17, 0.0}, -1.0},
+    {{-0.17, -0.17, 0.0}, 1.0},
+    {{-0.17, 0.17, 0.0}, -1.0},
+    {{0.17, 0.17, 0.0}, 1.0},
+}};
+
+bool IsFiniteFraction(const float value)
+{
+  return std::isfinite(value) && value >= 0.0F && value <= 1.0F;
+}
+
+bool IsZero(const float value)
+{
+  return value == 0.0F;
+}
+
+void WriteU32(std::uint8_t *destination, const std::uint32_t value)
+{
+  for (std::size_t index = 0; index < sizeof(value); ++index)
+  {
+    destination[index] = static_cast<std::uint8_t>(value >> (index * 8));
+  }
+}
+
+void WriteU64(std::uint8_t *destination, const std::uint64_t value)
+{
+  for (std::size_t index = 0; index < sizeof(value); ++index)
+  {
+    destination[index] = static_cast<std::uint8_t>(value >> (index * 8));
+  }
+}
+
+std::uint32_t ReadU32(const std::uint8_t *source)
+{
+  std::uint32_t value = 0;
+  for (std::size_t index = 0; index < sizeof(value); ++index)
+  {
+    value |= static_cast<std::uint32_t>(source[index]) << (index * 8);
+  }
+  return value;
+}
+
+std::uint64_t ReadU64(const std::uint8_t *source)
+{
+  std::uint64_t value = 0;
+  for (std::size_t index = 0; index < sizeof(value); ++index)
+  {
+    value |= static_cast<std::uint64_t>(source[index]) << (index * 8);
+  }
+  return value;
+}
+
+void WriteF32(std::uint8_t *destination, const float value)
+{
+  WriteU32(destination, std::bit_cast<std::uint32_t>(value));
+}
+
+float ReadF32(const std::uint8_t *source)
+{
+  return std::bit_cast<float>(ReadU32(source));
+}
+
+bool SequenceAfter(const std::uint32_t next, const std::uint32_t previous)
+{
+  const std::uint32_t distance = next - previous;
+  return distance != 0 && distance <= UINT32_MAX / 2;
+}
+
+class UdpSocket
+{
+ public:
+  UdpSocket() = default;
+  ~UdpSocket()
+  {
+    if (this->descriptor_ >= 0)
+    {
+      close(this->descriptor_);
+    }
+  }
+
+  UdpSocket(const UdpSocket &) = delete;
+  UdpSocket &operator=(const UdpSocket &) = delete;
+
+  bool BindLoopback(const std::uint16_t port)
+  {
+    this->descriptor_ = socket(AF_INET, SOCK_DGRAM, 0);
+    if (this->descriptor_ < 0)
+    {
+      return false;
+    }
+    const int flags = fcntl(this->descriptor_, F_GETFL, 0);
+    if (flags < 0 || fcntl(this->descriptor_, F_SETFL, flags | O_NONBLOCK) < 0)
+    {
+      return false;
+    }
+    sockaddr_in address{};
+    address.sin_family = AF_INET;
+    address.sin_port = htons(port);
+    address.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
+    return bind(
+               this->descriptor_,
+               reinterpret_cast<const sockaddr *>(&address),
+               sizeof(address)) == 0;
+  }
+
+  bool SendToLoopback(
+      const std::uint16_t port, const std::uint8_t *data, const std::size_t size) const
+  {
+    sockaddr_in address{};
+    address.sin_family = AF_INET;
+    address.sin_port = htons(port);
+    address.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
+    return sendto(
+               this->descriptor_, data, size, MSG_DONTWAIT,
+               reinterpret_cast<const sockaddr *>(&address), sizeof(address)) ==
+           static_cast<ssize_t>(size);
+  }
+
+  ssize_t Receive(std::uint8_t *data, const std::size_t size) const
+  {
+    sockaddr_in sender{};
+    socklen_t sender_size = sizeof(sender);
+    const ssize_t received = recvfrom(
+        this->descriptor_, data, size, MSG_DONTWAIT,
+        reinterpret_cast<sockaddr *>(&sender), &sender_size);
+    if (received < 0 || sender.sin_addr.s_addr != htonl(INADDR_LOOPBACK))
+    {
+      return -1;
+    }
+    return received;
+  }
+
+ private:
+  int descriptor_{-1};
+};
+
+struct OutputCommand
+{
+  std::uint64_t controller_time_usec{};
+  std::uint32_t control_sequence{};
+  bool permitted{};
+  std::array<float, kQuadMotorCount> motors{};
+};
+
+bool DecodeOutput(const std::array<std::uint8_t, kOutputPacketSize> &packet,
+                  OutputCommand &output)
+{
+  if (!std::equal(kOutputMagic.begin(), kOutputMagic.end(), packet.begin()) ||
+      packet[4] != kPacketVersion || packet[5] != 0 || packet[6] != 0 || packet[7] != 0)
+  {
+    return false;
+  }
+  output.controller_time_usec = ReadU64(packet.data() + 8);
+  output.control_sequence = ReadU32(packet.data() + 16);
+  if (packet[20] > 1 || packet[21] != 0 || packet[22] != 0 || packet[23] != 0)
+  {
+    return false;
+  }
+  output.permitted = packet[20] == 1;
+  for (std::size_t index = 0; index < output.motors.size(); ++index)
+  {
+    output.motors[index] = ReadF32(packet.data() + 24 + index * sizeof(float));
+    if (!IsFiniteFraction(output.motors[index]))
+    {
+      return false;
+    }
+  }
+  return output.permitted || std::all_of(output.motors.begin(), output.motors.end(), IsZero);
+}
+}  // namespace
+
+class Quad final : public gz::sim::System,
+                   public gz::sim::ISystemConfigure,
+                   public gz::sim::ISystemPreUpdate,
+                   public gz::sim::ISystemPostUpdate,
+                   public gz::sim::ISystemReset
+{
+ public:
+  Quad() : data_(std::make_unique<Data>()) {}
+
+  void Configure(const gz::sim::Entity &entity,
+                 const std::shared_ptr<const sdf::Element> &sdf,
+                 gz::sim::EntityComponentManager &ecm,
+                 gz::sim::EventManager &) override;
+
+  void PreUpdate(const gz::sim::UpdateInfo &info,
+                 gz::sim::EntityComponentManager &ecm) override;
+
+  void PostUpdate(const gz::sim::UpdateInfo &info,
+                  const gz::sim::EntityComponentManager &) override;
+
+  void Reset(const gz::sim::UpdateInfo &,
+             gz::sim::EntityComponentManager &) override;
+
+ private:
+  struct Data
+  {
+    void OnImu(const gz::msgs::IMU &message)
+    {
+      std::lock_guard lock(this->imu_mutex);
+      this->latest_imu = message;
+      ++this->imu_generation;
+      if (!this->received_first_imu)
+      {
+        gzmsg << "Kaptan SITL quad received its first Gazebo IMU sample.\n";
+        this->received_first_imu = true;
+      }
+    }
+
+    gz::sim::Link body;
+    gz::transport::Node node;
+    UdpSocket socket;
+    std::uint16_t sensor_port{};
+    std::uint16_t actuator_port{};
+    std::uint16_t truth_port{};
+    std::uint16_t operator_clock_port{};
+    std::chrono::microseconds actuator_timeout{};
+    double maximum_motor_thrust_newtons{};
+    double motor_time_constant_seconds{};
+    std::normal_distribution<double> local_position_noise{0.0, 0.0};
+    std::normal_distribution<double> local_velocity_noise{0.0, 0.0};
+    std::mt19937 local_state_random{};
+    double reaction_torque_per_newton_metre{};
+    std::optional<gz::math::Vector3d> launch_origin_enu;
+    std::optional<gz::math::Quaterniond> launch_attitude_enu;
+    std::mutex imu_mutex;
+    gz::msgs::IMU latest_imu;
+    std::uint64_t imu_generation{};
+    std::uint64_t published_imu_generation{};
+    bool received_first_imu{};
+    std::optional<OutputCommand> output;
+    std::array<double, kQuadMotorCount> applied_motor_fractions{};
+    std::optional<std::pair<std::uint64_t, std::uint32_t>> previous_output_order;
+    // Actuator freshness belongs to the simulated vehicle timeline, never
+    // host scheduling time. That keeps timeout behavior deterministic when
+    // Gazebo is paused, stepped, or runs at a non-real-time factor.
+    std::optional<std::chrono::microseconds> last_output_sim_time;
+  };
+
+  std::unique_ptr<Data> data_;
+};
+
+void Quad::Configure(const gz::sim::Entity &entity,
+                     const std::shared_ptr<const sdf::Element> &sdf,
+                     gz::sim::EntityComponentManager &ecm,
+                     gz::sim::EventManager &)
+{
+  const gz::sim::Model model(entity);
+  if (!model.Valid(ecm))
+  {
+    gzerr << "Kaptan SITL quad must be attached to a model.\n";
+    return;
+  }
+  const auto body_entity = model.LinkByName(ecm, "base_link");
+  if (body_entity == gz::sim::kNullEntity)
+  {
+    gzerr << "Kaptan SITL quad requires a base_link.\n";
+    return;
+  }
+  this->data_->body = gz::sim::Link(body_entity);
+  this->data_->body.EnableVelocityChecks(ecm);
+
+  const std::string imu_topic = sdf->Get<std::string>("imu_topic", "").first;
+  const unsigned int sensor_port = sdf->Get<unsigned int>("sensor_udp_port", 0U).first;
+  const unsigned int actuator_port =
+      sdf->Get<unsigned int>("actuator_udp_port", 0U).first;
+  const unsigned int truth_port = sdf->Get<unsigned int>("truth_udp_port", 0U).first;
+  const unsigned int operator_clock_port =
+      sdf->Get<unsigned int>("operator_clock_udp_port", 0U).first;
+  const std::uint64_t timeout_usec =
+      sdf->Get<std::uint64_t>("actuator_timeout_usec", 0).first;
+  this->data_->maximum_motor_thrust_newtons =
+      sdf->Get<double>("maximum_motor_thrust_newtons", 0.0).first;
+  this->data_->motor_time_constant_seconds =
+      sdf->Get<double>("motor_time_constant_seconds", 0.0).first;
+  const double local_position_noise_stddev =
+      sdf->Get<double>("local_position_noise_stddev_m", -1.0).first;
+  const double local_velocity_noise_stddev =
+      sdf->Get<double>("local_velocity_noise_stddev_mps", -1.0).first;
+  const unsigned int local_state_noise_seed =
+      sdf->Get<unsigned int>("local_state_noise_seed", 0U).first;
+  this->data_->reaction_torque_per_newton_metre =
+      sdf->Get<double>("reaction_torque_per_newton_metre", 0.0).first;
+  if (imu_topic.empty() || sensor_port == 0 || actuator_port == 0 || truth_port == 0 ||
+      operator_clock_port == 0 ||
+      sensor_port > std::numeric_limits<std::uint16_t>::max() ||
+      actuator_port > std::numeric_limits<std::uint16_t>::max() ||
+      truth_port > std::numeric_limits<std::uint16_t>::max() ||
+      operator_clock_port > std::numeric_limits<std::uint16_t>::max() ||
+      timeout_usec == 0 || !std::isfinite(this->data_->maximum_motor_thrust_newtons) ||
+      this->data_->maximum_motor_thrust_newtons <= 0.0 ||
+      !std::isfinite(this->data_->motor_time_constant_seconds) ||
+      this->data_->motor_time_constant_seconds <= 0.0 ||
+      !std::isfinite(local_position_noise_stddev) || local_position_noise_stddev < 0.0 ||
+      !std::isfinite(local_velocity_noise_stddev) || local_velocity_noise_stddev < 0.0 ||
+      !std::isfinite(this->data_->reaction_torque_per_newton_metre) ||
+      this->data_->reaction_torque_per_newton_metre < 0.0)
+  {
+    gzerr << "Kaptan SITL quad has an invalid static transport or vehicle configuration.\n";
+    return;
+  }
+  this->data_->sensor_port = static_cast<std::uint16_t>(sensor_port);
+  this->data_->actuator_port = static_cast<std::uint16_t>(actuator_port);
+  this->data_->truth_port = static_cast<std::uint16_t>(truth_port);
+  this->data_->operator_clock_port = static_cast<std::uint16_t>(operator_clock_port);
+  this->data_->actuator_timeout = std::chrono::microseconds(timeout_usec);
+  this->data_->local_position_noise =
+      std::normal_distribution<double>(0.0, local_position_noise_stddev);
+  this->data_->local_velocity_noise =
+      std::normal_distribution<double>(0.0, local_velocity_noise_stddev);
+  this->data_->local_state_random.seed(local_state_noise_seed);
+  if (!this->data_->socket.BindLoopback(this->data_->actuator_port))
+  {
+    gzerr << "Kaptan SITL quad could not bind its loopback actuator socket.\n";
+    return;
+  }
+  if (!this->data_->node.Subscribe(imu_topic, &Data::OnImu, this->data_.get()))
+  {
+    gzerr << "Kaptan SITL quad could not subscribe to its configured IMU topic.\n";
+  }
+  else
+  {
+    gzmsg << "Kaptan SITL quad subscribed to Gazebo IMU topic [" << imu_topic << "].\n";
+  }
+}
+
+void Quad::PreUpdate(const gz::sim::UpdateInfo &info,
+                     gz::sim::EntityComponentManager &ecm)
+{
+  if (info.paused || info.simTime.count() < 0 || !this->data_->body.Valid(ecm))
+  {
+    return;
+  }
+  const auto simulation_time =
+      std::chrono::duration_cast<std::chrono::microseconds>(info.simTime);
+  for (std::size_t attempt = 0; attempt < kMaximumPacketsPerStep; ++attempt)
+  {
+    std::array<std::uint8_t, kOutputPacketSize> packet{};
+    if (this->data_->socket.Receive(packet.data(), packet.size()) !=
+        static_cast<ssize_t>(packet.size()))
+    {
+      break;
+    }
+    OutputCommand command{};
+    if (!DecodeOutput(packet, command))
+    {
+      continue;
+    }
+    if (!command.permitted)
+    {
+      this->data_->output = command;
+      continue;
+    }
+    if (this->data_->previous_output_order.has_value() &&
+        (command.controller_time_usec <= this->data_->previous_output_order->first ||
+         !SequenceAfter(command.control_sequence,
+                        this->data_->previous_output_order->second)))
+    {
+      continue;
+    }
+    this->data_->previous_output_order =
+        std::pair{command.controller_time_usec, command.control_sequence};
+    this->data_->last_output_sim_time = simulation_time;
+    this->data_->output = command;
+  }
+
+  const bool command_is_fresh =
+      this->data_->output.has_value() && this->data_->output->permitted &&
+      this->data_->last_output_sim_time.has_value() &&
+      simulation_time >= *this->data_->last_output_sim_time &&
+      simulation_time - *this->data_->last_output_sim_time <= this->data_->actuator_timeout;
+  const auto pose = this->data_->body.WorldPose(ecm);
+  if (!pose.has_value())
+  {
+    return;
+  }
+  const double dt_seconds = std::chrono::duration<double>(info.dt).count();
+  if (!std::isfinite(dt_seconds) || dt_seconds <= 0.0)
+  {
+    return;
+  }
+  const double alpha = 1.0 - std::exp(-dt_seconds / this->data_->motor_time_constant_seconds);
+  const auto desired_motors = command_is_fresh
+      ? this->data_->output->motors
+      : std::array<float, kQuadMotorCount>{};
+  const gz::math::Vector3d thrust_axis_world = pose->Rot().RotateVector({0.0, 0.0, 1.0});
+  for (std::size_t index = 0; index < kQuadMotorCount; ++index)
+  {
+    this->data_->applied_motor_fractions[index] +=
+        alpha * (static_cast<double>(desired_motors[index]) -
+                 this->data_->applied_motor_fractions[index]);
+    const double thrust =
+        this->data_->applied_motor_fractions[index] *
+        this->data_->maximum_motor_thrust_newtons;
+    this->data_->body.AddWorldWrench(
+        ecm, thrust_axis_world * thrust,
+        thrust_axis_world *
+            (kQuadXGeometry[index].reaction_torque_sign * thrust *
+             this->data_->reaction_torque_per_newton_metre),
+        kQuadXGeometry[index].offset_flu);
+  }
+}
+
+void Quad::PostUpdate(const gz::sim::UpdateInfo &info,
+                      const gz::sim::EntityComponentManager &_ecm)
+{
+  if (info.paused || info.simTime.count() < 0)
+  {
+    return;
+  }
+  // This is deliberately emitted for every unpaused Gazebo step, even when
+  // an IMU sample is absent. The host runtime uses it as its sole freshness
+  // and lease time base; host scheduling cannot advance safety state.
+  const auto send_clock = [this, &info]() {
+    std::array<std::uint8_t, kClockPacketSize> clock_packet{};
+    std::copy(kClockMagic.begin(), kClockMagic.end(), clock_packet.begin());
+    clock_packet[4] = kPacketVersion;
+    WriteU64(clock_packet.data() + 8, static_cast<std::uint64_t>(
+                                          std::chrono::duration_cast<std::chrono::microseconds>(
+                                              info.simTime)
+                                              .count()));
+    (void)this->data_->socket.SendToLoopback(
+        this->data_->sensor_port, clock_packet.data(), clock_packet.size());
+    (void)this->data_->socket.SendToLoopback(
+        this->data_->operator_clock_port, clock_packet.data(), clock_packet.size());
+  };
+  gz::msgs::IMU imu;
+  {
+    std::lock_guard lock(this->data_->imu_mutex);
+    if (this->data_->imu_generation == this->data_->published_imu_generation)
+    {
+      send_clock();
+      return;
+    }
+    imu = this->data_->latest_imu;
+    this->data_->published_imu_generation = this->data_->imu_generation;
+  }
+
+  // Gazebo's standard body sensor convention is FLU. Kaptan's is FRD, so the
+  // single explicit boundary conversion is x, -y, -z for both vectors.
+  std::array<std::uint8_t, kImuPacketSize> packet{};
+  std::copy(kImuMagic.begin(), kImuMagic.end(), packet.begin());
+  packet[4] = kPacketVersion;
+  WriteU64(packet.data() + 8, static_cast<std::uint64_t>(
+                                  std::chrono::duration_cast<std::chrono::microseconds>(
+                                      info.simTime)
+                                      .count()));
+  WriteU32(packet.data() + 16, static_cast<std::uint32_t>(this->data_->imu_generation));
+  const std::array<float, 6> frd{
+      static_cast<float>(imu.linear_acceleration().x()),
+      -static_cast<float>(imu.linear_acceleration().y()),
+      -static_cast<float>(imu.linear_acceleration().z()),
+      static_cast<float>(imu.angular_velocity().x()),
+      -static_cast<float>(imu.angular_velocity().y()),
+      -static_cast<float>(imu.angular_velocity().z()),
+  };
+  if (!std::all_of(frd.begin(), frd.end(), [](const float value) { return std::isfinite(value); }))
+  {
+    send_clock();
+    return;
+  }
+  for (std::size_t index = 0; index < frd.size(); ++index)
+  {
+    WriteF32(packet.data() + 20 + index * sizeof(float), frd[index]);
+  }
+  (void)this->data_->socket.SendToLoopback(
+      this->data_->sensor_port, packet.data(), packet.size());
+
+  const auto pose = this->data_->body.WorldPose(_ecm);
+  const auto velocity = this->data_->body.WorldLinearVelocity(_ecm);
+  if (!pose.has_value() || !velocity.has_value())
+  {
+    send_clock();
+    return;
+  }
+  if (!this->data_->launch_origin_enu.has_value())
+  {
+    this->data_->launch_origin_enu = pose->Pos();
+    this->data_->launch_attitude_enu = pose->Rot();
+  }
+  // This fixed test vehicle is yawed so ENU world +Y/+X/-Z are respectively
+  // its launch-local NED north/east/down axes. The conversion happens once at
+  // the Gazebo boundary; all flight code receives only explicit NED SI data.
+  const auto &origin = *this->data_->launch_origin_enu;
+  const std::array<float, 6> local_ned{
+      static_cast<float>(pose->Pos().Y() - origin.Y() +
+                         this->data_->local_position_noise(this->data_->local_state_random)),
+      static_cast<float>(pose->Pos().X() - origin.X() +
+                         this->data_->local_position_noise(this->data_->local_state_random)),
+      static_cast<float>(origin.Z() - pose->Pos().Z() +
+                         this->data_->local_position_noise(this->data_->local_state_random)),
+      static_cast<float>(velocity->Y() +
+                         this->data_->local_velocity_noise(this->data_->local_state_random)),
+      static_cast<float>(velocity->X() +
+                         this->data_->local_velocity_noise(this->data_->local_state_random)),
+      static_cast<float>(-velocity->Z() +
+                         this->data_->local_velocity_noise(this->data_->local_state_random)),
+  };
+  if (!std::all_of(
+          local_ned.begin(), local_ned.end(),
+          [](const float value) { return std::isfinite(value); }))
+  {
+    send_clock();
+    return;
+  }
+  std::array<std::uint8_t, kImuPacketSize> state_packet{};
+  std::copy(kLocalStateMagic.begin(), kLocalStateMagic.end(), state_packet.begin());
+  state_packet[4] = kPacketVersion;
+  WriteU64(state_packet.data() + 8, static_cast<std::uint64_t>(
+                                        std::chrono::duration_cast<std::chrono::microseconds>(
+                                            info.simTime)
+                                            .count()));
+  WriteU32(state_packet.data() + 16, static_cast<std::uint32_t>(this->data_->imu_generation));
+  for (std::size_t index = 0; index < local_ned.size(); ++index)
+  {
+    WriteF32(state_packet.data() + 20 + index * sizeof(float), local_ned[index]);
+  }
+  (void)this->data_->socket.SendToLoopback(
+      this->data_->sensor_port, state_packet.data(), state_packet.size());
+
+  // This packet is intentionally sent to a separate test-only port. The
+  // controller never binds that port, so these noise-free values cannot form a
+  // parallel control path or mask estimator behavior. The quaternion is the
+  // body attitude relative to launch; its rotation magnitude is frame
+  // invariant, which permits the test client to verify attitude without
+  // duplicating the ENU/FLU-to-NED/FRD conversion path.
+  const auto launch_attitude = *this->data_->launch_attitude_enu;
+  const auto relative_attitude = launch_attitude.Inverse() * pose->Rot();
+  const std::array<float, 14> truth_ned{
+      static_cast<float>(pose->Pos().Y() - origin.Y()),
+      static_cast<float>(pose->Pos().X() - origin.X()),
+      static_cast<float>(origin.Z() - pose->Pos().Z()),
+      static_cast<float>(velocity->Y()),
+      static_cast<float>(velocity->X()),
+      static_cast<float>(-velocity->Z()),
+      static_cast<float>(relative_attitude.W()),
+      static_cast<float>(relative_attitude.X()),
+      static_cast<float>(relative_attitude.Y()),
+      static_cast<float>(relative_attitude.Z()),
+      static_cast<float>(this->data_->applied_motor_fractions[0]),
+      static_cast<float>(this->data_->applied_motor_fractions[1]),
+      static_cast<float>(this->data_->applied_motor_fractions[2]),
+      static_cast<float>(this->data_->applied_motor_fractions[3]),
+  };
+  if (!std::all_of(
+          truth_ned.begin(), truth_ned.end(),
+          [](const float value) { return std::isfinite(value); }))
+  {
+    send_clock();
+    return;
+  }
+  std::array<std::uint8_t, kTruthPacketSize> truth_packet{};
+  std::copy(kTruthMagic.begin(), kTruthMagic.end(), truth_packet.begin());
+  truth_packet[4] = kPacketVersion;
+  WriteU64(truth_packet.data() + 8, static_cast<std::uint64_t>(
+                                        std::chrono::duration_cast<std::chrono::microseconds>(
+                                            info.simTime)
+                                            .count()));
+  WriteU32(truth_packet.data() + 16, static_cast<std::uint32_t>(this->data_->imu_generation));
+  for (std::size_t index = 0; index < truth_ned.size(); ++index)
+  {
+    WriteF32(truth_packet.data() + 20 + index * sizeof(float), truth_ned[index]);
+  }
+  (void)this->data_->socket.SendToLoopback(
+      this->data_->truth_port, truth_packet.data(), truth_packet.size());
+  send_clock();
+}
+
+void Quad::Reset(const gz::sim::UpdateInfo &,
+                 gz::sim::EntityComponentManager &)
+{
+  this->data_->output.reset();
+  this->data_->applied_motor_fractions = {};
+  this->data_->previous_output_order.reset();
+  this->data_->last_output_sim_time.reset();
+  this->data_->launch_origin_enu.reset();
+  this->data_->launch_attitude_enu.reset();
+  std::lock_guard lock(this->data_->imu_mutex);
+  this->data_->published_imu_generation = this->data_->imu_generation;
+}
+
+}  // namespace kaptan::sitl
+
+GZ_ADD_PLUGIN(kaptan::sitl::Quad,
+              gz::sim::System,
+              kaptan::sitl::Quad::ISystemConfigure,
+              kaptan::sitl::Quad::ISystemPreUpdate,
+              kaptan::sitl::Quad::ISystemPostUpdate,
+              kaptan::sitl::Quad::ISystemReset)
+GZ_ADD_PLUGIN_ALIAS(kaptan::sitl::Quad, "kaptan::sitl::Quad")

firmware/tools/sitl/src/protocol.rs

@@ -0,0 +1,360 @@
+//! Fixed-size local UDP records for Kaptan's host-only simulator harness.
+//!
+//! This transport is deliberately not MAVLink and not part of Kaptan's flight
+//! 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;
+pub const ACTUATOR_PORT: u16 = 14_661;
+pub const CONTROL_PORT: u16 = 14_662;
+pub const VERSION: u8 = 1;
+pub const IMU_MAGIC: [u8; 4] = *b"KSI1";
+pub const LOCAL_STATE_MAGIC: [u8; 4] = *b"KSV1";
+/// Gazebo simulation time, sent once for every unpaused simulation step.
+/// This is the sole time base for host-SITL freshness and command leases.
+pub const CLOCK_MAGIC: [u8; 4] = *b"KSC1";
+pub const OUTPUT_MAGIC: [u8; 4] = *b"KSO1";
+pub const GOAL_MAGIC: [u8; 4] = *b"KSG1";
+/// A host-SITL-only request to descend to a declared local landing point and
+/// revoke motor authority once the landing point has settled.
+pub const LAND_MAGIC: [u8; 4] = *b"KSD1";
+pub const GOAL_KEEPALIVE_MAGIC: [u8; 4] = *b"KSK1";
+pub const STATUS_QUERY_MAGIC: [u8; 4] = *b"KSQ1";
+pub const STATUS_MAGIC: [u8; 4] = *b"KSS1";
+pub const GOAL_STATUS_MAGIC: [u8; 4] = *b"KSL1";
+
+pub const SENSOR_PACKET_SIZE: usize = 44;
+pub const CLOCK_PACKET_SIZE: usize = 16;
+pub const OUTPUT_PACKET_SIZE: usize = 40;
+pub const GOAL_PACKET_SIZE: usize = 24;
+pub const GOAL_KEEPALIVE_PACKET_SIZE: usize = 12;
+pub const STATUS_QUERY_PACKET_SIZE: usize = 8;
+pub const STATUS_PACKET_SIZE: usize = 20;
+pub const GOAL_STATUS_PACKET_SIZE: usize = 16;
+
+pub const STATUS_ESTIMATOR_READY: u16 = 1 << 0;
+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, KaptanError)]
+pub enum PacketError {
+    Length,
+    Version,
+    NonFinite,
+}
+
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct ImuPacket {
+    pub time_microseconds: u64,
+    pub sequence: u32,
+    pub acceleration_frd_mps2: [f32; 3],
+    pub angular_rate_frd_rad_s: [f32; 3],
+}
+
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct LocalStatePacket {
+    pub time_microseconds: u64,
+    pub sequence: u32,
+    pub position_ned_metres: [f32; 3],
+    pub velocity_ned_mps: [f32; 3],
+}
+
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct ClockPacket {
+    pub time_microseconds: u64,
+}
+
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct GoalPacket {
+    pub goal_id: u32,
+    pub north_metres: f32,
+    pub east_metres: f32,
+    pub up_metres: f32,
+}
+
+/// Renews the command lease for one accepted goal without modifying it.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct GoalKeepalivePacket {
+    pub goal_id: u32,
+}
+
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+#[repr(u8)]
+pub enum GoalLifecycle {
+    Rejected = 0,
+    Active = 1,
+    Settled = 2,
+    Landed = 3,
+}
+
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct GoalStatusPacket {
+    pub goal_id: u32,
+    pub lifecycle: GoalLifecycle,
+}
+
+pub fn decode_imu(bytes: &[u8]) -> Result<ImuPacket, PacketError> {
+    let (time_microseconds, sequence, values) = decode_sensor(bytes, IMU_MAGIC)?;
+    Ok(ImuPacket {
+        time_microseconds,
+        sequence,
+        acceleration_frd_mps2: [values[0], values[1], values[2]],
+        angular_rate_frd_rad_s: [values[3], values[4], values[5]],
+    })
+}
+
+pub fn decode_local_state(bytes: &[u8]) -> Result<LocalStatePacket, PacketError> {
+    let (time_microseconds, sequence, values) = decode_sensor(bytes, LOCAL_STATE_MAGIC)?;
+    Ok(LocalStatePacket {
+        time_microseconds,
+        sequence,
+        position_ned_metres: [values[0], values[1], values[2]],
+        velocity_ned_mps: [values[3], values[4], values[5]],
+    })
+}
+
+pub fn decode_clock(bytes: &[u8]) -> Result<ClockPacket, PacketError> {
+    if bytes.len() != CLOCK_PACKET_SIZE {
+        return Err(PacketError::Length);
+    }
+    if bytes[0..4] != CLOCK_MAGIC || bytes[4] != VERSION || bytes[5..8] != [0; 3] {
+        return Err(PacketError::Version);
+    }
+    Ok(ClockPacket {
+        time_microseconds: read_u64(&bytes[8..16]).ok_or(PacketError::Length)?,
+    })
+}
+
+pub fn decode_goal(bytes: &[u8]) -> Result<GoalPacket, PacketError> {
+    decode_goal_with_magic(bytes, GOAL_MAGIC, false)
+}
+
+/// Decodes a host-SITL landing request. Landing always targets the declared
+/// local ground plane (`up == 0`), rather than silently treating an arbitrary
+/// move goal as a landing operation.
+pub fn decode_landing(bytes: &[u8]) -> Result<GoalPacket, PacketError> {
+    decode_goal_with_magic(bytes, LAND_MAGIC, true)
+}
+
+fn decode_goal_with_magic(
+    bytes: &[u8],
+    magic: [u8; 4],
+    require_ground_plane: bool,
+) -> Result<GoalPacket, PacketError> {
+    if bytes.len() != GOAL_PACKET_SIZE {
+        return Err(PacketError::Length);
+    }
+    if bytes[0..4] != magic || bytes[4] != VERSION || bytes[5..8] != [0; 3] {
+        return Err(PacketError::Version);
+    }
+    let goal = GoalPacket {
+        goal_id: read_u32(&bytes[8..12]).ok_or(PacketError::Length)?,
+        north_metres: read_f32(&bytes[12..16]).ok_or(PacketError::Length)?,
+        east_metres: read_f32(&bytes[16..20]).ok_or(PacketError::Length)?,
+        up_metres: read_f32(&bytes[20..24]).ok_or(PacketError::Length)?,
+    };
+    if goal.goal_id == 0
+        || !goal.north_metres.is_finite()
+        || !goal.east_metres.is_finite()
+        || !goal.up_metres.is_finite()
+        || (require_ground_plane && goal.up_metres != 0.0)
+    {
+        return Err(PacketError::NonFinite);
+    }
+    Ok(goal)
+}
+
+pub fn decode_goal_keepalive(bytes: &[u8]) -> Result<GoalKeepalivePacket, PacketError> {
+    if bytes.len() != GOAL_KEEPALIVE_PACKET_SIZE {
+        return Err(PacketError::Length);
+    }
+    if bytes[0..4] != GOAL_KEEPALIVE_MAGIC || bytes[4] != VERSION || bytes[5..8] != [0; 3] {
+        return Err(PacketError::Version);
+    }
+    let goal_id = read_u32(&bytes[8..12]).ok_or(PacketError::Length)?;
+    if goal_id == 0 {
+        return Err(PacketError::NonFinite);
+    }
+    Ok(GoalKeepalivePacket { goal_id })
+}
+
+pub fn is_status_query(bytes: &[u8]) -> bool {
+    bytes.len() == STATUS_QUERY_PACKET_SIZE
+        && bytes[0..4] == STATUS_QUERY_MAGIC
+        && bytes[4] == VERSION
+        && bytes[5..8] == [0; 3]
+}
+
+pub fn encode_output(
+    time_microseconds: u64,
+    sequence: u32,
+    permitted: bool,
+    motors: [f32; 4],
+) -> Result<[u8; OUTPUT_PACKET_SIZE], PacketError> {
+    if motors
+        .iter()
+        .any(|motor| !motor.is_finite() || !(0.0..=1.0).contains(motor))
+        || (!permitted && motors.iter().any(|motor| *motor != 0.0))
+    {
+        return Err(PacketError::NonFinite);
+    }
+    let mut bytes = [0_u8; OUTPUT_PACKET_SIZE];
+    bytes[0..4].copy_from_slice(&OUTPUT_MAGIC);
+    bytes[4] = VERSION;
+    bytes[8..16].copy_from_slice(&time_microseconds.to_le_bytes());
+    bytes[16..20].copy_from_slice(&sequence.to_le_bytes());
+    bytes[20] = u8::from(permitted);
+    for (index, motor) in motors.iter().enumerate() {
+        bytes[24 + index * 4..28 + index * 4].copy_from_slice(&motor.to_le_bytes());
+    }
+    Ok(bytes)
+}
+
+pub fn encode_status(time_microseconds: u64, flags: u16) -> [u8; STATUS_PACKET_SIZE] {
+    let mut bytes = [0_u8; STATUS_PACKET_SIZE];
+    bytes[0..4].copy_from_slice(&STATUS_MAGIC);
+    bytes[4] = VERSION;
+    bytes[8..16].copy_from_slice(&time_microseconds.to_le_bytes());
+    bytes[16..18].copy_from_slice(&flags.to_le_bytes());
+    bytes
+}
+
+pub fn encode_goal_status(status: GoalStatusPacket) -> [u8; GOAL_STATUS_PACKET_SIZE] {
+    let mut bytes = [0_u8; GOAL_STATUS_PACKET_SIZE];
+    bytes[0..4].copy_from_slice(&GOAL_STATUS_MAGIC);
+    bytes[4] = VERSION;
+    bytes[8..12].copy_from_slice(&status.goal_id.to_le_bytes());
+    bytes[12] = status.lifecycle as u8;
+    bytes
+}
+
+pub fn localhost(sender: SocketAddr) -> bool {
+    sender.ip().is_loopback()
+}
+
+fn decode_sensor(bytes: &[u8], magic: [u8; 4]) -> Result<(u64, u32, [f32; 6]), PacketError> {
+    if bytes.len() != SENSOR_PACKET_SIZE {
+        return Err(PacketError::Length);
+    }
+    if bytes[0..4] != magic || bytes[4] != VERSION || bytes[5..8] != [0; 3] {
+        return Err(PacketError::Version);
+    }
+    let mut values = [0.0; 6];
+    for (index, value) in values.iter_mut().enumerate() {
+        *value = read_f32(&bytes[20 + index * 4..24 + index * 4]).ok_or(PacketError::Length)?;
+    }
+    if values.iter().any(|value| !value.is_finite()) {
+        return Err(PacketError::NonFinite);
+    }
+    Ok((
+        read_u64(&bytes[8..16]).ok_or(PacketError::Length)?,
+        read_u32(&bytes[16..20]).ok_or(PacketError::Length)?,
+        values,
+    ))
+}
+
+fn read_u32(bytes: &[u8]) -> Option<u32> {
+    Some(u32::from_le_bytes(bytes.try_into().ok()?))
+}
+
+fn read_u64(bytes: &[u8]) -> Option<u64> {
+    Some(u64::from_le_bytes(bytes.try_into().ok()?))
+}
+
+fn read_f32(bytes: &[u8]) -> Option<f32> {
+    Some(f32::from_le_bytes(bytes.try_into().ok()?))
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn sensor(magic: [u8; 4]) -> [u8; SENSOR_PACKET_SIZE] {
+        let mut bytes = [0_u8; SENSOR_PACKET_SIZE];
+        bytes[..4].copy_from_slice(&magic);
+        bytes[4] = VERSION;
+        bytes[8..16].copy_from_slice(&123_u64.to_le_bytes());
+        bytes[16..20].copy_from_slice(&9_u32.to_le_bytes());
+        for index in 0..6 {
+            bytes[20 + index * 4..24 + index * 4]
+                .copy_from_slice(&(index as f32 + 0.5).to_le_bytes());
+        }
+        bytes
+    }
+
+    #[test]
+    fn sensor_records_are_finite() {
+        let packet = decode_imu(&sensor(IMU_MAGIC)).unwrap();
+        assert_eq!(packet.time_microseconds, 123);
+        assert_eq!(packet.acceleration_frd_mps2, [0.5, 1.5, 2.5]);
+        assert_eq!(packet.angular_rate_frd_rad_s, [3.5, 4.5, 5.5]);
+        assert_eq!(
+            decode_imu(&sensor(LOCAL_STATE_MAGIC)),
+            Err(PacketError::Version)
+        );
+    }
+
+    #[test]
+    fn simulator_clock_has_no_host_receipt_time() {
+        let mut packet = [0_u8; CLOCK_PACKET_SIZE];
+        packet[..4].copy_from_slice(&CLOCK_MAGIC);
+        packet[4] = VERSION;
+        packet[8..16].copy_from_slice(&987_654_u64.to_le_bytes());
+        assert_eq!(
+            decode_clock(&packet),
+            Ok(ClockPacket {
+                time_microseconds: 987_654,
+            })
+        );
+        packet[4] = VERSION + 1;
+        assert_eq!(decode_clock(&packet), Err(PacketError::Version));
+    }
+
+    #[test]
+    fn inhibited_output() {
+        assert!(encode_output(1, 2, true, [0.1, 0.2, 0.3, 0.4]).is_ok());
+        assert_eq!(
+            encode_output(1, 2, false, [0.1, 0.0, 0.0, 0.0]),
+            Err(PacketError::NonFinite)
+        );
+    }
+
+    #[test]
+    fn goal_keepalive_validation() {
+        let mut packet = [0_u8; GOAL_KEEPALIVE_PACKET_SIZE];
+        packet[..4].copy_from_slice(&GOAL_KEEPALIVE_MAGIC);
+        packet[4] = VERSION;
+        packet[8..12].copy_from_slice(&7_u32.to_le_bytes());
+        assert_eq!(
+            decode_goal_keepalive(&packet),
+            Ok(GoalKeepalivePacket { goal_id: 7 })
+        );
+        packet[8..12].copy_from_slice(&0_u32.to_le_bytes());
+        assert_eq!(decode_goal_keepalive(&packet), Err(PacketError::NonFinite));
+    }
+
+    #[test]
+    fn landing_is_an_explicit_ground_plane_command() {
+        let mut packet = [0_u8; GOAL_PACKET_SIZE];
+        packet[..4].copy_from_slice(&LAND_MAGIC);
+        packet[4] = VERSION;
+        packet[8..12].copy_from_slice(&7_u32.to_le_bytes());
+        packet[12..16].copy_from_slice(&1.0_f32.to_le_bytes());
+        packet[16..20].copy_from_slice(&(-2.0_f32).to_le_bytes());
+        assert_eq!(
+            decode_landing(&packet),
+            Ok(GoalPacket {
+                goal_id: 7,
+                north_metres: 1.0,
+                east_metres: -2.0,
+                up_metres: 0.0,
+            })
+        );
+        packet[20..24].copy_from_slice(&0.1_f32.to_le_bytes());
+        assert_eq!(decode_landing(&packet), Err(PacketError::NonFinite));
+    }
+}