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

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

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

Validation: assembled-prefix host tests, strict Clippy, platform/USB tests
and both ARM release builds. Final-source coverage and scoped Miri/Kani
evidence are retained separately from hardware qualification.

firmware/libs/kaptan-drivers/src/icm42688/error.rs

@@ -18,4 +18,16 @@ pub enum Error<BusError> {
     /// The selected SPI route reported a terminal transfer fault.
     #[kaptan_error(id = 5104)]
     TransferFault,
+    /// The bus could not safely accept and copy out the completed transfer.
+    #[kaptan_error(id = 5105)]
+    Completion(kaptan_spi::CompletionError),
+    /// An accepted SPI transfer exceeded the 10 ms service deadline.
+    #[kaptan_error(id = 5106)]
+    TransferTimeout,
+    /// The bus stayed busy for the pending operation's 20 ms admission budget.
+    #[kaptan_error(id = 5107)]
+    AdmissionTimeout,
+    /// No acquisition was requested within two output periods plus 10 ms.
+    #[kaptan_error(id = 5108)]
+    DataReadyTimeout,
 }

firmware/libs/kaptan-drivers/src/icm42688/mod.rs

@@ -5,7 +5,7 @@ mod sample;
 pub mod service;
 
 use kaptan_sensor::imu::ImuSnapshot;
-use kaptan_spi::{SpiCompletion, SpiDevice};
+use kaptan_spi::{CompletionError, SpiCompletion, SpiDevice};
 use kaptan_time::Timestamp;
 
 pub use error::Error;
@@ -27,6 +27,19 @@ const INTF_CONFIG1: u8 = 0x4d;
 const TEMP_DATA1: u8 = 0x1d;
 const SAMPLE_BYTES: usize = 14;
 const TRANSFER_CAPACITY: usize = SAMPLE_BYTES + 1;
+// TDK DS-000347 rev 1.5 §14.1 p.64: soft reset requires 1 ms. §3.3.2 p.14
+// also requires up to 1 ms before power-on register access. §14.36 p.76
+// requires 200 us without writes after OFF -> ON and at least 45 ms gyro ON
+// time. Use 45 ms as a conservative start-up allowance over §3.1/3.2's
+// TYPICAL 30/10 ms gyro/accel start-up, not a physical settling guarantee.
+const RESET_SETTLE_US: u64 = 1_000;
+const SENSOR_SETTLE_US: u64 = 45_000;
+const SAMPLE_PERIOD_US: u64 = 1_000;
+// Host service budget, not a datasheet delay. Supported SPI3 bursts take less
+// than 0.3 ms. Periodic service must continue independently of DMA and DRDY.
+const TRANSFER_TIMEOUT_US: u64 = 10_000;
+// Allows a retry on the F722's 10 ms periodic service before admission expires.
+const ADMISSION_TIMEOUT_US: u64 = 20_000;
 
 // SPI mode 0, active-high push-pull, pulsed INT1. Data-ready is cleared by
 // the SPI burst read and its source is explicitly enabled on INT1.
@@ -47,19 +60,40 @@ const CONFIGURATION: [(u8, u8); 9] = [
 #[derive(Clone, Copy, Debug, Eq, PartialEq)]
 enum State {
     Dormant,
-    Resetting,
-    WaitingForReset,
-    ReadingIdentity,
-    WritingConfiguration(usize),
-    WaitingForSensorStart,
-    Ready,
-    ReadingSample { timestamp: Timestamp },
+    Pending {
+        operation: Operation,
+        since: Timestamp,
+    },
+    Active {
+        operation: Operation,
+        since: Timestamp,
+    },
+    Waiting {
+        next: Operation,
+        since: Timestamp,
+        delay_us: u64,
+    },
+    Settling {
+        since: Timestamp,
+    },
+    Ready {
+        since: Timestamp,
+    },
+    Faulted,
 }
 
-/// Observable outcome of one ICM-42688-P SPI terminal event.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+enum Operation {
+    Reset,
+    Identity,
+    Configure(usize),
+    Sample,
+}
+
+/// Observable outcome of one timer or ICM-42688-P SPI terminal event.
 #[derive(Debug, Eq, PartialEq)]
 pub enum Icm42688Event<BusError> {
-    /// The selected SPI interrupt had no terminal transfer event.
+    /// No sample, initialization completion or terminal fault was produced.
     NotPending,
     /// One initialization transfer completed; initialization continues.
     Initializing,
@@ -73,9 +107,8 @@ pub enum Icm42688Event<BusError> {
 
 /// Configured ICM-42688-P driver.
 ///
-/// The driver owns fixed transfer buffers and progresses exclusively from
-/// timer, data-ready and SPI-DMA completion events. It never waits for a
-/// peripheral status flag.
+/// The driver owns ordinary movable staging buffers and progresses from timer,
+/// data-ready and completion events. Hardware never addresses these buffers.
 pub struct Icm42688<Bus> {
     bus: Bus,
     state: State,
@@ -96,125 +129,139 @@ impl<Bus> Icm42688<Bus> {
 }
 
 impl<Bus: SpiDevice> Icm42688<Bus> {
-    /// Starts the sensor reset. A subsequent periodic tick provides the
-    /// datasheet-required reset-settling interval before identification.
-    pub fn begin_initialization(&mut self) -> Result<(), Error<Bus::StartError>> {
+    /// Accepts initialization once. Busy admission is retried by periodic
+    /// service. All supplied instants must use one non-regressing boot clock.
+    /// Power must be stable by this call. Register access waits another 1 ms.
+    pub fn begin_initialization(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
         if self.state != State::Dormant {
             return Err(Error::NotReady);
         }
-        self.prepare_write(DEVICE_CONFIG, SOFT_RESET);
-        self.submit(2)?;
-        self.state = State::Resetting;
+        self.wait(Operation::Reset, now, RESET_SETTLE_US);
         Ok(())
     }
 
-    /// Advances reset/wake delays from the board periodic event.
-    pub fn advance_initialization(&mut self) -> Result<(), Error<Bus::StartError>> {
+    /// Services elapsed settling, admission and acquisition deadlines. Call
+    /// periodically even with an active transfer. No unsolicited reads occur.
+    pub fn on_periodic_tick(&mut self, now: Timestamp) -> Icm42688Event<Bus::StartError> {
+        if let Some(error) = self.check_deadline(now) {
+            return Icm42688Event::Fault(error);
+        }
         match self.state {
-            State::WaitingForReset => {
-                self.prepare_read(WHO_AM_I, 2);
-                self.submit(2)?;
-                self.state = State::ReadingIdentity;
+            State::Waiting {
+                next,
+                since,
+                delay_us,
+            } if elapsed(now, since, delay_us) => {
+                self.queue(next, now);
             }
-            State::WaitingForSensorStart => {
-                self.start_configuration(7)?;
+            State::Settling { since } if elapsed(now, since, SAMPLE_PERIOD_US) => {
+                self.state = State::Ready { since: now };
+                return Icm42688Event::Initialized;
             }
             _ => {}
         }
-        Ok(())
+        match self.submit_pending(now) {
+            Ok(()) => Icm42688Event::NotPending,
+            Err(error) => Icm42688Event::Fault(error),
+        }
     }
 
-    /// Starts one temperature, accelerometer and gyroscope burst read.
-    pub fn request_sample(&mut self, timestamp: Timestamp) -> Result<(), Error<Bus::StartError>> {
-        if self.state != State::Ready {
+    /// Accepts a burst-read request. Busy admission remains pending for periodic
+    /// retry. The eventual accepted submission time becomes the snapshot
+    /// timestamp, not the ISR time or a physical sampling epoch.
+    pub fn request_sample(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
+        if let Some(error) = self.check_deadline(now) {
+            return Err(error);
+        }
+        if !self.is_ready() {
             return Err(Error::NotReady);
         }
-        self.prepare_read(TEMP_DATA1, TRANSFER_CAPACITY);
-        self.submit(TRANSFER_CAPACITY)?;
-        self.state = State::ReadingSample { timestamp };
-        Ok(())
+        self.queue(Operation::Sample, now);
+        self.submit_pending(now)
     }
 
     /// Advances the state machine from an already-acknowledged SPI completion.
     pub fn on_transfer_complete(
         &mut self,
         completion: SpiCompletion,
+        now: Timestamp,
     ) -> Icm42688Event<Bus::StartError> {
-        self.bus.complete_transfer(completion);
-        match completion {
-            SpiCompletion::NotPending => Icm42688Event::NotPending,
-            SpiCompletion::TransferFault => {
-                self.state = State::Dormant;
+        if completion == SpiCompletion::NotPending {
+            return Icm42688Event::NotPending;
+        }
+        if let Some(error) = self.check_deadline(now) {
+            return Icm42688Event::Fault(error);
+        }
+        let State::Active { operation, since } = self.state else {
+            return Icm42688Event::NotPending;
+        };
+        let length = if operation == Operation::Sample {
+            TRANSFER_CAPACITY
+        } else {
+            2
+        };
+        match self
+            .bus
+            .complete_transfer(completion, &mut self.receive[..length])
+        {
+            Ok(()) => self.advance_after_completion(operation, since, now),
+            Err(CompletionError::NotPending) => Icm42688Event::NotPending,
+            Err(CompletionError::TransferFault) => {
+                self.state = State::Faulted;
                 Icm42688Event::Fault(Error::TransferFault)
             }
-            SpiCompletion::Complete => self.advance_after_completion(),
+            Err(error) => {
+                self.state = State::Faulted;
+                Icm42688Event::Fault(Error::Completion(error))
+            }
         }
     }
 
     /// Returns whether an IMU data-ready event can start a sensor transfer.
     pub fn is_ready(&self) -> bool {
-        self.state == State::Ready
+        matches!(self.state, State::Ready { .. })
     }
 
-    fn advance_after_completion(&mut self) -> Icm42688Event<Bus::StartError> {
-        match self.state {
-            State::Dormant => Icm42688Event::NotPending,
-            State::Resetting => {
-                self.state = State::WaitingForReset;
-                Icm42688Event::Initializing
-            }
-            State::WaitingForReset | State::WaitingForSensorStart => Icm42688Event::NotPending,
-            State::ReadingIdentity => {
+    fn advance_after_completion(
+        &mut self,
+        operation: Operation,
+        requested: Timestamp,
+        now: Timestamp,
+    ) -> Icm42688Event<Bus::StartError> {
+        match operation {
+            Operation::Reset => self.wait(Operation::Identity, now, RESET_SETTLE_US),
+            Operation::Identity => {
                 let identity = self.receive[1];
                 if identity != EXPECTED_WHO_AM_I {
-                    self.state = State::Dormant;
+                    self.state = State::Faulted;
                     return Icm42688Event::Fault(Error::UnexpectedIdentity(identity));
                 }
-                self.start_configuration_event(0)
+                self.queue(Operation::Configure(0), now);
             }
-            State::WritingConfiguration(index) => {
+            Operation::Configure(index) => {
                 let next = index + 1;
                 if next == 7 {
-                    // PWR_MGMT0 starts both sensor paths.  The following
-                    // timer tick gives them more than the required 200 µs
-                    // before their output-rate configuration is written.
-                    self.state = State::WaitingForSensorStart;
-                    Icm42688Event::Initializing
+                    // Includes the 200 us no-write interval after PWR_MGMT0.
+                    self.wait(Operation::Configure(next), now, SENSOR_SETTLE_US);
                 } else if next == CONFIGURATION.len() {
-                    self.state = State::Ready;
-                    Icm42688Event::Initialized
+                    self.state = State::Settling { since: now };
                 } else {
-                    self.start_configuration_event(next)
+                    self.queue(Operation::Configure(next), now);
                 }
             }
-            State::Ready => Icm42688Event::NotPending,
-            State::ReadingSample { timestamp } => {
+            Operation::Sample => {
                 let mut bytes = [0; SAMPLE_BYTES];
                 bytes.copy_from_slice(&self.receive[1..]);
-                self.state = State::Ready;
-                Icm42688Event::Sample(sample::decode(&bytes, timestamp))
+                self.state = State::Ready { since: now };
+                return Icm42688Event::Sample(sample::decode(&bytes, requested));
             }
         }
-    }
-
-    fn start_configuration_event(&mut self, index: usize) -> Icm42688Event<Bus::StartError> {
-        match self.start_configuration(index) {
+        match self.submit_pending(now) {
             Ok(()) => Icm42688Event::Initializing,
             Err(error) => Icm42688Event::Fault(error),
         }
     }
 
-    fn start_configuration(&mut self, index: usize) -> Result<(), Error<Bus::StartError>> {
-        let (register, value) = CONFIGURATION[index];
-        self.prepare_write(register, value);
-        if let Err(error) = self.submit(2) {
-            self.state = State::Dormant;
-            return Err(error);
-        }
-        self.state = State::WritingConfiguration(index);
-        Ok(())
-    }
-
     fn prepare_read(&mut self, register: u8, length: usize) {
         self.transmit = [0; TRANSFER_CAPACITY];
         self.receive = [0; TRANSFER_CAPACITY];
@@ -228,83 +275,337 @@ impl<Bus: SpiDevice> Icm42688<Bus> {
         self.transmit[..2].copy_from_slice(&[register, value]);
     }
 
-    fn submit(&mut self, length: usize) -> Result<(), Error<Bus::StartError>> {
-        let (bus, transmit, receive) = (&mut self.bus, &self.transmit, &mut self.receive);
-        bus.start_transfer(&transmit[..length], &mut receive[..length])
-            .map_err(Error::Start)
+    fn queue(&mut self, operation: Operation, now: Timestamp) {
+        self.state = State::Pending {
+            operation,
+            since: now,
+        };
     }
+
+    fn wait(&mut self, next: Operation, now: Timestamp, delay_us: u64) {
+        self.state = State::Waiting {
+            next,
+            since: now,
+            delay_us,
+        };
+    }
+
+    fn submit_pending(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
+        let State::Pending { operation, .. } = self.state else {
+            return Ok(());
+        };
+        match operation {
+            Operation::Reset => self.prepare_write(DEVICE_CONFIG, SOFT_RESET),
+            Operation::Identity => self.prepare_read(WHO_AM_I, 2),
+            Operation::Configure(index) => {
+                let (register, value) = CONFIGURATION[index];
+                self.prepare_write(register, value);
+            }
+            Operation::Sample => self.prepare_read(TEMP_DATA1, TRANSFER_CAPACITY),
+        }
+        let length = if operation == Operation::Sample {
+            TRANSFER_CAPACITY
+        } else {
+            2
+        };
+        match self.bus.start_transfer(&self.transmit[..length]) {
+            Ok(()) => {
+                self.state = State::Active {
+                    operation,
+                    since: now,
+                };
+                Ok(())
+            }
+            Err(error) if Bus::is_busy(&error) => Ok(()),
+            Err(error) => {
+                self.state = State::Faulted;
+                Err(Error::Start(error))
+            }
+        }
+    }
+
+    fn check_deadline(&mut self, now: Timestamp) -> Option<Error<Bus::StartError>> {
+        let error = match self.state {
+            State::Active { since, .. } if elapsed(now, since, TRANSFER_TIMEOUT_US) => {
+                // This terminal fault proves quiescence or quarantines permanent
+                // DMA storage. A timeout alone never releases that storage.
+                match self
+                    .bus
+                    .complete_transfer(SpiCompletion::TransferFault, &mut [])
+                {
+                    Err(CompletionError::TransferFault | CompletionError::NotActive) | Ok(()) => {
+                        Error::TransferTimeout
+                    }
+                    Err(error) => Error::Completion(error),
+                }
+            }
+            State::Pending { since, .. } if elapsed(now, since, ADMISSION_TIMEOUT_US) => {
+                Error::AdmissionTimeout
+            }
+            State::Ready { since }
+                if elapsed(now, since, 2 * SAMPLE_PERIOD_US + TRANSFER_TIMEOUT_US) =>
+            {
+                Error::DataReadyTimeout
+            }
+            _ => return None,
+        };
+        self.state = State::Faulted;
+        Some(error)
+    }
+}
+
+fn elapsed(now: Timestamp, since: Timestamp, duration_us: u64) -> bool {
+    now.as_microseconds()
+        .checked_sub(since.as_microseconds())
+        .is_some_and(|elapsed| elapsed >= duration_us)
 }
 
 #[cfg(test)]
 mod tests {
-    use super::{Error, Icm42688, Icm42688Event, TRANSFER_CAPACITY};
-    use kaptan_spi::{SpiCompletion, SpiDevice};
+    use super::*;
+    use crate::mpu6000::test_bus::{FakeBus, FakeError};
+    use kaptan_spi::{CompletionError, SpiCompletion};
     use kaptan_time::Timestamp;
 
-    struct Bus {
-        response: [u8; TRANSFER_CAPACITY],
+    fn t(us: u64) -> Timestamp {
+        Timestamp::from_microseconds(us + 1_000)
     }
 
-    impl SpiDevice for Bus {
-        type StartError = ();
-
-        fn start_transfer(&mut self, _: &[u8], receive: &mut [u8]) -> Result<(), Self::StartError> {
-            receive.copy_from_slice(&self.response[..receive.len()]);
-            Ok(())
-        }
-
-        fn complete_transfer(&mut self, _: SpiCompletion) {}
+    fn start(driver: &mut Icm42688<FakeBus>) {
+        driver
+            .begin_initialization(Timestamp::from_microseconds(0))
+            .unwrap();
+        driver.on_periodic_tick(Timestamp::from_microseconds(999));
+        assert_eq!(driver.bus.starts, 0);
+        driver.on_periodic_tick(t(0));
     }
 
-    #[test]
-    fn accepts_a_sample_only_after_identity_and_wake_delays() {
-        let mut response = [0; TRANSFER_CAPACITY];
-        response[1] = 0x47;
-        let mut imu = Icm42688::new(Bus { response });
-        imu.begin_initialization().unwrap();
+    fn initialized() -> Icm42688<FakeBus> {
+        let mut bus = FakeBus::ready();
+        bus.response[1] = 0x47;
+        let mut imu = Icm42688::new(bus);
+        start(&mut imu);
         assert!(matches!(
-            imu.on_transfer_complete(SpiCompletion::Complete),
+            imu.on_transfer_complete(SpiCompletion::Complete, t(100)),
             Icm42688Event::Initializing
         ));
-        assert!(matches!(imu.advance_initialization(), Ok(())));
+        imu.on_periodic_tick(t(1_099));
+        assert_eq!(imu.bus.starts, 1);
+        imu.on_periodic_tick(t(1_100));
         assert!(matches!(
-            imu.on_transfer_complete(SpiCompletion::Complete),
+            imu.on_transfer_complete(SpiCompletion::Complete, t(1_200)),
             Icm42688Event::Initializing
         ));
-        for _ in 0..7 {
+        for index in 0..7 {
             assert!(matches!(
-                imu.on_transfer_complete(SpiCompletion::Complete),
+                imu.on_transfer_complete(SpiCompletion::Complete, t(1_300 + index * 100)),
                 Icm42688Event::Initializing
             ));
         }
-        assert!(matches!(imu.advance_initialization(), Ok(())));
+        for now in [1_900, 2_099, 2_100, 31_900, 46_899] {
+            imu.on_periodic_tick(t(now));
+            assert_eq!(imu.bus.starts, 9);
+        }
+        imu.on_periodic_tick(t(46_900));
         assert!(matches!(
-            imu.on_transfer_complete(SpiCompletion::Complete),
+            imu.on_transfer_complete(SpiCompletion::Complete, t(47_000)),
             Icm42688Event::Initializing
         ));
-        assert!(matches!(
-            imu.on_transfer_complete(SpiCompletion::Complete),
-            Icm42688Event::Initialized
-        ));
+        imu.on_transfer_complete(SpiCompletion::Complete, t(47_100));
+        imu.on_periodic_tick(t(48_099));
+        assert!(!imu.is_ready());
+        assert_eq!(imu.request_sample(t(48_099)), Err(Error::NotReady));
+        assert_eq!(imu.on_periodic_tick(t(48_100)), Icm42688Event::Initialized);
         assert!(imu.is_ready());
-        imu.request_sample(Timestamp::from_microseconds(1)).unwrap();
+        imu
+    }
+
+    #[test]
+    fn accepts_a_sample_only_after_elapsed_reset_wake_and_data_settling() {
+        let mut imu = initialized();
+        imu.request_sample(t(48_101)).unwrap();
+        assert_eq!(imu.receive, [0; TRANSFER_CAPACITY]);
+        assert_eq!(
+            imu.on_transfer_complete(SpiCompletion::NotPending, t(48_102)),
+            Icm42688Event::NotPending
+        );
+        assert_eq!(imu.receive, [0; TRANSFER_CAPACITY]);
+        imu.transmit.fill(0xff);
+        imu.bus.response.fill(0xff);
+        let mut imu = core::mem::replace(&mut imu, Icm42688::new(FakeBus::ready()));
         assert!(matches!(
-            imu.on_transfer_complete(SpiCompletion::Complete),
-            Icm42688Event::Sample(_)
+            imu.on_transfer_complete(SpiCompletion::Complete, t(48_300)),
+            Icm42688Event::Sample(sample) if sample.timestamp == t(48_101)
         ));
+        assert_eq!(imu.receive[1], 0x47);
+        assert_eq!(
+            imu.on_transfer_complete(SpiCompletion::Complete, t(48_301)),
+            Icm42688Event::NotPending
+        );
     }
 
     #[test]
     fn rejects_a_wrong_identity() {
-        let mut imu = Icm42688::new(Bus {
-            response: [0; TRANSFER_CAPACITY],
-        });
-        imu.begin_initialization().unwrap();
-        let _ = imu.on_transfer_complete(SpiCompletion::Complete);
-        imu.advance_initialization().unwrap();
+        let mut imu = Icm42688::new(FakeBus::ready());
+        start(&mut imu);
+        let _ = imu.on_transfer_complete(SpiCompletion::Complete, t(0));
+        imu.on_periodic_tick(t(1_000));
         assert!(matches!(
-            imu.on_transfer_complete(SpiCompletion::Complete),
+            imu.on_transfer_complete(SpiCompletion::Complete, t(1_100)),
             Icm42688Event::Fault(Error::UnexpectedIdentity(0))
         ));
     }
+
+    #[test]
+    fn completion_errors_never_republish_a_previous_sample() {
+        for (event, error) in [
+            (
+                SpiCompletion::Complete,
+                Some(CompletionError::InvalidLength),
+            ),
+            (SpiCompletion::Complete, Some(CompletionError::Quarantined)),
+            (SpiCompletion::Complete, Some(CompletionError::NotActive)),
+            (
+                SpiCompletion::Complete,
+                Some(CompletionError::TransferFault),
+            ),
+            (SpiCompletion::TransferFault, None),
+        ] {
+            let mut driver = Icm42688::new(FakeBus::ready());
+            driver.state = State::Ready { since: t(0) };
+            driver.request_sample(t(1)).unwrap();
+            assert!(matches!(
+                driver.on_transfer_complete(SpiCompletion::Complete, t(1)),
+                Icm42688Event::Sample(_)
+            ));
+            driver.request_sample(t(2)).unwrap();
+            driver.receive.fill(0xa5);
+            driver.bus.fail_completion = error;
+            assert!(matches!(
+                driver.on_transfer_complete(event, t(2)),
+                Icm42688Event::Fault(_)
+            ));
+            assert_eq!(driver.receive, [0xa5; TRANSFER_CAPACITY]);
+            assert!(!driver.is_ready());
+            assert!(!matches!(
+                driver.on_transfer_complete(SpiCompletion::Complete, t(3)),
+                Icm42688Event::Sample(_)
+            ));
+            if error == Some(CompletionError::Quarantined) {
+                assert_eq!(driver.begin_initialization(t(4)), Err(Error::NotReady));
+            }
+        }
+    }
+
+    #[test]
+    fn rejected_submission_cannot_complete_a_stale_sample() {
+        let mut driver = Icm42688::new(FakeBus::ready());
+        driver.state = State::Ready { since: t(0) };
+        driver.receive.fill(0xa5);
+        driver.bus.fail_start = true;
+        assert_eq!(
+            driver.request_sample(t(1)),
+            Err(Error::Start(FakeError::Start))
+        );
+        assert_eq!(
+            driver.on_transfer_complete(SpiCompletion::Complete, t(2)),
+            Icm42688Event::NotPending
+        );
+    }
+
+    #[test]
+    fn every_phase_times_out_and_cancels_once_without_rearming() {
+        for operation in [Operation::Reset, Operation::Identity, Operation::Sample]
+            .into_iter()
+            .chain((0..9).map(Operation::Configure))
+        {
+            let mut driver = Icm42688::new(FakeBus::ready());
+            driver.queue(operation, t(100));
+            driver.submit_pending(t(100)).unwrap();
+            assert_eq!(
+                driver.on_periodic_tick(t(10_099)),
+                Icm42688Event::NotPending
+            );
+            assert_eq!(driver.bus.cancellations, 0);
+            assert_eq!(
+                driver.on_periodic_tick(t(10_100)),
+                Icm42688Event::Fault(Error::TransferTimeout)
+            );
+            assert_eq!(driver.bus.cancellations, 1);
+            assert_eq!(
+                driver.on_transfer_complete(SpiCompletion::Complete, t(10_101)),
+                Icm42688Event::NotPending
+            );
+            driver.on_periodic_tick(t(1_000_000));
+            assert_eq!(driver.bus.starts, 1);
+            assert_eq!(
+                driver.begin_initialization(t(1_000_000)),
+                Err(Error::NotReady)
+            );
+        }
+    }
+
+    #[test]
+    fn every_busy_phase_preserves_pending_operation_and_has_an_admission_deadline() {
+        for operation in [Operation::Reset, Operation::Identity, Operation::Sample]
+            .into_iter()
+            .chain((0..9).map(Operation::Configure))
+        {
+            let mut driver = Icm42688::new(FakeBus::ready());
+            driver.bus.busy = true;
+            driver.queue(operation, t(100));
+            driver.submit_pending(t(100)).unwrap();
+            assert_eq!(
+                driver.state,
+                State::Pending {
+                    operation,
+                    since: t(100)
+                }
+            );
+            driver.on_periodic_tick(t(200));
+            driver.bus.busy = false;
+            driver.on_periodic_tick(t(300));
+            assert_eq!(
+                driver.state,
+                State::Active {
+                    operation,
+                    since: t(300)
+                }
+            );
+            assert_eq!(driver.bus.starts, 1);
+        }
+        let mut driver = Icm42688::new(FakeBus::ready());
+        driver.bus.busy = true;
+        start(&mut driver);
+        assert_eq!(
+            driver.on_periodic_tick(t(20_000)),
+            Icm42688Event::Fault(Error::AdmissionTimeout)
+        );
+        assert_eq!(driver.bus.cancellations, 0);
+    }
+
+    #[test]
+    fn missed_drdy_late_completion_and_failed_cancellation_never_publish() {
+        let mut driver = initialized();
+        driver.on_periodic_tick(t(60_099));
+        assert_eq!(driver.bus.starts, 11);
+        assert_eq!(
+            driver.on_periodic_tick(t(60_100)),
+            Icm42688Event::Fault(Error::DataReadyTimeout)
+        );
+        let mut driver = initialized();
+        driver.request_sample(t(49_000)).unwrap();
+        assert_eq!(
+            driver.on_transfer_complete(SpiCompletion::Complete, t(59_000)),
+            Icm42688Event::Fault(Error::TransferTimeout)
+        );
+        let mut driver = initialized();
+        driver.request_sample(t(49_000)).unwrap();
+        driver.bus.fail_completion = Some(CompletionError::Quarantined);
+        assert_eq!(
+            driver.on_periodic_tick(t(59_000)),
+            Icm42688Event::Fault(Error::Completion(CompletionError::Quarantined))
+        );
+        assert_eq!(driver.bus.start_transfer(&[1]), Err(FakeError::Quarantined));
+    }
 }

firmware/libs/kaptan-drivers/src/icm42688/sample.rs

@@ -1,4 +1,9 @@
 //! ICM-42688-P burst-sample decoding.
+//!
+//! Integer division rounds toward zero before adding temperature's offset.
+//! Over the full signed raw domain, the configured ranges decode to
+//! -4,000..=3,999 mg, -8,731..=8,731 mrad/s and -22,234..=27,233 cdegC.
+//! Products fit i32 and the acceleration and temperature results fit i16.
 
 use kaptan_sensor::imu::ImuSnapshot;
 use kaptan_time::Timestamp;
@@ -32,18 +37,18 @@ const fn scale_acceleration(raw: i16) -> i16 {
 }
 
 // The configured ±500°/s range has 65.5 counts/(°/s).
-const fn scale_angular_rate(raw: i16) -> i16 {
-    ((raw as i32 * 17_453) / 65_500) as i16
+const fn scale_angular_rate(raw: i16) -> i32 {
+    (raw as i32 * 17_453) / 65_500
 }
 
 // ICM-42688-P temperature is 132.48 counts/°C with a 25°C offset.
 const fn scale_temperature(raw: i16) -> i16 {
-    ((raw as i32 * 100) / 13_248 + 2_500) as i16
+    ((raw as i32 * 10_000) / 13_248 + 2_500) as i16
 }
 
 #[cfg(test)]
 mod tests {
-    use super::decode;
+    use super::{decode, scale_temperature};
     use kaptan_time::Timestamp;
 
     #[test]
@@ -54,4 +59,61 @@ mod tests {
         assert_eq!(sample.angular_rate_milliradians_per_second[0], 8_725);
         assert_eq!(sample.temperature_centidegrees_celsius, 2_500);
     }
+
+    #[test]
+    fn signed_raw_endpoints_fit_the_configured_ranges() {
+        for (temperature_raw, temperature) in [(i16::MIN, -22_234), (i16::MAX, 27_233)] {
+            let mut bytes = [0; 14];
+            for (index, raw) in [
+                temperature_raw,
+                i16::MIN,
+                0,
+                i16::MAX,
+                i16::MIN,
+                0,
+                i16::MAX,
+            ]
+            .into_iter()
+            .enumerate()
+            {
+                bytes[index * 2..index * 2 + 2].copy_from_slice(&raw.to_be_bytes());
+            }
+            let timestamp = Timestamp::from_microseconds(123);
+            let sample = decode(&bytes, timestamp);
+            assert_eq!(sample.timestamp, timestamp);
+            assert_eq!(sample.acceleration_milligravity, [-4_000, 0, 3_999]);
+            assert_eq!(
+                sample.angular_rate_milliradians_per_second,
+                [-8_731, 0, 8_731]
+            );
+            assert_eq!(sample.temperature_centidegrees_celsius, temperature);
+        }
+    }
+
+    #[test]
+    fn temperature_matches_datasheet_scale_and_rounding() {
+        // ±13,248 counts is ±100°C relative to the 25°C offset.
+        for (raw, expected) in [
+            (-13_248, -7_500),
+            (-132, 2_401),
+            (-1, 2_500),
+            (0, 2_500),
+            (1, 2_500),
+            (132, 2_599),
+            (13_248, 12_500),
+        ] {
+            assert_eq!(scale_temperature(raw), expected);
+        }
+
+        // Independent floating-point datasheet reference over all 65,536 raw
+        // codes. Truncation may lose less than one centidegree, never wrap.
+        let mut previous = i16::MIN;
+        for raw in i16::MIN..=i16::MAX {
+            let actual = scale_temperature(raw);
+            let reference = (f64::from(raw) / 132.48 + 25.0) * 100.0;
+            assert!((f64::from(actual) - reference).abs() < 1.0);
+            assert!(actual >= previous);
+            previous = actual;
+        }
+    }
 }

firmware/libs/kaptan-drivers/src/icm42688/service.rs

@@ -36,27 +36,18 @@ where
     Clock: MonotonicClock,
     Bus: SpiDevice,
 {
-    /// Submits the asynchronous reset transaction.
+    /// Accepts initialization and starts the power-on register-access delay.
     pub fn begin(&mut self) -> Result<(), Error<Bus::StartError>> {
-        self.imu.begin_initialization()
+        self.imu.begin_initialization(self.clock.now())
     }
 
-    /// Advances reset and sensor-start settling states on a periodic tick.
+    /// Services settling and missing-IRQ deadlines even with an active transfer.
     pub fn on_periodic_tick(&mut self) -> Icm42688ServiceEvent<Bus::StartError> {
-        if self.imu.is_ready() {
-            return match self.imu.request_sample(self.clock.now()) {
-                Ok(()) | Err(Error::NotReady) => Icm42688ServiceEvent::Idle,
-                Err(error) => Icm42688ServiceEvent::Fault(error),
-            };
-        }
-        let _ = self.clock.now();
-        match self.imu.advance_initialization() {
-            Ok(()) => Icm42688ServiceEvent::Idle,
-            Err(error) => Icm42688ServiceEvent::Fault(error),
-        }
+        service_event(self.imu.on_periodic_tick(self.clock.now()))
     }
 
-    /// Handles one coalesced data-ready event.
+    /// Handles one coalesced data-ready notification. Time is captured here in
+    /// the worker, at read submission, not at IRQ entry or physical sampling.
     pub fn on_data_ready(&mut self) -> Icm42688ServiceEvent<Bus::StartError> {
         if !self.imu.is_ready() {
             return Icm42688ServiceEvent::Idle;
@@ -72,11 +63,110 @@ where
         &mut self,
         completion: SpiCompletion,
     ) -> Icm42688ServiceEvent<Bus::StartError> {
-        match self.imu.on_transfer_complete(completion) {
-            Icm42688Event::NotPending | Icm42688Event::Initializing => Icm42688ServiceEvent::Idle,
-            Icm42688Event::Initialized => Icm42688ServiceEvent::Initialized,
-            Icm42688Event::Sample(sample) => Icm42688ServiceEvent::Sample(sample),
-            Icm42688Event::Fault(error) => Icm42688ServiceEvent::Fault(error),
+        service_event(self.imu.on_transfer_complete(completion, self.clock.now()))
+    }
+}
+
+fn service_event<E>(event: Icm42688Event<E>) -> Icm42688ServiceEvent<E> {
+    match event {
+        Icm42688Event::NotPending | Icm42688Event::Initializing => Icm42688ServiceEvent::Idle,
+        Icm42688Event::Initialized => Icm42688ServiceEvent::Initialized,
+        Icm42688Event::Sample(sample) => Icm42688ServiceEvent::Sample(sample),
+        Icm42688Event::Fault(error) => Icm42688ServiceEvent::Fault(error),
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::mpu6000::test_bus::FakeBus;
+    use kaptan_time::Timestamp;
+
+    struct Clock(u64);
+    impl MonotonicClock for Clock {
+        fn now(&mut self) -> Timestamp {
+            Timestamp::from_microseconds(self.0)
         }
+    }
+
+    fn initialized() -> Icm42688Service<Clock, FakeBus> {
+        let mut bus = FakeBus::ready();
+        bus.response[1] = 0x47;
+        let mut service = Icm42688Service::new(Clock(0), Icm42688::new(bus));
+        service.begin().unwrap();
+        service.clock.0 = 999;
+        service.on_periodic_tick();
+        assert_eq!(service.imu.bus.starts, 0);
+        service.clock.0 = 1_000;
+        service.on_periodic_tick();
+        service.on_transfer_complete(SpiCompletion::Complete);
+        service.clock.0 = 1_999;
+        service.on_periodic_tick();
+        assert_eq!(service.imu.bus.starts, 1);
+        service.clock.0 = 2_000;
+        service.on_periodic_tick();
+        for _ in 0..8 {
+            service.on_transfer_complete(SpiCompletion::Complete);
+        }
+        service.clock.0 = 46_999;
+        service.on_periodic_tick();
+        assert_eq!(service.imu.bus.starts, 9);
+        service.clock.0 = 47_000;
+        service.on_periodic_tick();
+        service.on_transfer_complete(SpiCompletion::Complete);
+        service.on_transfer_complete(SpiCompletion::Complete);
+        service.clock.0 = 48_000;
+        assert!(matches!(
+            service.on_periodic_tick(),
+            Icm42688ServiceEvent::Initialized
+        ));
+        service
+    }
+
+    #[test]
+    fn fake_clock_labels_the_accepted_read_request_even_after_busy_admission() {
+        let mut service = initialized();
+        service.clock.0 = 48_100;
+        service.imu.bus.busy = true;
+        assert!(matches!(
+            service.on_data_ready(),
+            Icm42688ServiceEvent::Idle
+        ));
+        service.clock.0 = 48_200;
+        service.imu.bus.busy = false;
+        service.on_periodic_tick();
+        service.clock.0 = 48_500;
+        assert!(
+            matches!(service.on_transfer_complete(SpiCompletion::Complete),
+            Icm42688ServiceEvent::Sample(sample) if sample.timestamp == Timestamp::from_microseconds(48_200))
+        );
+    }
+
+    #[test]
+    fn fake_clock_reports_missing_drdy_and_dma_without_polling_samples() {
+        let mut service = initialized();
+        service.clock.0 = 59_999;
+        assert!(matches!(
+            service.on_periodic_tick(),
+            Icm42688ServiceEvent::Idle
+        ));
+        assert_eq!(service.imu.bus.starts, 11);
+        service.clock.0 = 60_000;
+        assert!(matches!(
+            service.on_periodic_tick(),
+            Icm42688ServiceEvent::Fault(Error::DataReadyTimeout)
+        ));
+        let mut service = initialized();
+        service.on_data_ready();
+        service.clock.0 = 58_000;
+        assert!(matches!(
+            service.on_periodic_tick(),
+            Icm42688ServiceEvent::Fault(Error::TransferTimeout)
+        ));
+        assert!(matches!(
+            service.on_transfer_complete(SpiCompletion::Complete),
+            Icm42688ServiceEvent::Idle
+        ));
+        assert_eq!(service.imu.bus.cancellations, 1);
     }
 }

firmware/libs/kaptan-drivers/src/lib.rs

@@ -1,8 +1,4 @@
 //! Hardware-independent chip protocol drivers.
-//!
-//! Drivers express device register protocols and data conversion through small
-//! bus traits. MCU, pin, DMA, interrupt, and RTIC mechanics belong below this
-//! layer in a HAL route implementation and its platform composition.
 
 #![no_std]
 

firmware/libs/kaptan-drivers/src/lidar_lite_v3/error.rs

@@ -29,6 +29,9 @@ pub enum Error<BusError> {
     /// The device reported a documented no-signal sentinel distance of zero or one centimetre.
     #[kaptan_error(id = 5208)]
     NoSignal,
+    /// The controller rejected completion or could not prove physical STOP.
+    #[kaptan_error(id = 5209)]
+    Completion(kaptan_i2c::CompletionError),
 }
 
 /// LIDAR-Lite v3 startup lifecycle failure.

firmware/libs/kaptan-drivers/src/lidar_lite_v3/mod.rs

@@ -4,7 +4,7 @@ mod error;
 mod registers;
 pub mod service;
 
-use kaptan_i2c::{I2cBus, I2cCompletion};
+use kaptan_i2c::{CompletionError, I2cBus, I2cCompletion};
 use kaptan_sensor::range::RangeSnapshot;
 use kaptan_time::Timestamp;
 
@@ -24,6 +24,8 @@ const DISTANCE_LENGTH: usize = 2;
 ///
 /// The extra margin preserves liveness if the optical engine remains busy
 /// while still accepting a documented slow acquisition.
+/// Completion at exactly this elapsed time is accepted. Later completion is
+/// discarded, regardless of whether the periodic timeout handler has run.
 const MEASUREMENT_TIMEOUT_MICROSECONDS: u64 = 200_000;
 
 /// Receiver-bias correction selection for one acquisition.
@@ -50,6 +52,7 @@ impl AcquisitionMode {
 #[derive(Clone, Copy, Debug, Eq, PartialEq)]
 enum State {
     Settling,
+    Faulted,
     Idle,
     Starting { timestamp: Timestamp },
     Waiting { timestamp: Timestamp },
@@ -174,6 +177,7 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
         timestamp: Timestamp,
         mode: AcquisitionMode,
     ) -> Result<(), Error<Bus::StartError>> {
+        self.service_bus(timestamp)?;
         if self.state != State::Idle {
             return Err(Error::NotReady);
         }
@@ -192,15 +196,16 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
     /// therefore only records phase completion; this periodic path begins the
     /// following transaction after the bus has been released.
     pub fn poll_measurement(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
+        self.service_bus(now)?;
         let timestamp = match self.state {
             State::Waiting { timestamp }
             | State::StatusReadPending { timestamp }
             | State::DistanceRegisterPending { timestamp }
             | State::DistanceReadPending { timestamp } => timestamp,
-            State::Settling | State::Idle => return Err(Error::NotReady),
+            State::Settling | State::Idle | State::Faulted => return Err(Error::NotReady),
             _ => {
                 if self.measurement_timed_out(now) {
-                    self.bus.cancel_transaction();
+                    self.bus.cancel_transaction(now);
                     self.state = State::Idle;
                     return Err(Error::MeasurementTimeout);
                 }
@@ -208,7 +213,7 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
             }
         };
         if self.measurement_timed_out(now) {
-            self.bus.cancel_transaction();
+            self.bus.cancel_transaction(now);
             self.state = State::Idle;
             return Err(Error::MeasurementTimeout);
         }
@@ -222,17 +227,36 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
     }
 
     /// Advances the state machine from an already-acknowledged I²C terminal event.
+    /// `now` is the actual completion-handler time in the acquisition clock domain.
     pub fn on_transaction_complete(
         &mut self,
         completion: I2cCompletion,
+        now: Timestamp,
     ) -> LidarLiteV3Event<Bus::StartError> {
         if completion == I2cCompletion::NotPending || !self.has_active_transaction() {
             return LidarLiteV3Event::NotPending;
         }
 
+        if self.measurement_timed_out(now) {
+            // Discard a controller-owned completed read without copying it out.
+            // If STOP is still pending, cancellation keeps admission closed and
+            // periodic bus service must finish STOP or report StopUnproven.
+            self.bus.cancel_transaction(now);
+            self.state = State::Idle;
+            return LidarLiteV3Event::Fault(Error::MeasurementTimeout);
+        }
+
         match completion {
             I2cCompletion::TransferFault => {
-                self.bus.complete_transaction(completion, None);
+                let result = self.bus.complete_transaction(completion, None);
+                match result {
+                    Err(CompletionError::NotPending) => return LidarLiteV3Event::NotPending,
+                    Err(CompletionError::TransferFault) | Ok(()) => {}
+                    Err(error) => {
+                        self.state = State::Faulted;
+                        return LidarLiteV3Event::Fault(Error::Completion(error));
+                    }
+                }
                 self.state = State::Idle;
                 LidarLiteV3Event::Fault(Error::TransferFault)
             }
@@ -242,7 +266,13 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
                     State::ReadingDistance { .. } => Some(&mut self.receive[..DISTANCE_LENGTH]),
                     _ => None,
                 };
-                self.bus.complete_transaction(completion, read);
+                if let Err(error) = self.bus.complete_transaction(completion, read) {
+                    if error == CompletionError::NotPending {
+                        return LidarLiteV3Event::NotPending;
+                    }
+                    self.state = State::Faulted;
+                    return LidarLiteV3Event::Fault(Error::Completion(error));
+                }
                 self.advance_after_completion()
             }
             I2cCompletion::NotPending => unreachable!(),
@@ -254,6 +284,14 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
         self.state == State::Idle
     }
 
+    fn service_bus(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
+        if let Err(error) = self.bus.service(now) {
+            self.state = State::Faulted;
+            return Err(Error::Completion(error));
+        }
+        Ok(())
+    }
+
     fn has_active_transaction(&self) -> bool {
         matches!(
             self.state,
@@ -276,16 +314,16 @@ impl<Bus: I2cBus> LidarLiteV3<Bus> {
             | State::WritingDistanceRegister { timestamp }
             | State::DistanceReadPending { timestamp }
             | State::ReadingDistance { timestamp } => timestamp,
-            State::Settling | State::Idle => return false,
+            State::Settling | State::Idle | State::Faulted => return false,
         };
         now.as_microseconds()
             .saturating_sub(timestamp.as_microseconds())
-            >= MEASUREMENT_TIMEOUT_MICROSECONDS
+            > MEASUREMENT_TIMEOUT_MICROSECONDS
     }
 
     fn advance_after_completion(&mut self) -> LidarLiteV3Event<Bus::StartError> {
         match self.state {
-            State::Settling => LidarLiteV3Event::NotPending,
+            State::Settling | State::Faulted => LidarLiteV3Event::NotPending,
             State::Idle => LidarLiteV3Event::NotPending,
             State::Starting { timestamp } => {
                 self.state = State::Waiting { timestamp };
@@ -380,7 +418,7 @@ mod tests {
         AcquisitionMode, Error, LidarLiteV3, LidarLiteV3Address, LidarLiteV3Event,
         MEASUREMENT_TIMEOUT_MICROSECONDS, StartupError,
     };
-    use kaptan_i2c::{I2cBus, I2cCompletion};
+    use kaptan_i2c::{CompletionError, I2cBus, I2cCompletion};
     use kaptan_sensor::range::RangeSnapshot;
     use kaptan_time::Timestamp;
 
@@ -408,7 +446,11 @@ mod tests {
         response: [u8; 2],
         fail_start: bool,
         completions: usize,
+        read_copies: usize,
         cancellations: usize,
+        completion_error: Option<CompletionError>,
+        service_error: Option<CompletionError>,
+        cancelled_at: Option<Timestamp>,
     }
 
     impl FakeBus {
@@ -418,7 +460,11 @@ mod tests {
                 response: [0; 2],
                 fail_start: false,
                 completions: 0,
+                read_copies: 0,
                 cancellations: 0,
+                completion_error: None,
+                service_error: None,
+                cancelled_at: None,
             }
         }
     }
@@ -454,17 +500,36 @@ mod tests {
             Ok(())
         }
 
-        fn complete_transaction(&mut self, completion: I2cCompletion, read: Option<&mut [u8]>) {
+        fn complete_transaction(
+            &mut self,
+            completion: I2cCompletion,
+            read: Option<&mut [u8]>,
+        ) -> Result<(), CompletionError> {
+            if completion == I2cCompletion::NotPending {
+                return Err(CompletionError::NotPending);
+            }
+            if let Some(error) = self.completion_error.take() {
+                return Err(error);
+            }
             if completion == I2cCompletion::Complete
                 && let Some(read) = read
             {
                 read.copy_from_slice(&self.response[..read.len()]);
+                self.read_copies += 1;
             }
             self.completions += 1;
+            self.transaction = Transaction::None;
+            Ok(())
         }
 
-        fn cancel_transaction(&mut self) {
+        fn cancel_transaction(&mut self, now: Timestamp) {
             self.cancellations += 1;
+            self.cancelled_at = Some(now);
+            self.transaction = Transaction::None;
+        }
+
+        fn service(&mut self, _: Timestamp) -> Result<(), CompletionError> {
+            self.service_error.take().map_or(Ok(()), Err)
         }
     }
 
@@ -473,12 +538,18 @@ mod tests {
         let mut driver = LidarLiteV3::new(FakeBus::ready());
         assert!(!driver.is_ready());
         assert_eq!(
-            driver.on_transaction_complete(I2cCompletion::NotPending),
+            driver.on_transaction_complete(
+                I2cCompletion::NotPending,
+                Timestamp::from_microseconds(0)
+            ),
             LidarLiteV3Event::NotPending
         );
         assert_eq!(driver.bus.completions, 0);
         assert_eq!(
-            driver.on_transaction_complete(I2cCompletion::TransferFault),
+            driver.on_transaction_complete(
+                I2cCompletion::TransferFault,
+                Timestamp::from_microseconds(0)
+            ),
             LidarLiteV3Event::NotPending
         );
         assert_eq!(driver.bus.completions, 0);
@@ -514,7 +585,7 @@ mod tests {
             }
         );
         assert_eq!(
-            driver.on_transaction_complete(I2cCompletion::Complete),
+            driver.on_transaction_complete(I2cCompletion::Complete, timestamp),
             LidarLiteV3Event::MeasurementStarted
         );
         driver.poll_measurement(timestamp).unwrap();
@@ -528,7 +599,7 @@ mod tests {
         );
         driver.bus.response = [STATUS_HEALTHY, 0];
         assert_eq!(
-            driver.on_transaction_complete(I2cCompletion::Complete),
+            driver.on_transaction_complete(I2cCompletion::Complete, timestamp),
             LidarLiteV3Event::ReadingStatus
         );
         driver.poll_measurement(timestamp).unwrap();
@@ -540,7 +611,7 @@ mod tests {
             }
         );
         assert_eq!(
-            driver.on_transaction_complete(I2cCompletion::Complete),
+            driver.on_transaction_complete(I2cCompletion::Complete, timestamp),
             LidarLiteV3Event::SelectingDistanceRegister
         );
         driver.poll_measurement(timestamp).unwrap();
@@ -553,7 +624,7 @@ mod tests {
             }
         );
         assert_eq!(
-            driver.on_transaction_complete(I2cCompletion::Complete),
+            driver.on_transaction_complete(I2cCompletion::Complete, timestamp),
             LidarLiteV3Event::ReadingMeasurement
         );
         driver.bus.response = 291_u16.to_be_bytes();
@@ -566,7 +637,7 @@ mod tests {
             }
         );
         assert_eq!(
-            driver.on_transaction_complete(I2cCompletion::Complete),
+            driver.on_transaction_complete(I2cCompletion::Complete, timestamp),
             LidarLiteV3Event::Measurement(RangeSnapshot::new(timestamp, 2_910))
         );
         assert!(driver.is_ready());
@@ -581,17 +652,20 @@ mod tests {
                 AcquisitionMode::WithoutReceiverBiasCorrection,
             )
             .unwrap();
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         driver
             .poll_measurement(Timestamp::from_microseconds(0))
             .unwrap();
         driver.bus.response = [STATUS_HEALTHY | 1, 0];
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         driver
             .poll_measurement(Timestamp::from_microseconds(0))
             .unwrap();
         assert_eq!(
-            driver.on_transaction_complete(I2cCompletion::Complete),
+            driver
+                .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
             LidarLiteV3Event::MeasurementBusy
         );
         assert!(!driver.is_ready());
@@ -611,11 +685,12 @@ mod tests {
                 AcquisitionMode::WithReceiverBiasCorrection,
             )
             .unwrap();
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
 
         assert_eq!(
             driver.poll_measurement(Timestamp::from_microseconds(
-                MEASUREMENT_TIMEOUT_MICROSECONDS,
+                MEASUREMENT_TIMEOUT_MICROSECONDS + 1,
             )),
             Err(Error::MeasurementTimeout)
         );
@@ -632,17 +707,20 @@ mod tests {
                 AcquisitionMode::WithReceiverBiasCorrection,
             )
             .unwrap();
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         driver
             .poll_measurement(Timestamp::from_microseconds(0))
             .unwrap();
         driver.bus.response = [STATUS_HEALTHY | STATUS_PROCESS_ERROR, 0];
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         driver
             .poll_measurement(Timestamp::from_microseconds(0))
             .unwrap();
         assert_eq!(
-            driver.on_transaction_complete(I2cCompletion::Complete),
+            driver
+                .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
             LidarLiteV3Event::Fault(Error::ProcessError)
         );
 
@@ -652,17 +730,20 @@ mod tests {
                 AcquisitionMode::WithReceiverBiasCorrection,
             )
             .unwrap();
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         driver
             .poll_measurement(Timestamp::from_microseconds(0))
             .unwrap();
         driver.bus.response = [0, 0];
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         driver
             .poll_measurement(Timestamp::from_microseconds(0))
             .unwrap();
         assert_eq!(
-            driver.on_transaction_complete(I2cCompletion::Complete),
+            driver
+                .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
             LidarLiteV3Event::Fault(Error::Unhealthy)
         );
 
@@ -672,17 +753,20 @@ mod tests {
                 AcquisitionMode::WithReceiverBiasCorrection,
             )
             .unwrap();
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         driver
             .poll_measurement(Timestamp::from_microseconds(0))
             .unwrap();
         driver.bus.response = [STATUS_HEALTHY | STATUS_INVALID_SIGNAL, 0];
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         driver
             .poll_measurement(Timestamp::from_microseconds(0))
             .unwrap();
         assert_eq!(
-            driver.on_transaction_complete(I2cCompletion::Complete),
+            driver
+                .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
             LidarLiteV3Event::Fault(Error::InvalidSignal)
         );
     }
@@ -696,20 +780,25 @@ mod tests {
                 AcquisitionMode::WithReceiverBiasCorrection,
             )
             .unwrap();
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         driver
             .poll_measurement(Timestamp::from_microseconds(0))
             .unwrap();
         driver.bus.response = [STATUS_HEALTHY, 0];
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         driver.bus.response = 1_u16.to_be_bytes();
         let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
         assert_eq!(
-            driver.on_transaction_complete(I2cCompletion::Complete),
+            driver
+                .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
             LidarLiteV3Event::Fault(Error::NoSignal)
         );
 
@@ -719,20 +808,25 @@ mod tests {
                 AcquisitionMode::WithReceiverBiasCorrection,
             )
             .unwrap();
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         driver
             .poll_measurement(Timestamp::from_microseconds(0))
             .unwrap();
         driver.bus.response = [STATUS_HEALTHY, 0];
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         driver.bus.response = 0_u16.to_be_bytes();
         let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
         assert_eq!(
-            driver.on_transaction_complete(I2cCompletion::Complete),
+            driver
+                .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
             LidarLiteV3Event::Fault(Error::NoSignal)
         );
 
@@ -742,20 +836,25 @@ mod tests {
                 AcquisitionMode::WithReceiverBiasCorrection,
             )
             .unwrap();
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         driver
             .poll_measurement(Timestamp::from_microseconds(0))
             .unwrap();
         driver.bus.response = [STATUS_HEALTHY, 0];
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
-        let _ = driver.on_transaction_complete(I2cCompletion::Complete);
+        let _ = driver
+            .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0));
         driver.bus.response = u16::MAX.to_be_bytes();
         let _ = driver.poll_measurement(Timestamp::from_microseconds(0));
         assert_eq!(
-            driver.on_transaction_complete(I2cCompletion::Complete),
+            driver
+                .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
             LidarLiteV3Event::Measurement(RangeSnapshot::new(
                 Timestamp::from_microseconds(0),
                 655_350,
@@ -773,9 +872,147 @@ mod tests {
             )
             .unwrap();
         assert_eq!(
-            driver.on_transaction_complete(I2cCompletion::TransferFault),
+            driver.on_transaction_complete(
+                I2cCompletion::TransferFault,
+                Timestamp::from_microseconds(0)
+            ),
             LidarLiteV3Event::Fault(Error::TransferFault)
         );
         assert!(driver.is_ready());
     }
+
+    #[test]
+    fn cancellation_is_serviced_after_acquisition_timeout_and_stop_failure_latches_fault() {
+        let mut driver = initialized();
+        driver
+            .request_measurement(
+                Timestamp::from_microseconds(123),
+                AcquisitionMode::WithReceiverBiasCorrection,
+            )
+            .unwrap();
+        let timeout = Timestamp::from_microseconds(200_124);
+        assert_eq!(
+            driver.poll_measurement(timeout),
+            Err(Error::MeasurementTimeout)
+        );
+        assert_eq!(driver.bus.cancelled_at, Some(timeout));
+        driver.bus.service_error = Some(CompletionError::StopUnproven);
+        assert_eq!(
+            driver.request_measurement(
+                Timestamp::from_microseconds(210_123),
+                AcquisitionMode::WithReceiverBiasCorrection
+            ),
+            Err(Error::Completion(CompletionError::StopUnproven))
+        );
+        assert!(!driver.is_ready());
+        assert_eq!(
+            driver.on_transaction_complete(
+                I2cCompletion::Complete,
+                Timestamp::from_microseconds(210_123)
+            ),
+            LidarLiteV3Event::NotPending
+        );
+        assert_eq!(
+            driver.poll_measurement(Timestamp::from_microseconds(220_123)),
+            Err(Error::NotReady)
+        );
+        assert_eq!(driver.bus.cancellations, 1);
+    }
+
+    #[test]
+    fn unproven_completion_cannot_advance_a_phase_or_publish_stale_bytes() {
+        let mut driver = initialized();
+        driver
+            .request_measurement(
+                Timestamp::from_microseconds(0),
+                AcquisitionMode::WithReceiverBiasCorrection,
+            )
+            .unwrap();
+        driver.bus.completion_error = Some(CompletionError::NotPending);
+        assert_eq!(
+            driver
+                .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
+            LidarLiteV3Event::NotPending
+        );
+        assert_eq!(
+            driver.state,
+            super::State::Starting {
+                timestamp: Timestamp::from_microseconds(0)
+            }
+        );
+        driver.bus.completion_error = Some(CompletionError::StopUnproven);
+        assert_eq!(
+            driver
+                .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
+            LidarLiteV3Event::Fault(Error::Completion(CompletionError::StopUnproven))
+        );
+        assert!(!driver.is_ready());
+        assert_eq!(
+            driver
+                .on_transaction_complete(I2cCompletion::Complete, Timestamp::from_microseconds(0)),
+            LidarLiteV3Event::NotPending
+        );
+    }
+
+    #[test]
+    fn late_terminal_events_discard_every_active_phase_without_copying() {
+        for phase in 0..5 {
+            for completion in [I2cCompletion::Complete, I2cCompletion::TransferFault] {
+                let mut driver = initialized();
+                let start = Timestamp::from_microseconds(123);
+                driver
+                    .request_measurement(start, AcquisitionMode::WithReceiverBiasCorrection)
+                    .unwrap();
+                driver.bus.response = [STATUS_HEALTHY, 0];
+                for step in 0..phase {
+                    let now = 124 + step * 2;
+                    driver.on_transaction_complete(
+                        I2cCompletion::Complete,
+                        Timestamp::from_microseconds(now),
+                    );
+                    driver
+                        .poll_measurement(Timestamp::from_microseconds(now + 1))
+                        .unwrap();
+                }
+                let before = driver.state;
+                let copies = driver.bus.read_copies;
+                let completions = driver.bus.completions;
+                driver.receive = [0xa5; 2];
+                let late = Timestamp::from_microseconds(200_124);
+
+                // A nonterminal notification is not permission to cancel, even
+                // when the timer has been delayed past the acquisition limit.
+                assert_eq!(
+                    driver.on_transaction_complete(I2cCompletion::NotPending, late),
+                    LidarLiteV3Event::NotPending
+                );
+                assert_eq!(driver.state, before);
+                assert_eq!(driver.bus.cancellations, 0);
+                assert_eq!(driver.bus.completions, completions);
+                assert_eq!(driver.receive, [0xa5; 2]);
+
+                assert_eq!(
+                    driver.on_transaction_complete(completion, late),
+                    LidarLiteV3Event::Fault(Error::MeasurementTimeout)
+                );
+                assert_eq!(driver.bus.cancelled_at, Some(late));
+                assert_eq!(driver.bus.cancellations, 1);
+                assert_eq!(driver.bus.transaction, Transaction::None);
+                assert_eq!(driver.bus.read_copies, copies);
+                assert_eq!(driver.receive, [0xa5; 2]);
+                assert_eq!(
+                    driver.on_transaction_complete(completion, late),
+                    LidarLiteV3Event::NotPending
+                );
+
+                // Cancellation is still serviced if physical STOP was unproven.
+                driver.bus.service_error = Some(CompletionError::StopUnproven);
+                assert_eq!(
+                    driver.poll_measurement(Timestamp::from_microseconds(210_124)),
+                    Err(Error::Completion(CompletionError::StopUnproven))
+                );
+                assert!(!driver.is_ready());
+            }
+        }
+    }
 }

firmware/libs/kaptan-drivers/src/lidar_lite_v3/service.rs

@@ -3,16 +3,16 @@
 use super::{AcquisitionMode, Error, LidarLiteV3, LidarLiteV3Event, StartupError};
 use kaptan_i2c::{I2cBus, I2cCompletion};
 use kaptan_sensor::range::RangeSnapshot;
-use kaptan_time::MonotonicClock;
+use kaptan_time::{MonotonicClock, Timestamp};
 
 /// One observable result from a LIDAR-Lite v3 acquisition service.
 #[derive(Debug)]
 pub enum LidarLiteV3ServiceEvent<BusError> {
-    /// No observation or terminal fault was produced.
+    /// No observation or reportable error was produced.
     Idle,
     /// One coherent range observation is ready for publication.
     Measurement(RangeSnapshot),
-    /// The LIDAR-Lite v3 path entered a terminal fault state.
+    /// Acquisition or controller failed.
     Fault(Error<BusError>),
 }
 
@@ -55,22 +55,21 @@ impl<Clock: MonotonicClock, Bus: I2cBus> LidarLiteV3Service<Clock, Bus> {
         &mut self,
         completion: I2cCompletion,
     ) -> LidarLiteV3ServiceEvent<Bus::StartError> {
-        service_event(self.lidar.on_transaction_complete(completion))
+        let now = self.clock.now();
+        service_event(self.lidar.on_transaction_complete(completion, now))
     }
 
-    /// Acknowledges a board-controller interrupt and advances the acquisition.
-    ///
-    /// The closure keeps controller-specific interrupt handling at the board
-    /// boundary while preserving this service as the sole owner of LIDAR state.
+    /// Acknowledges a controller interrupt and advances the acquisition.
     pub fn on_controller_interrupt<F>(
         &mut self,
         acknowledge: F,
     ) -> LidarLiteV3ServiceEvent<Bus::StartError>
     where
-        F: FnOnce(&mut Bus) -> I2cCompletion,
+        F: FnOnce(&mut Bus, Timestamp) -> I2cCompletion,
     {
-        let completion = self.lidar.acknowledge_with(acknowledge);
-        self.on_transaction_complete(completion)
+        let now = self.clock.now();
+        let completion = self.lidar.acknowledge_with(|bus| acknowledge(bus, now));
+        service_event(self.lidar.on_transaction_complete(completion, now))
     }
 }
 
@@ -89,19 +88,22 @@ fn service_event<BusError>(event: LidarLiteV3Event<BusError>) -> LidarLiteV3Serv
 mod tests {
     use super::{LidarLiteV3Service, LidarLiteV3ServiceEvent, service_event};
     use crate::lidar_lite_v3::{AcquisitionMode, Error, LidarLiteV3, LidarLiteV3Event};
-    use kaptan_i2c::{I2cBus, I2cCompletion};
+    use kaptan_i2c::{CompletionError, I2cBus, I2cCompletion};
     use kaptan_time::{MonotonicClock, Timestamp};
 
-    struct Clock;
+    struct Clock(u64);
 
     impl MonotonicClock for Clock {
         fn now(&mut self) -> Timestamp {
-            Timestamp::from_microseconds(123)
+            Timestamp::from_microseconds(self.0)
         }
     }
 
+    #[derive(Default)]
     struct Bus {
         response: [u8; 2],
+        cancelled_at: Option<Timestamp>,
+        read_copies: usize,
     }
 
     impl I2cBus for Bus {
@@ -115,22 +117,37 @@ mod tests {
             Ok(())
         }
 
-        fn complete_transaction(&mut self, completion: I2cCompletion, read: Option<&mut [u8]>) {
+        fn complete_transaction(
+            &mut self,
+            completion: I2cCompletion,
+            read: Option<&mut [u8]>,
+        ) -> Result<(), CompletionError> {
+            if completion == I2cCompletion::NotPending {
+                return Err(CompletionError::NotPending);
+            }
             if completion == I2cCompletion::Complete
                 && let Some(read) = read
             {
                 read.copy_from_slice(&self.response[..read.len()]);
+                self.read_copies += 1;
             }
+            Ok(())
         }
 
-        fn cancel_transaction(&mut self) {}
+        fn cancel_transaction(&mut self, now: Timestamp) {
+            self.cancelled_at = Some(now);
+        }
+
+        fn service(&mut self, _: Timestamp) -> Result<(), CompletionError> {
+            Ok(())
+        }
     }
 
     #[test]
     fn produces_a_measurement_after_stop_separated_i2c_transactions() {
         let mut service = LidarLiteV3Service::new(
-            Clock,
-            LidarLiteV3::new(Bus { response: [0; 2] }),
+            Clock(123),
+            LidarLiteV3::new(Bus::default()),
             AcquisitionMode::WithReceiverBiasCorrection,
         );
         service.finish_startup().unwrap();
@@ -148,7 +165,7 @@ mod tests {
             LidarLiteV3ServiceEvent::Idle
         ));
         assert!(matches!(
-            service.on_controller_interrupt(|bus| {
+            service.on_controller_interrupt(|bus, _| {
                 bus.response = [1 << 5, 0];
                 I2cCompletion::Complete
             }),
@@ -159,7 +176,7 @@ mod tests {
             LidarLiteV3ServiceEvent::Idle
         ));
         assert!(matches!(
-            service.on_controller_interrupt(|_| I2cCompletion::Complete),
+            service.on_controller_interrupt(|_, _| I2cCompletion::Complete),
             LidarLiteV3ServiceEvent::Idle
         ));
         assert!(matches!(
@@ -167,7 +184,7 @@ mod tests {
             LidarLiteV3ServiceEvent::Idle
         ));
         assert!(matches!(
-            service.on_controller_interrupt(|bus| {
+            service.on_controller_interrupt(|bus, _| {
                 bus.response = [0x01, 0x23];
                 I2cCompletion::Complete
             }),
@@ -178,7 +195,7 @@ mod tests {
             LidarLiteV3ServiceEvent::Idle
         ));
         assert!(matches!(
-            service.on_controller_interrupt(|_| I2cCompletion::Complete),
+            service.on_controller_interrupt(|_, _| I2cCompletion::Complete),
             LidarLiteV3ServiceEvent::Measurement(sample)
                 if sample.timestamp == Timestamp::from_microseconds(123)
                     && sample.distance_millimetres == 2_910
@@ -200,4 +217,102 @@ mod tests {
             LidarLiteV3ServiceEvent::Fault(Error::Unhealthy)
         ));
     }
+
+    fn distance_pending() -> LidarLiteV3Service<Clock, Bus> {
+        let mut service = LidarLiteV3Service::new(
+            Clock(123),
+            LidarLiteV3::new(Bus::default()),
+            AcquisitionMode::WithReceiverBiasCorrection,
+        );
+        service.finish_startup().unwrap();
+        assert!(matches!(
+            service.on_periodic_tick(),
+            LidarLiteV3ServiceEvent::Idle
+        ));
+        for _ in 0..4 {
+            service.clock.0 += 1_000;
+            assert!(matches!(
+                service.on_controller_interrupt(|bus, _| {
+                    bus.response = [1 << 5, 0];
+                    I2cCompletion::Complete
+                }),
+                LidarLiteV3ServiceEvent::Idle
+            ));
+            service.clock.0 += 1_000;
+            assert!(matches!(
+                service.on_periodic_tick(),
+                LidarLiteV3ServiceEvent::Idle
+            ));
+        }
+        service.lidar.bus.response = [0x01, 0x23];
+        service
+    }
+
+    #[test]
+    fn completion_accepts_exact_acquisition_limit_and_discards_one_microsecond_late() {
+        for controller_irq in [false, true] {
+            for elapsed in [200_000, 200_001] {
+                let mut service = distance_pending();
+                service.clock.0 = 123 + elapsed;
+                let event = if controller_irq {
+                    service.on_controller_interrupt(|_, now| {
+                        assert_eq!(now, Timestamp::from_microseconds(123 + elapsed));
+                        I2cCompletion::Complete
+                    })
+                } else {
+                    service.on_transaction_complete(I2cCompletion::Complete)
+                };
+                if elapsed == 200_000 {
+                    assert!(matches!(event, LidarLiteV3ServiceEvent::Measurement(sample)
+                        if sample.timestamp == Timestamp::from_microseconds(123)
+                            && sample.distance_millimetres == 2_910));
+                    assert_eq!(service.lidar.bus.cancelled_at, None);
+                    assert_eq!(service.lidar.bus.read_copies, 2);
+                } else {
+                    assert!(matches!(
+                        event,
+                        LidarLiteV3ServiceEvent::Fault(Error::MeasurementTimeout)
+                    ));
+                    assert_eq!(
+                        service.lidar.bus.cancelled_at,
+                        Some(Timestamp::from_microseconds(200_124))
+                    );
+                    assert_eq!(service.lidar.bus.read_copies, 1);
+                }
+            }
+        }
+    }
+
+    #[test]
+    fn delayed_timer_and_final_irq_order_cannot_publish_an_expired_measurement() {
+        for irq_first in [true, false] {
+            let mut service = distance_pending();
+            // Model a flash blackout ending with both priority-11 I2C and
+            // priority-10 periodic service pending. The clock has advanced.
+            service.clock.0 = 250_123;
+            let irq = |service: &mut LidarLiteV3Service<Clock, Bus>| {
+                service.on_controller_interrupt(|_, _| I2cCompletion::Complete)
+            };
+            let first = if irq_first {
+                irq(&mut service)
+            } else {
+                service.on_periodic_tick()
+            };
+            assert!(matches!(
+                first,
+                LidarLiteV3ServiceEvent::Fault(Error::MeasurementTimeout)
+            ));
+            let second = if irq_first {
+                service.on_periodic_tick()
+            } else {
+                irq(&mut service)
+            };
+            assert!(matches!(second, LidarLiteV3ServiceEvent::Idle));
+            assert_eq!(
+                service.lidar.bus.cancelled_at,
+                Some(Timestamp::from_microseconds(250_123))
+            );
+            assert_eq!(service.lidar.bus.read_copies, 1);
+        }
+    }
 }

firmware/libs/kaptan-drivers/src/mpu6000/error.rs

@@ -18,4 +18,16 @@ pub enum Error<BusError> {
     /// The selected SPI route reported a terminal transfer fault.
     #[kaptan_error(id = 5004)]
     TransferFault,
+    /// The bus could not safely accept and copy out the completed transfer.
+    #[kaptan_error(id = 5005)]
+    Completion(kaptan_spi::CompletionError),
+    /// An accepted SPI transfer exceeded the 10 ms service deadline.
+    #[kaptan_error(id = 5006)]
+    TransferTimeout,
+    /// The bus stayed busy for the pending operation's 20 ms admission budget.
+    #[kaptan_error(id = 5007)]
+    AdmissionTimeout,
+    /// No acquisition was requested within two output periods plus 10 ms.
+    #[kaptan_error(id = 5008)]
+    DataReadyTimeout,
 }

firmware/libs/kaptan-drivers/src/mpu6000/mod.rs

@@ -6,8 +6,11 @@ mod registers;
 mod sample;
 pub mod service;
 
+#[cfg(test)]
+pub(crate) mod test_bus;
+
 use kaptan_sensor::imu::ImuSnapshot;
-use kaptan_spi::{SpiCompletion, SpiDevice};
+use kaptan_spi::{CompletionError, SpiCompletion, SpiDevice};
 use kaptan_time::Timestamp;
 
 pub use config::{AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000Config};
@@ -22,6 +25,21 @@ use registers::{
 };
 
 const TRANSFER_CAPACITY: usize = SAMPLE_BYTES + 1;
+// RM-MPU-6000A-00 rev 4.2, §4.28 p.41 (bundled datasheet.pdf):
+// DEVICE_RESET, 100 ms, SIGNAL_PATH_RESET=7, 100 ms.
+const RESET_SETTLE_US: u64 = 100_000;
+const SIGNAL_PATH_RESET: u8 = 0x68;
+// PS-MPU-6000A-00 rev 3.1 §6.3 p.14: register access needs up to 100 ms
+// after power-up. §6.1 p.12 lists 30 ms TYPICAL gyro start-up and §6.6 p.17
+// gives a 10 ms maximum PLL settling time. Use a conservative 100 ms wake
+// allowance, not a guaranteed physical settling bound.
+const WAKE_SETTLE_US: u64 = 100_000;
+// Host service budgets, not sensor timing specifications. A 15-byte burst on
+// the supported F722 SPI route takes less than 0.3 ms. Periodic service must
+// continue even if no DMA/DRDY interrupt arrives.
+const TRANSFER_TIMEOUT_US: u64 = 10_000;
+// Allows a retry on the F722's 10 ms periodic service before admission expires.
+const ADMISSION_TIMEOUT_US: u64 = 20_000;
 const CONFIGURATION: [(u8, u8); 9] = [
     (PWR_MGMT_1, CLOCK_SOURCE_PLL_X_GYRO),
     (USER_CTRL, I2C_INTERFACE_DISABLE),
@@ -37,18 +55,41 @@ const CONFIGURATION: [(u8, u8); 9] = [
 #[derive(Clone, Copy, Debug, Eq, PartialEq)]
 enum State {
     Dormant,
-    Resetting,
-    WaitingForReset,
-    ReadingIdentity,
-    WritingConfiguration(usize),
-    Ready,
-    ReadingSample { timestamp: Timestamp },
+    Pending {
+        operation: Operation,
+        since: Timestamp,
+    },
+    Active {
+        operation: Operation,
+        since: Timestamp,
+    },
+    Waiting {
+        next: Operation,
+        since: Timestamp,
+        delay_us: u64,
+    },
+    Settling {
+        since: Timestamp,
+    },
+    Ready {
+        since: Timestamp,
+    },
+    Faulted,
 }
 
-/// Observable outcome of one SPI terminal event.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+enum Operation {
+    Reset,
+    ResetSignalPaths,
+    Identity,
+    Configure(usize),
+    Sample,
+}
+
+/// Observable outcome of one timer or SPI terminal event.
 #[derive(Debug, Eq, PartialEq)]
 pub enum Mpu6000Event<BusError> {
-    /// The selected SPI interrupt had no terminal transfer event.
+    /// No sample, initialization completion or terminal fault was produced.
     NotPending,
     /// One initialization transfer completed; initialization continues.
     Initializing,
@@ -62,8 +103,8 @@ pub enum Mpu6000Event<BusError> {
 
 /// Configured MPU6000 driver.
 ///
-/// The driver owns fixed transfer buffers and advances only from external
-/// interrupt events. It never waits for SPI status flags.
+/// The driver owns ordinary movable staging buffers and advances only from
+/// external events. The bus copies these bytes; hardware never addresses them.
 pub struct Mpu6000<Bus> {
     bus: Bus,
     config: Mpu6000Config,
@@ -86,109 +127,144 @@ impl<Bus> Mpu6000<Bus> {
 }
 
 impl<Bus: SpiDevice> Mpu6000<Bus> {
-    /// Starts a device reset without waiting for SPI.
-    ///
-    /// The caller must subsequently call [`Self::advance_initialization`] only
-    /// after the platform-selected reset-settling interval has elapsed.
-    pub fn begin_initialization(&mut self) -> Result<(), Error<Bus::StartError>> {
+    /// Accepts initialization once. Busy admission is retried by periodic
+    /// service. All supplied instants must use one non-regressing boot clock.
+    /// Power must be stable by this call. Register access waits another 100 ms.
+    pub fn begin_initialization(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
         if self.state != State::Dormant {
             return Err(Error::NotReady);
         }
-
-        self.prepare_write(PWR_MGMT_1, DEVICE_RESET);
-        self.submit(2)?;
-        self.state = State::Resetting;
+        self.wait(Operation::Reset, now, RESET_SETTLE_US);
         Ok(())
     }
 
-    /// Starts identity verification after the required reset-settling interval.
-    ///
-    /// This operation only submits the next asynchronous SPI transfer; it
-    /// never waits for a peripheral or sensor status flag.
-    pub fn advance_initialization(&mut self) -> Result<(), Error<Bus::StartError>> {
-        if self.state != State::WaitingForReset {
-            return Ok(());
+    /// Services elapsed settling, admission and acquisition deadlines. This must
+    /// be called periodically throughout the driver's lifetime, including while
+    /// a transfer is active. It never starts an unsolicited sample read.
+    pub fn on_periodic_tick(&mut self, now: Timestamp) -> Mpu6000Event<Bus::StartError> {
+        if let Some(error) = self.check_deadline(now) {
+            return Mpu6000Event::Fault(error);
         }
+        match self.state {
+            State::Waiting {
+                next,
+                since,
+                delay_us,
+            } if elapsed(now, since, delay_us) => {
+                self.queue(next, now);
+            }
+            State::Settling { since } if elapsed(now, since, self.sample_period_us()) => {
+                self.state = State::Ready { since: now };
+                return Mpu6000Event::Initialized;
+            }
+            _ => {}
+        }
+        match self.submit_pending(now) {
+            Ok(()) => Mpu6000Event::NotPending,
+            Err(error) => Mpu6000Event::Fault(error),
+        }
+    }
 
-        self.prepare_read(WHO_AM_I, 2);
-        if let Err(error) = self.submit(2) {
-            self.state = State::Dormant;
+    /// Accepts a burst-read request. Busy admission remains pending for periodic
+    /// retry. The eventual accepted submission time becomes the snapshot
+    /// timestamp, not the ISR time or a physical sampling epoch.
+    pub fn request_sample(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
+        if let Some(error) = self.check_deadline(now) {
             return Err(error);
         }
-        self.state = State::ReadingIdentity;
-        Ok(())
-    }
-
-    /// Starts one fourteen-byte sensor read when initialization is complete.
-    pub fn request_sample(&mut self, timestamp: Timestamp) -> Result<(), Error<Bus::StartError>> {
-        if self.state != State::Ready {
+        if !self.is_ready() {
             return Err(Error::NotReady);
         }
-
-        self.prepare_read(ACCEL_XOUT_H, TRANSFER_CAPACITY);
-        self.submit(TRANSFER_CAPACITY)?;
-        self.state = State::ReadingSample { timestamp };
-        Ok(())
+        self.queue(Operation::Sample, now);
+        self.submit_pending(now)
     }
 
     /// Advances the state machine from an already-acknowledged DMA terminal event.
     pub fn on_transfer_complete(
         &mut self,
         completion: SpiCompletion,
+        now: Timestamp,
     ) -> Mpu6000Event<Bus::StartError> {
-        self.bus.complete_transfer(completion);
-        match completion {
-            SpiCompletion::NotPending => Mpu6000Event::NotPending,
-            SpiCompletion::TransferFault => {
-                self.state = State::Dormant;
+        if completion == SpiCompletion::NotPending {
+            return Mpu6000Event::NotPending;
+        }
+        if let Some(error) = self.check_deadline(now) {
+            return Mpu6000Event::Fault(error);
+        }
+        let State::Active { operation, since } = self.state else {
+            return Mpu6000Event::NotPending;
+        };
+        let length = if operation == Operation::Sample {
+            TRANSFER_CAPACITY
+        } else {
+            2
+        };
+        match self
+            .bus
+            .complete_transfer(completion, &mut self.receive[..length])
+        {
+            Ok(()) => self.advance_after_completion(operation, since, now),
+            Err(CompletionError::NotPending) => Mpu6000Event::NotPending,
+            Err(CompletionError::TransferFault) => {
+                self.state = State::Faulted;
                 Mpu6000Event::Fault(Error::TransferFault)
             }
-            SpiCompletion::Complete => self.advance_after_completion(),
+            Err(error) => {
+                self.state = State::Faulted;
+                Mpu6000Event::Fault(Error::Completion(error))
+            }
         }
     }
 
     /// Returns whether an IMU data-ready event can start a sensor transfer.
     pub fn is_ready(&self) -> bool {
-        self.state == State::Ready
+        matches!(self.state, State::Ready { .. })
     }
 
-    fn advance_after_completion(&mut self) -> Mpu6000Event<Bus::StartError> {
-        match self.state {
-            State::Dormant => Mpu6000Event::NotPending,
-            State::Resetting => {
-                self.state = State::WaitingForReset;
-                Mpu6000Event::Initializing
-            }
-            State::WaitingForReset => Mpu6000Event::NotPending,
-            State::ReadingIdentity => {
+    fn advance_after_completion(
+        &mut self,
+        operation: Operation,
+        requested: Timestamp,
+        now: Timestamp,
+    ) -> Mpu6000Event<Bus::StartError> {
+        match operation {
+            Operation::Reset => self.wait(Operation::ResetSignalPaths, now, RESET_SETTLE_US),
+            Operation::ResetSignalPaths => self.wait(Operation::Identity, now, RESET_SETTLE_US),
+            Operation::Identity => {
                 let identity = self.receive[1];
                 if identity != EXPECTED_WHO_AM_I {
-                    self.state = State::Dormant;
+                    self.state = State::Faulted;
                     return Mpu6000Event::Fault(Error::UnexpectedIdentity(identity));
                 }
 
-                self.start_configuration(0)
+                self.queue(Operation::Configure(0), now);
             }
-            State::WritingConfiguration(index) => {
+            Operation::Configure(index) => {
                 let next = index + 1;
-                if next == CONFIGURATION.len() {
-                    self.state = State::Ready;
-                    Mpu6000Event::Initialized
+                if index == 0 {
+                    self.wait(Operation::Configure(next), now, WAKE_SETTLE_US);
+                } else if next == CONFIGURATION.len() {
+                    // Allow a complete output period after the last write before
+                    // accepting DRDY. RM §4.2 defines the configured sample rate.
+                    self.state = State::Settling { since: now };
                 } else {
-                    self.start_configuration(next)
+                    self.queue(Operation::Configure(next), now);
                 }
             }
-            State::Ready => Mpu6000Event::NotPending,
-            State::ReadingSample { timestamp } => {
+            Operation::Sample => {
                 let mut bytes = [0; SAMPLE_BYTES];
                 bytes.copy_from_slice(&self.receive[1..]);
-                self.state = State::Ready;
-                Mpu6000Event::Sample(sample::decode(&bytes, timestamp, self.config))
+                self.state = State::Ready { since: now };
+                return Mpu6000Event::Sample(sample::decode(&bytes, requested, self.config));
             }
         }
+        match self.submit_pending(now) {
+            Ok(()) => Mpu6000Event::Initializing,
+            Err(error) => Mpu6000Event::Fault(error),
+        }
     }
 
-    fn start_configuration(&mut self, index: usize) -> Mpu6000Event<Bus::StartError> {
+    fn prepare_configuration(&mut self, index: usize) {
         let (register, value) = CONFIGURATION[index];
         let value = match register {
             CONFIG => self.config.digital_low_pass_filter.register_value(),
@@ -198,16 +274,6 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
             _ => value,
         };
         self.prepare_write(register, value);
-        match self.submit(2) {
-            Ok(()) => {
-                self.state = State::WritingConfiguration(index);
-                Mpu6000Event::Initializing
-            }
-            Err(error) => {
-                self.state = State::Dormant;
-                Mpu6000Event::Fault(error)
-            }
-        }
     }
 
     fn prepare_read(&mut self, register: u8, length: usize) {
@@ -223,21 +289,106 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
         self.transmit[..2].copy_from_slice(&[register, value]);
     }
 
-    fn submit(&mut self, length: usize) -> Result<(), Error<Bus::StartError>> {
-        let (bus, transmit, receive) = (&mut self.bus, &self.transmit, &mut self.receive);
-        bus.start_transfer(&transmit[..length], &mut receive[..length])
-            .map_err(Error::Start)
+    fn queue(&mut self, operation: Operation, now: Timestamp) {
+        self.state = State::Pending {
+            operation,
+            since: now,
+        };
     }
+
+    fn wait(&mut self, next: Operation, now: Timestamp, delay_us: u64) {
+        self.state = State::Waiting {
+            next,
+            since: now,
+            delay_us,
+        };
+    }
+
+    fn submit_pending(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
+        let State::Pending { operation, .. } = self.state else {
+            return Ok(());
+        };
+        match operation {
+            Operation::Reset => self.prepare_write(PWR_MGMT_1, DEVICE_RESET),
+            Operation::ResetSignalPaths => self.prepare_write(SIGNAL_PATH_RESET, 0x07),
+            Operation::Identity => self.prepare_read(WHO_AM_I, 2),
+            Operation::Configure(index) => self.prepare_configuration(index),
+            Operation::Sample => self.prepare_read(ACCEL_XOUT_H, TRANSFER_CAPACITY),
+        }
+        let length = if operation == Operation::Sample {
+            TRANSFER_CAPACITY
+        } else {
+            2
+        };
+        match self.bus.start_transfer(&self.transmit[..length]) {
+            Ok(()) => {
+                self.state = State::Active {
+                    operation,
+                    since: now,
+                };
+                Ok(())
+            }
+            Err(error) if Bus::is_busy(&error) => Ok(()),
+            Err(error) => {
+                self.state = State::Faulted;
+                Err(Error::Start(error))
+            }
+        }
+    }
+
+    fn sample_period_us(&self) -> u64 {
+        let base = if self.config.digital_low_pass_filter == DigitalLowPassFilter::Bandwidth260Hz {
+            125
+        } else {
+            1_000
+        };
+        base * (u64::from(self.config.sample_rate_divider) + 1)
+    }
+
+    fn check_deadline(&mut self, now: Timestamp) -> Option<Error<Bus::StartError>> {
+        let error = match self.state {
+            State::Active { since, .. } if elapsed(now, since, TRANSFER_TIMEOUT_US) => {
+                // Terminal fault completion is also the bus cancellation contract.
+                // It proves quiescence or quarantines permanent DMA storage.
+                match self
+                    .bus
+                    .complete_transfer(SpiCompletion::TransferFault, &mut [])
+                {
+                    Err(CompletionError::TransferFault | CompletionError::NotActive) | Ok(()) => {
+                        Error::TransferTimeout
+                    }
+                    Err(error) => Error::Completion(error),
+                }
+            }
+            State::Pending { since, .. } if elapsed(now, since, ADMISSION_TIMEOUT_US) => {
+                Error::AdmissionTimeout
+            }
+            State::Ready { since }
+                if elapsed(
+                    now,
+                    since,
+                    2 * self.sample_period_us() + TRANSFER_TIMEOUT_US,
+                ) =>
+            {
+                Error::DataReadyTimeout
+            }
+            _ => return None,
+        };
+        self.state = State::Faulted;
+        Some(error)
+    }
+}
+
+fn elapsed(now: Timestamp, since: Timestamp, duration_us: u64) -> bool {
+    now.as_microseconds()
+        .checked_sub(since.as_microseconds())
+        .is_some_and(|elapsed| elapsed >= duration_us)
 }
 
 #[cfg(test)]
 mod tests {
-    use super::{
-        AccelerometerRange, DigitalLowPassFilter, Error, GyroscopeRange, Mpu6000, Mpu6000Config,
-        Mpu6000Event, TRANSFER_CAPACITY,
-    };
-    use kaptan_spi::{SpiCompletion, SpiDevice};
-    use kaptan_time::Timestamp;
+    use super::test_bus::{FakeBus, FakeError};
+    use super::*;
 
     const TEST_CONFIG: Mpu6000Config = Mpu6000Config {
         accelerometer: AccelerometerRange::PlusMinus4G,
@@ -246,79 +397,87 @@ mod tests {
         sample_rate_divider: 9,
     };
 
-    #[derive(Debug, Eq, PartialEq)]
-    enum FakeError {
-        Start,
+    fn t(us: u64) -> Timestamp {
+        Timestamp::from_microseconds(us + 100_000)
     }
 
-    struct FakeBus {
-        receive: [u8; TRANSFER_CAPACITY],
-        length: usize,
-        fail_start: bool,
+    fn start(driver: &mut Mpu6000<FakeBus>) {
+        driver
+            .begin_initialization(Timestamp::from_microseconds(0))
+            .unwrap();
+        driver.on_periodic_tick(Timestamp::from_microseconds(99_999));
+        assert_eq!(driver.bus.starts, 0);
+        driver.on_periodic_tick(t(0));
     }
 
-    impl FakeBus {
-        fn ready() -> Self {
-            Self {
-                receive: [0; TRANSFER_CAPACITY],
-                length: 0,
-                fail_start: false,
-            }
-        }
-    }
-
-    impl SpiDevice for FakeBus {
-        type StartError = FakeError;
-
-        fn start_transfer(&mut self, _: &[u8], receive: &mut [u8]) -> Result<(), Self::StartError> {
-            if self.fail_start {
-                return Err(FakeError::Start);
-            }
-
-            self.length = receive.len();
-            receive.copy_from_slice(&self.receive[..self.length]);
-            Ok(())
-        }
-
-        fn complete_transfer(&mut self, _: SpiCompletion) {}
-    }
-
-    #[test]
-    fn configures_a_verified_device_without_waiting() {
+    fn initialized() -> Mpu6000<FakeBus> {
         let mut bus = FakeBus::ready();
-        bus.receive[1] = 0x68;
+        bus.response[1] = 0x68;
         let mut driver = Mpu6000::new(bus, TEST_CONFIG);
-
-        driver.begin_initialization().unwrap();
-        assert_eq!(
-            driver.on_transfer_complete(SpiCompletion::Complete),
-            Mpu6000Event::Initializing
-        );
-        driver.advance_initialization().unwrap();
-        for _ in 0..9 {
-            assert_eq!(
-                driver.on_transfer_complete(SpiCompletion::Complete),
-                Mpu6000Event::Initializing
-            );
+        start(&mut driver);
+        driver.on_transfer_complete(SpiCompletion::Complete, t(100));
+        driver.on_periodic_tick(t(100_100));
+        assert_eq!(&driver.bus.transmitted[..2], &[0x68, 7]);
+        driver.on_transfer_complete(SpiCompletion::Complete, t(100_200));
+        driver.on_periodic_tick(t(200_200));
+        driver.on_transfer_complete(SpiCompletion::Complete, t(200_300));
+        driver.on_transfer_complete(SpiCompletion::Complete, t(200_400));
+        driver.on_periodic_tick(t(300_400));
+        for index in 1..9 {
+            driver.on_transfer_complete(SpiCompletion::Complete, t(300_400 + index * 100));
         }
         assert_eq!(
-            driver.on_transfer_complete(SpiCompletion::Complete),
+            driver.on_periodic_tick(t(311_200)),
             Mpu6000Event::Initialized
         );
         assert!(driver.is_ready());
+        driver
+    }
+
+    #[test]
+    fn reset_signal_reset_and_wake_wait_elapsed_time_from_completion() {
+        let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
+        driver.bus.response[1] = 0x68;
+        start(&mut driver);
+        driver.on_transfer_complete(SpiCompletion::Complete, t(100));
+        for now in [100, 100, 50_000, 100_099] {
+            driver.on_periodic_tick(t(now));
+            assert_eq!(driver.bus.starts, 1);
+            assert!(!driver.is_ready());
+        }
+        driver.on_periodic_tick(t(100_100));
+        assert_eq!(driver.bus.starts, 2);
+        driver.on_transfer_complete(SpiCompletion::Complete, t(100_200));
+        driver.on_periodic_tick(t(200_199));
+        assert_eq!(driver.bus.starts, 2);
+        driver.on_periodic_tick(t(200_200));
+        driver.on_transfer_complete(SpiCompletion::Complete, t(200_300));
+        driver.on_transfer_complete(SpiCompletion::Complete, t(200_400));
+        driver.on_periodic_tick(t(300_399));
+        assert_eq!(driver.bus.starts, 4);
+        driver.on_periodic_tick(t(300_400));
+        for index in 1..9 {
+            driver.on_transfer_complete(SpiCompletion::Complete, t(300_400 + index * 100));
+        }
+        driver.on_periodic_tick(t(311_199));
+        assert!(!driver.is_ready());
+        assert_eq!(driver.request_sample(t(311_199)), Err(Error::NotReady));
+        assert_eq!(
+            driver.on_periodic_tick(t(311_200)),
+            Mpu6000Event::Initialized
+        );
     }
 
     #[test]
     fn rejects_an_unexpected_identity() {
         let mut bus = FakeBus::ready();
-        bus.receive[1] = 0x70;
+        bus.response[1] = 0x70;
         let mut driver = Mpu6000::new(bus, TEST_CONFIG);
 
-        driver.begin_initialization().unwrap();
-        let _ = driver.on_transfer_complete(SpiCompletion::Complete);
-        driver.advance_initialization().unwrap();
+        driver.queue(Operation::Identity, t(0));
+        driver.submit_pending(t(0)).unwrap();
         assert!(matches!(
-            driver.on_transfer_complete(SpiCompletion::Complete),
+            driver.on_transfer_complete(SpiCompletion::Complete, t(100)),
             Mpu6000Event::Fault(Error::UnexpectedIdentity(0x70))
         ));
     }
@@ -330,22 +489,23 @@ mod tests {
             0x00, 0x00, // 36.53 C
             0x40, 0x10, 0xBF, 0xF0, 0x00, 0x00, // gyro: 500, -500, 0 dps
         ];
-        let mut bus = FakeBus::ready();
-        bus.receive[1] = 0x68;
-        let mut driver = Mpu6000::new(bus, TEST_CONFIG);
-
-        driver.begin_initialization().unwrap();
-        let _ = driver.on_transfer_complete(SpiCompletion::Complete);
-        driver.advance_initialization().unwrap();
-        for _ in 0..9 {
-            let _ = driver.on_transfer_complete(SpiCompletion::Complete);
-        }
-        let _ = driver.on_transfer_complete(SpiCompletion::Complete);
-        driver.bus.receive[1..].copy_from_slice(&burst);
-        driver
-            .request_sample(Timestamp::from_microseconds(42))
-            .unwrap();
-        let Mpu6000Event::Sample(sample) = driver.on_transfer_complete(SpiCompletion::Complete)
+        let mut driver = initialized();
+        driver.bus.response[1..].copy_from_slice(&burst);
+        driver.request_sample(t(312_000)).unwrap();
+        assert_eq!(driver.receive, [0; TRANSFER_CAPACITY]);
+        assert_eq!(
+            driver.on_transfer_complete(SpiCompletion::NotPending, t(312_100)),
+            Mpu6000Event::NotPending
+        );
+        assert_eq!(driver.receive, [0; TRANSFER_CAPACITY]);
+        // Overwrite staging bytes and move the whole driver while work is pending.
+        // The fake bus, like the DMA route, owns the pending response itself.
+        driver.transmit.fill(0xff);
+        driver.bus.response.fill(0xff);
+        let mut driver =
+            core::mem::replace(&mut driver, Mpu6000::new(FakeBus::ready(), TEST_CONFIG));
+        let Mpu6000Event::Sample(sample) =
+            driver.on_transfer_complete(SpiCompletion::Complete, t(312_200))
         else {
             panic!("sample transfer must complete");
         };
@@ -356,18 +516,184 @@ mod tests {
             [8_726, -8_726, 0]
         );
         assert_eq!(sample.temperature_centidegrees_celsius, 3_653);
+        assert_eq!(sample.timestamp, t(312_000));
+        assert_eq!(
+            driver.on_transfer_complete(SpiCompletion::Complete, t(312_300)),
+            Mpu6000Event::NotPending
+        );
     }
 
     #[test]
     fn reports_a_route_transfer_fault() {
         let mut bus = FakeBus::ready();
-        bus.receive[1] = 0x68;
+        bus.response[1] = 0x68;
         let mut driver = Mpu6000::new(bus, TEST_CONFIG);
 
-        driver.begin_initialization().unwrap();
+        start(&mut driver);
         assert!(matches!(
-            driver.on_transfer_complete(SpiCompletion::TransferFault),
+            driver.on_transfer_complete(SpiCompletion::TransferFault, t(100)),
             Mpu6000Event::Fault(Error::TransferFault)
         ));
     }
+
+    #[test]
+    fn completion_errors_never_republish_a_previous_sample() {
+        for (event, error) in [
+            (
+                SpiCompletion::Complete,
+                Some(CompletionError::InvalidLength),
+            ),
+            (SpiCompletion::Complete, Some(CompletionError::Quarantined)),
+            (SpiCompletion::Complete, Some(CompletionError::NotActive)),
+            (
+                SpiCompletion::Complete,
+                Some(CompletionError::TransferFault),
+            ),
+            (SpiCompletion::TransferFault, None),
+        ] {
+            let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
+            driver.state = State::Ready { since: t(0) };
+            driver.request_sample(t(1)).unwrap();
+            assert!(matches!(
+                driver.on_transfer_complete(SpiCompletion::Complete, t(1)),
+                Mpu6000Event::Sample(_)
+            ));
+            driver.request_sample(t(2)).unwrap();
+            driver.receive.fill(0xa5);
+            driver.bus.fail_completion = error;
+            assert!(matches!(
+                driver.on_transfer_complete(event, t(2)),
+                Mpu6000Event::Fault(_)
+            ));
+            assert_eq!(driver.receive, [0xa5; TRANSFER_CAPACITY]);
+            assert!(!driver.is_ready());
+            assert!(!matches!(
+                driver.on_transfer_complete(SpiCompletion::Complete, t(3)),
+                Mpu6000Event::Sample(_)
+            ));
+            if error == Some(CompletionError::Quarantined) {
+                assert_eq!(driver.begin_initialization(t(4)), Err(Error::NotReady));
+            }
+        }
+    }
+
+    #[test]
+    fn rejected_submission_cannot_complete_a_stale_sample() {
+        let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
+        driver.state = State::Ready { since: t(0) };
+        driver.receive.fill(0xa5);
+        driver.bus.fail_start = true;
+        assert_eq!(
+            driver.request_sample(t(1)),
+            Err(Error::Start(FakeError::Start))
+        );
+        assert_eq!(
+            driver.on_transfer_complete(SpiCompletion::Complete, t(2)),
+            Mpu6000Event::NotPending
+        );
+    }
+
+    #[test]
+    fn every_transfer_phase_times_out_and_cancels_once_without_rearming() {
+        let operations = [
+            Operation::Reset,
+            Operation::ResetSignalPaths,
+            Operation::Identity,
+            Operation::Sample,
+        ];
+        for operation in operations
+            .into_iter()
+            .chain((0..9).map(Operation::Configure))
+        {
+            let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
+            driver.queue(operation, t(100));
+            driver.submit_pending(t(100)).unwrap();
+            assert_eq!(driver.on_periodic_tick(t(10_099)), Mpu6000Event::NotPending);
+            assert_eq!(driver.bus.cancellations, 0);
+            assert_eq!(
+                driver.on_periodic_tick(t(10_100)),
+                Mpu6000Event::Fault(Error::TransferTimeout)
+            );
+            assert_eq!(driver.bus.cancellations, 1);
+            assert_eq!(
+                driver.on_transfer_complete(SpiCompletion::Complete, t(10_101)),
+                Mpu6000Event::NotPending
+            );
+            driver.on_periodic_tick(t(1_000_000));
+            assert_eq!(driver.bus.starts, 1);
+            assert_eq!(
+                driver.begin_initialization(t(1_000_000)),
+                Err(Error::NotReady)
+            );
+        }
+    }
+
+    #[test]
+    fn every_busy_phase_preserves_pending_operation_and_has_an_admission_deadline() {
+        for operation in [
+            Operation::Reset,
+            Operation::ResetSignalPaths,
+            Operation::Identity,
+            Operation::Sample,
+        ]
+        .into_iter()
+        .chain((0..9).map(Operation::Configure))
+        {
+            let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
+            driver.bus.busy = true;
+            driver.queue(operation, t(100));
+            driver.submit_pending(t(100)).unwrap();
+            assert_eq!(
+                driver.state,
+                State::Pending {
+                    operation,
+                    since: t(100)
+                }
+            );
+            driver.on_periodic_tick(t(200));
+            driver.bus.busy = false;
+            driver.on_periodic_tick(t(300));
+            assert_eq!(
+                driver.state,
+                State::Active {
+                    operation,
+                    since: t(300)
+                }
+            );
+            assert_eq!(driver.bus.starts, 1);
+        }
+        let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
+        driver.bus.busy = true;
+        start(&mut driver);
+        assert_eq!(
+            driver.on_periodic_tick(t(20_000)),
+            Mpu6000Event::Fault(Error::AdmissionTimeout)
+        );
+        assert_eq!(driver.bus.cancellations, 0);
+    }
+
+    #[test]
+    fn missed_drdy_and_late_completions_fault_instead_of_polling_or_publishing() {
+        let mut driver = initialized();
+        driver.on_periodic_tick(t(341_199));
+        assert_eq!(driver.bus.starts, 12);
+        assert_eq!(
+            driver.on_periodic_tick(t(341_200)),
+            Mpu6000Event::Fault(Error::DataReadyTimeout)
+        );
+        let mut driver = initialized();
+        driver.request_sample(t(312_000)).unwrap();
+        assert_eq!(
+            driver.on_transfer_complete(SpiCompletion::Complete, t(322_000)),
+            Mpu6000Event::Fault(Error::TransferTimeout)
+        );
+        let mut driver = initialized();
+        driver.request_sample(t(312_000)).unwrap();
+        driver.bus.fail_completion = Some(CompletionError::Quarantined);
+        assert_eq!(
+            driver.on_periodic_tick(t(322_000)),
+            Mpu6000Event::Fault(Error::Completion(CompletionError::Quarantined))
+        );
+        assert_eq!(driver.bus.start_transfer(&[1]), Err(FakeError::Quarantined));
+    }
 }

firmware/libs/kaptan-drivers/src/mpu6000/sample.rs

@@ -1,4 +1,10 @@
 //! MPU6000 burst-sample decoding.
+//!
+//! Integer division rounds toward zero before adding temperature's offset.
+//! For the full signed raw domain, acceleration stays in -16,000..=15,999 mg
+//! across all ranges and temperature in -5,984..=13,290 cdegC. Gyro conversion
+//! uses 0.017453 rad/degree and reaches -34,871..=34,870 mrad/s at ±2000°/s.
+//! Its largest intermediate magnitude is 571,899,904, which fits i32.
 
 use kaptan_sensor::imu::ImuSnapshot;
 use kaptan_time::Timestamp;
@@ -34,10 +40,98 @@ const fn scale_acceleration(raw: i16, config: Mpu6000Config) -> i16 {
     ((raw as i32 * 1_000) / config.accelerometer.counts_per_g()) as i16
 }
 
-const fn scale_angular_rate(raw: i16, config: Mpu6000Config) -> i16 {
-    ((raw as i32 * 17_453) / config.gyroscope.milliradians_per_second_divisor()) as i16
+const fn scale_angular_rate(raw: i16, config: Mpu6000Config) -> i32 {
+    (raw as i32 * 17_453) / config.gyroscope.milliradians_per_second_divisor()
 }
 
 const fn scale_temperature(raw: i16) -> i16 {
     ((raw as i32 * 100) / 340 + 3_653) as i16
 }
+
+#[cfg(test)]
+mod tests {
+    use super::{decode, scale_temperature};
+    use crate::mpu6000::config::{
+        AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000Config,
+    };
+    use kaptan_time::Timestamp;
+
+    #[test]
+    fn signed_raw_endpoints_decode_in_every_full_scale_configuration() {
+        // Known answers from the datasheet sensitivities and the documented
+        // 0.017453 rad/degree conversion, truncated toward zero.
+        let accelerometers = [
+            (AccelerometerRange::PlusMinus2G, [-2_000, 0, 1_999]),
+            (AccelerometerRange::PlusMinus4G, [-4_000, 0, 3_999]),
+            (AccelerometerRange::PlusMinus8G, [-8_000, 0, 7_999]),
+            (AccelerometerRange::PlusMinus16G, [-16_000, 0, 15_999]),
+        ];
+        let gyroscopes = [
+            (
+                GyroscopeRange::PlusMinus250DegreesPerSecond,
+                [-4_365, 0, 4_365],
+            ),
+            (
+                GyroscopeRange::PlusMinus500DegreesPerSecond,
+                [-8_731, 0, 8_731],
+            ),
+            (
+                GyroscopeRange::PlusMinus1000DegreesPerSecond,
+                [-17_435, 0, 17_435],
+            ),
+            (
+                GyroscopeRange::PlusMinus2000DegreesPerSecond,
+                [-34_871, 0, 34_870],
+            ),
+        ];
+        let timestamp = Timestamp::from_microseconds(123);
+        for (accelerometer, acceleration) in accelerometers {
+            for (gyroscope, angular_rate) in gyroscopes {
+                for (temperature_raw, temperature) in [(i16::MIN, -5_984), (i16::MAX, 13_290)] {
+                    let mut bytes = [0; 14];
+                    for (index, raw) in [
+                        i16::MIN,
+                        0,
+                        i16::MAX,
+                        temperature_raw,
+                        i16::MIN,
+                        0,
+                        i16::MAX,
+                    ]
+                    .into_iter()
+                    .enumerate()
+                    {
+                        bytes[index * 2..index * 2 + 2].copy_from_slice(&raw.to_be_bytes());
+                    }
+                    let sample = decode(
+                        &bytes,
+                        timestamp,
+                        Mpu6000Config {
+                            accelerometer,
+                            gyroscope,
+                            digital_low_pass_filter: DigitalLowPassFilter::Bandwidth184Hz,
+                            sample_rate_divider: 0,
+                        },
+                    );
+                    assert_eq!(sample.timestamp, timestamp);
+                    assert_eq!(sample.acceleration_milligravity, acceleration);
+                    assert_eq!(sample.angular_rate_milliradians_per_second, angular_rate);
+                    assert_eq!(sample.temperature_centidegrees_celsius, temperature);
+                }
+            }
+        }
+    }
+
+    #[test]
+    fn temperature_rounds_toward_zero_before_adding_offset() {
+        for (raw, expected) in [
+            (-340, 3_553),
+            (-1, 3_653),
+            (0, 3_653),
+            (1, 3_653),
+            (340, 3_753),
+        ] {
+            assert_eq!(scale_temperature(raw), expected);
+        }
+    }
+}

firmware/libs/kaptan-drivers/src/mpu6000/service.rs

@@ -39,28 +39,18 @@ where
     Clock: MonotonicClock,
     Bus: SpiDevice,
 {
-    /// Submits the asynchronous sensor reset transaction.
+    /// Accepts initialization and starts the power-on register-access delay.
     pub fn begin(&mut self) -> Result<(), Error<Bus::StartError>> {
-        self.imu.begin_initialization()
+        self.imu.begin_initialization(self.clock.now())
     }
 
-    /// Advances the reset-settling state on a board periodic event.
+    /// Services settling and missing-IRQ deadlines even with an active transfer.
     pub fn on_periodic_tick(&mut self) -> Mpu6000ServiceEvent<Bus::StartError> {
-        if self.imu.is_ready() {
-            return match self.imu.request_sample(self.clock.now()) {
-                Ok(()) | Err(Error::NotReady) => Mpu6000ServiceEvent::Idle,
-                Err(error) => Mpu6000ServiceEvent::Fault(error),
-            };
-        }
-
-        let _ = self.clock.now();
-        match self.imu.advance_initialization() {
-            Ok(()) => Mpu6000ServiceEvent::Idle,
-            Err(error) => Mpu6000ServiceEvent::Fault(error),
-        }
+        service_event(self.imu.on_periodic_tick(self.clock.now()))
     }
 
-    /// Handles one coalesced data-ready event.
+    /// Handles one coalesced data-ready notification. Time is captured here in
+    /// the worker, at read submission, not at IRQ entry or physical sampling.
     pub fn on_data_ready(&mut self) -> Mpu6000ServiceEvent<Bus::StartError> {
         if !self.imu.is_ready() {
             return Mpu6000ServiceEvent::Idle;
@@ -77,22 +67,27 @@ where
         &mut self,
         completion: SpiCompletion,
     ) -> Mpu6000ServiceEvent<Bus::StartError> {
-        match self.imu.on_transfer_complete(completion) {
-            Mpu6000Event::NotPending | Mpu6000Event::Initializing => Mpu6000ServiceEvent::Idle,
-            Mpu6000Event::Initialized => Mpu6000ServiceEvent::Initialized,
-            Mpu6000Event::Sample(sample) => Mpu6000ServiceEvent::Sample(sample),
-            Mpu6000Event::Fault(error) => Mpu6000ServiceEvent::Fault(error),
-        }
+        service_event(self.imu.on_transfer_complete(completion, self.clock.now()))
+    }
+}
+
+fn service_event<E>(event: Mpu6000Event<E>) -> Mpu6000ServiceEvent<E> {
+    match event {
+        Mpu6000Event::NotPending | Mpu6000Event::Initializing => Mpu6000ServiceEvent::Idle,
+        Mpu6000Event::Initialized => Mpu6000ServiceEvent::Initialized,
+        Mpu6000Event::Sample(sample) => Mpu6000ServiceEvent::Sample(sample),
+        Mpu6000Event::Fault(error) => Mpu6000ServiceEvent::Fault(error),
     }
 }
 
 #[cfg(test)]
 mod tests {
     use super::{Mpu6000Service, Mpu6000ServiceEvent};
+    use crate::mpu6000::test_bus::FakeBus;
     use crate::mpu6000::{
         AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000, Mpu6000Config,
     };
-    use kaptan_spi::{SpiCompletion, SpiDevice};
+    use kaptan_spi::SpiCompletion;
     use kaptan_time::{MonotonicClock, Timestamp};
 
     const TEST_CONFIG: Mpu6000Config = Mpu6000Config {
@@ -102,62 +97,80 @@ mod tests {
         sample_rate_divider: 9,
     };
 
-    struct Clock;
+    struct Clock(u64);
 
     impl MonotonicClock for Clock {
         fn now(&mut self) -> Timestamp {
-            Timestamp::from_microseconds(42)
+            Timestamp::from_microseconds(self.0)
         }
     }
 
-    struct Bus {
-        response: [u8; 15],
-    }
-
-    impl SpiDevice for Bus {
-        type StartError = ();
-
-        fn start_transfer(&mut self, _: &[u8], receive: &mut [u8]) -> Result<(), Self::StartError> {
-            receive.copy_from_slice(&self.response[..receive.len()]);
-            Ok(())
-        }
-
-        fn complete_transfer(&mut self, _: SpiCompletion) {}
-    }
-
     #[test]
     fn accepts_a_sample_only_after_initialization() {
-        let mut response = [0; 15];
-        response[1] = 0x68;
-        let mut service = Mpu6000Service::new(Clock, Mpu6000::new(Bus { response }, TEST_CONFIG));
+        let mut bus = FakeBus::ready();
+        bus.response[1] = 0x68;
+        let mut service = Mpu6000Service::new(Clock(0), Mpu6000::new(bus, TEST_CONFIG));
 
         service.begin().unwrap();
         assert!(matches!(service.on_data_ready(), Mpu6000ServiceEvent::Idle));
+        service.clock.0 = 99_999;
+        service.on_periodic_tick();
+        assert_eq!(service.imu.bus.starts, 0);
+        service.clock.0 = 100_000;
+        service.on_periodic_tick();
         assert!(matches!(
             service.on_transfer_complete(SpiCompletion::Complete),
             Mpu6000ServiceEvent::Idle
         ));
-        assert!(matches!(
-            service.on_periodic_tick(),
-            Mpu6000ServiceEvent::Idle
-        ));
-        for _ in 0..9 {
+        service.clock.0 = 200_000;
+        service.on_periodic_tick();
+        service.on_transfer_complete(SpiCompletion::Complete);
+        service.clock.0 = 300_000;
+        service.on_periodic_tick();
+        service.on_transfer_complete(SpiCompletion::Complete);
+        service.on_transfer_complete(SpiCompletion::Complete);
+        service.clock.0 = 400_000;
+        service.on_periodic_tick();
+        for _ in 1..9 {
             assert!(matches!(
                 service.on_transfer_complete(SpiCompletion::Complete),
                 Mpu6000ServiceEvent::Idle
             ));
         }
+        service.clock.0 = 410_000;
         assert!(matches!(
-            service.on_transfer_complete(SpiCompletion::Complete),
+            service.on_periodic_tick(),
             Mpu6000ServiceEvent::Initialized
         ));
+        service.clock.0 = 410_042;
+        assert!(matches!(service.on_data_ready(), Mpu6000ServiceEvent::Idle));
+        service.clock.0 = 410_123;
+        assert!(matches!(
+            service.on_transfer_complete(SpiCompletion::Complete),
+            Mpu6000ServiceEvent::Sample(sample) if sample.timestamp == Timestamp::from_microseconds(410_042)
+        ));
+    }
+
+    #[test]
+    fn fake_clock_expires_active_work_without_any_irq() {
+        let mut service =
+            Mpu6000Service::new(Clock(50), Mpu6000::new(FakeBus::ready(), TEST_CONFIG));
+        service.begin().unwrap();
+        service.clock.0 = 100_050;
+        service.on_periodic_tick();
+        service.clock.0 = 110_049;
         assert!(matches!(
             service.on_periodic_tick(),
             Mpu6000ServiceEvent::Idle
         ));
+        service.clock.0 = 110_050;
+        assert!(matches!(
+            service.on_periodic_tick(),
+            Mpu6000ServiceEvent::Fault(super::Error::TransferTimeout)
+        ));
         assert!(matches!(
             service.on_transfer_complete(SpiCompletion::Complete),
-            Mpu6000ServiceEvent::Sample(_)
+            Mpu6000ServiceEvent::Idle
         ));
     }
 }

firmware/libs/kaptan-drivers/src/mpu6000/test_bus.rs

@@ -0,0 +1,97 @@
+//! Test transport with owned pending bytes, never retained caller pointers.
+
+use kaptan_spi::{CompletionError, SpiCompletion, SpiDevice};
+
+#[derive(Debug, Eq, PartialEq)]
+pub(crate) enum FakeError {
+    Start,
+    Busy,
+    InvalidLength,
+    Quarantined,
+}
+
+pub(crate) struct FakeBus {
+    pub response: [u8; 15],
+    pub fail_start: bool,
+    pub fail_completion: Option<CompletionError>,
+    pub busy: bool,
+    pub starts: usize,
+    pub cancellations: usize,
+    pub transmitted: [u8; 15],
+    pending: Option<([u8; 15], usize)>,
+    quarantined: bool,
+}
+
+impl FakeBus {
+    pub fn ready() -> Self {
+        Self {
+            response: [0; 15],
+            fail_start: false,
+            fail_completion: None,
+            busy: false,
+            starts: 0,
+            cancellations: 0,
+            transmitted: [0; 15],
+            pending: None,
+            quarantined: false,
+        }
+    }
+}
+
+impl SpiDevice for FakeBus {
+    type StartError = FakeError;
+
+    fn is_busy(error: &Self::StartError) -> bool {
+        *error == FakeError::Busy
+    }
+
+    fn start_transfer(&mut self, transmit: &[u8]) -> Result<(), Self::StartError> {
+        if self.quarantined {
+            return Err(FakeError::Quarantined);
+        }
+        if self.pending.is_some() || self.busy {
+            return Err(FakeError::Busy);
+        }
+        if self.fail_start {
+            return Err(FakeError::Start);
+        }
+        if transmit.is_empty() || transmit.len() > self.response.len() {
+            return Err(FakeError::InvalidLength);
+        }
+        // Simulated hardware computes its response at submission, but only the
+        // accepted completion is allowed to copy those owned bytes to a caller.
+        self.pending = Some((self.response, transmit.len()));
+        self.transmitted[..transmit.len()].copy_from_slice(transmit);
+        self.starts += 1;
+        Ok(())
+    }
+
+    fn complete_transfer(
+        &mut self,
+        completion: SpiCompletion,
+        receive: &mut [u8],
+    ) -> Result<(), CompletionError> {
+        if completion == SpiCompletion::NotPending {
+            return Err(CompletionError::NotPending);
+        }
+        if self.quarantined {
+            return Err(CompletionError::Quarantined);
+        }
+        if completion == SpiCompletion::TransferFault {
+            self.cancellations += 1;
+        }
+        let (pending, length) = self.pending.take().ok_or(CompletionError::NotActive)?;
+        if let Some(error) = self.fail_completion.take() {
+            self.quarantined = error == CompletionError::Quarantined;
+            return Err(error);
+        }
+        if completion == SpiCompletion::TransferFault {
+            return Err(CompletionError::TransferFault);
+        }
+        if receive.len() != length {
+            return Err(CompletionError::InvalidLength);
+        }
+        receive.copy_from_slice(&pending[..length]);
+        Ok(())
+    }
+}

firmware/libs/kaptan-drivers/src/mtf01/mod.rs

@@ -1,14 +1,7 @@
 //! MicoAir MTF-01 Microlink UART frame decoder.
-//!
-//! Device documentation belongs beside this module.
-//!
-//! The MTF-01 continuously transmits fixed-size Microlink range-sensor frames.
-//! Feed each received UART byte to [`mtf01::Mtf01::receive_byte`]. The parser is
-//! allocation-free, does not use packed structs, and resynchronizes on a new
-//! frame header after malformed input.
 
 use kaptan_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot};
-use kaptan_time::{MonotonicClock, Timestamp};
+use kaptan_time::Timestamp;
 
 const FRAME_HEADER: u8 = 0xEF;
 const RANGE_SENSOR_MESSAGE_ID: u8 = 0x51;
@@ -26,11 +19,7 @@ enum State {
     Checksum,
 }
 
-/// One complete MTF-01 range and optical-flow report.
-///
-/// This preserves the MTF-01's device-specific fields. Use
-/// [`Self::range_snapshot`] or [`Self::optical_flow_snapshot`] to obtain a
-/// device-independent observation for the rest of the flight stack.
+/// MTF-01 range and optical-flow report in device units.
 #[derive(Clone, Copy, Debug, Eq, PartialEq)]
 pub struct Mtf01Report {
     /// Flight-controller monotonic time at frame reception.
@@ -58,34 +47,6 @@ pub struct Mtf01Report {
 }
 
 impl Mtf01Report {
-    /// Creates an MTF-01 observation from one validated Microlink frame.
-    #[allow(clippy::too_many_arguments)]
-    pub const fn new(
-        timestamp: Timestamp,
-        sensor_time_milliseconds: u32,
-        distance_millimetres: u32,
-        distance_strength: u8,
-        distance_precision: u8,
-        distance_valid: bool,
-        flow_velocity_x_centimetres_per_second_at_one_metre: i16,
-        flow_velocity_y_centimetres_per_second_at_one_metre: i16,
-        flow_quality: u8,
-        flow_valid: bool,
-    ) -> Self {
-        Self {
-            timestamp,
-            sensor_time_milliseconds,
-            distance_millimetres,
-            distance_strength,
-            distance_precision,
-            distance_valid,
-            flow_velocity_x_centimetres_per_second_at_one_metre,
-            flow_velocity_y_centimetres_per_second_at_one_metre,
-            flow_quality,
-            flow_valid,
-        }
-    }
-
     /// Returns the valid range portion of this report.
     pub const fn range_snapshot(self) -> Option<RangeSnapshot> {
         if self.distance_valid && self.distance_millimetres != 0 {
@@ -129,13 +90,10 @@ impl Mtf01Report {
     }
 }
 
-/// MTF-01 Microlink decoder.
+/// Microlink decoder for one UART receive stream.
 ///
-/// One decoder is owned by one UART receive path. It accepts the MTF-01's
-/// documented range-sensor message ID and 20-byte payload length. Device and
-/// system IDs are deliberately accepted as transmitted, matching MicoAir's
-/// reference decoder and allowing sensor IDs configured outside factory
-/// defaults.
+/// Accepts message ID 0x51 with a 20-byte payload. Device and system IDs are
+/// unrestricted, matching MicoAir's reference decoder.
 pub struct Mtf01 {
     state: State,
     checksum: u8,
@@ -155,9 +113,8 @@ impl Mtf01 {
     /// Accepts one UART byte and returns a complete, checksum-validated MTF-01
     /// report when available.
     ///
-    /// `timestamp` should be the controller monotonic time at receipt of this
-    /// byte. It is used only for a completed frame, so it records the arrival
-    /// time of the report rather than an arbitrary intermediate byte.
+    /// `timestamp` is the controller's monotonic time at byte receipt.
+    /// The report retains the checksum byte's timestamp.
     pub fn receive_byte(&mut self, timestamp: Timestamp, byte: u8) -> Option<Mtf01Report> {
         match self.state {
             State::Header => {
@@ -198,10 +155,7 @@ impl Mtf01 {
             }
             State::Checksum => {
                 let valid = byte == self.checksum;
-                // The checksum terminates the frame even when its value is
-                // also the Microlink header. The reference parser returns to
-                // its initial state here rather than treating checksum data as
-                // the start of the next frame.
+                // A checksum of 0xEF ends the frame and is not a new header.
                 self.reset();
                 if valid {
                     return Some(self.snapshot(timestamp));
@@ -233,18 +187,22 @@ impl Mtf01 {
     }
 
     fn snapshot(&self, timestamp: Timestamp) -> Mtf01Report {
-        Mtf01Report::new(
+        Mtf01Report {
             timestamp,
-            u32::from_le_bytes(self.payload[0..4].try_into().unwrap()),
-            u32::from_le_bytes(self.payload[4..8].try_into().unwrap()),
-            self.payload[8],
-            self.payload[9],
-            self.payload[10] == 1,
-            i16::from_le_bytes(self.payload[12..14].try_into().unwrap()),
-            i16::from_le_bytes(self.payload[14..16].try_into().unwrap()),
-            self.payload[16],
-            self.payload[17] == 1,
-        )
+            sensor_time_milliseconds: u32::from_le_bytes(self.payload[0..4].try_into().unwrap()),
+            distance_millimetres: u32::from_le_bytes(self.payload[4..8].try_into().unwrap()),
+            distance_strength: self.payload[8],
+            distance_precision: self.payload[9],
+            distance_valid: self.payload[10] == 1,
+            flow_velocity_x_centimetres_per_second_at_one_metre: i16::from_le_bytes(
+                self.payload[12..14].try_into().unwrap(),
+            ),
+            flow_velocity_y_centimetres_per_second_at_one_metre: i16::from_le_bytes(
+                self.payload[14..16].try_into().unwrap(),
+            ),
+            flow_quality: self.payload[16],
+            flow_valid: self.payload[17] == 1,
+        }
     }
 }
 
@@ -254,42 +212,10 @@ impl Default for Mtf01 {
     }
 }
 
-/// Timestamping MTF-01 UART receive service.
-///
-/// Feed this service from the selected UART receive interrupt or DMA consumer.
-/// The board retains ownership of pin muxing, buffering, and interrupt
-/// acknowledgement.
-pub struct Mtf01Service<Clock> {
-    clock: Clock,
-    decoder: Mtf01,
-}
-
-impl<Clock> Mtf01Service<Clock> {
-    /// Creates an MTF-01 receive service awaiting a Microlink frame header.
-    pub const fn new(clock: Clock) -> Self {
-        Self {
-            clock,
-            decoder: Mtf01::new(),
-        }
-    }
-
-    /// Discards a partial frame after a UART transport fault.
-    pub fn reset(&mut self) {
-        self.decoder.reset();
-    }
-}
-
-impl<Clock: MonotonicClock> Mtf01Service<Clock> {
-    /// Consumes one UART byte and returns one validated MTF-01 report.
-    pub fn on_received_byte(&mut self, byte: u8) -> Option<Mtf01Report> {
-        self.decoder.receive_byte(self.clock.now(), byte)
-    }
-}
-
 #[cfg(test)]
 mod tests {
-    use super::{Mtf01, Mtf01Report, Mtf01Service};
-    use kaptan_time::{MonotonicClock, Timestamp};
+    use super::{Mtf01, Mtf01Report};
+    use kaptan_time::Timestamp;
 
     fn frame(payload: [u8; 20]) -> [u8; 27] {
         let mut frame = [0_u8; 27];
@@ -323,18 +249,18 @@ mod tests {
 
         assert_eq!(
             report,
-            Some(Mtf01Report::new(
+            Some(Mtf01Report {
                 timestamp,
-                0x1234_5678,
-                1_234,
-                200,
-                7,
-                true,
-                -1_234,
-                1_234,
-                250,
-                true,
-            ))
+                sensor_time_milliseconds: 0x1234_5678,
+                distance_millimetres: 1_234,
+                distance_strength: 200,
+                distance_precision: 7,
+                distance_valid: true,
+                flow_velocity_x_centimetres_per_second_at_one_metre: -1_234,
+                flow_velocity_y_centimetres_per_second_at_one_metre: 1_234,
+                flow_quality: 250,
+                flow_valid: true,
+            })
         );
     }
 
@@ -423,18 +349,18 @@ mod tests {
 
     #[test]
     fn exposes_only_valid_generic_observations() {
-        let mut report = Mtf01Report::new(
-            Timestamp::from_microseconds(123),
-            7,
-            2_000,
-            8,
-            9,
-            true,
-            50,
-            -25,
-            12,
-            true,
-        );
+        let mut report = Mtf01Report {
+            timestamp: Timestamp::from_microseconds(123),
+            sensor_time_milliseconds: 7,
+            distance_millimetres: 2_000,
+            distance_strength: 8,
+            distance_precision: 9,
+            distance_valid: true,
+            flow_velocity_x_centimetres_per_second_at_one_metre: 50,
+            flow_velocity_y_centimetres_per_second_at_one_metre: -25,
+            flow_quality: 12,
+            flow_valid: true,
+        };
         assert_eq!(report.range_snapshot().unwrap().distance_millimetres, 2_000);
         assert_eq!(
             report
@@ -449,21 +375,15 @@ mod tests {
         assert_eq!(report.optical_flow_snapshot(), None);
     }
 
-    struct Clock(u64);
-
-    impl MonotonicClock for Clock {
-        fn now(&mut self) -> Timestamp {
-            self.0 += 1;
-            Timestamp::from_microseconds(self.0)
-        }
-    }
-
     #[test]
     fn timestamps_a_report_with_its_last_received_byte() {
-        let mut service = Mtf01Service::new(Clock(100));
+        let mut decoder = Mtf01::new();
         let report = sample_frame()
             .into_iter()
-            .find_map(|byte| service.on_received_byte(byte))
+            .enumerate()
+            .find_map(|(index, byte)| {
+                decoder.receive_byte(Timestamp::from_microseconds(101 + index as u64), byte)
+            })
             .unwrap();
 
         assert_eq!(report.timestamp, Timestamp::from_microseconds(127));