feat(drivers): validate bounded IMU FIFO acquisition

Drain bounded MPU6000 and ICM-42688 FIFO batches. Publish the latest
validated sample only after overflow checks. Reject invalid drains.

Validation: driver, sensor and platform tests plus both ARM release builds
from a fresh build directory.

docs/src/mavlink.md

@@ -1,8 +1,7 @@
 # MAVLink and telemetry
 
-Kaptan uses MAVLink 2 framing exclusively with the generated Kaptan dialect.
+Kaptan uses MAVLink 2 with its private dialect. It does not support `common`,
-It neither emits nor accepts messages from `common`, `minimal`, or another
+`minimal` or other dialects. Reception follows these rules:
-dialect. Inbound processing is narrowly scoped:
 
 - MAVLink 1 is discarded.
 - CRC-invalid MAVLink 2 frames are discarded.
@@ -12,7 +11,7 @@ dialect. Inbound processing is narrowly scoped:
   and extra CRC belong to the Kaptan dialect. The board firmware consumes
   exact-target Kaptan file requests.
 
-Shared or remote inbound authority is deliberately unsupported. Enabling it
+Shared or remote inbound authority is unsupported. Enabling it
 requires verified MAVLink 2 signing, configured source/target binding,
 persistent anti-replay state, and a provisioning/key-rotation policy as one
 reviewed feature.
@@ -34,12 +33,9 @@ The current descriptor uses the pid.codes shared development identity
 `1209:0001` plus the STM32 factory device ID as its serial number. Obtain and
 substitute a project-specific PID before distributing hardware as a product.
 
-The normal telemetry links are operational file transports, not authenticated
+Any connected transmitter can issue file requests without authentication. Use a
-remote-control channels. Any transmitter physically connected to either link
+direct USB cable or USB-UART adapter and disconnect other transmitters while
-can issue file requests, so use a direct USB cable or USB-UART adapter and
+configuring. Motor pins remain low throughout configuration.
-disconnect other transmitters while configuring. The platform retains its
-startup-configured low motor-output state throughout this workflow; no marker
-type is treated as GPIO ownership or command authority.
 
 One file service serializes transfers across USB and all MAVLink UARTs. Its
 owner is the claimed source system/component plus ingress identity, so the same
@@ -60,10 +56,9 @@ replay state remain governed by the protocol lease rather than disconnects.
 USB receive backpressure masks only `GINTMSK.RXFLVLM` while application packet
 bytes remain unconsumed. The final byte cursor and gate update share the USB
 resource lock. The OTG FS NVIC vector stays enabled, as do IN/TX completion,
-reset and suspend interrupt sources. This does not make control reception
+reset and suspend interrupt sources. EP0 shares the RX FIFO and cannot bypass
-independent: EP0 shares the RX FIFO and cannot bypass a blocked bulk packet at
+a blocked bulk packet at its head. Once buffered bytes are consumed, RXFLVLM is
-its head. Once buffered bytes are consumed, RXFLVLM is restored. An incomplete
+restored. An incomplete MAVLink frame alone does not keep RX masked.
-MAVLink frame alone does not keep RX masked.
 
 UART role and baud changes take effect after reboot. See
 [Configuration](configuration.md) for allowed values and USB repair, and
@@ -71,18 +66,12 @@ UART role and baud changes take effect after reboot. See
 Normal USB configuration recovery does not guarantee availability after an
 arbitrary clock, USB hardware or initialization failure.
 
-The local web configurator exposes a bounded live view of CRC-valid inbound
+The web configurator groups CRC-valid packets by source and message ID and
-frames. It groups packets by source and message ID and measures receive
+measures receive frequency in one-second windows. Selecting a message shows
-frequency in one-second windows. Selecting a message reveals the latest decoded
+its latest decoded fields. This view has bounded storage and keeps no history.
-fields, including the readable zero-padded `ERROR.raised_at` reporting
-location. This observation path retains one latest value per bounded aggregate
-rather than packet history and grants no additional command authority.
 
-Diagnostic events also feed a separate bounded Runtime logs view at
+The separate Runtime logs view keeps a bounded history of `LOG` and `ERROR`
-`http://127.0.0.1:4861`. That view retains individual `LOG` and
+events in receive order, with local receive time, level and text.
-`ERROR` events in receive order with local receive time, level, and
-readable text. Packet inspection remains a latest-value view; it is not used as
-log history.
 
 The custom Kaptan definitions are aggregated by `definitions/kaptan.xml` from
 one-message leaves grouped by interaction contract:
@@ -92,23 +81,12 @@ one-message leaves grouped by interaction contract:
 | `publish/` | Zero-reply records for any interested receiver. |
 | `transfer/` | Exact-endpoint, bounded multi-message sessions. |
 
-The root includes every message leaf directly; there are no redundant category
+The root includes each message leaf directly and generates one flat
-index files or separate enum catalogs. Placement is a wire-contract
+`generated::kaptan` module. These names must not be merged with another dialect.
-classification, while bindgen still generates one flat `generated::kaptan`
+See [Configuration](configuration.md), [Logs](logs.md) and [Errors](errors.md)
-module. Bare message names are scoped by that sole dialect and must not be
+for the file-transfer and diagnostic contracts.
-merged with another MAVLink dialect.
-`LOG` is an outbound human-readable four-field record. `ERROR`
-is the separate bounded channel for reportable firmware errors, carrying a
-stable explicit ID plus the `file:line` of its reporting point, and needs no
-redundant level field. `FILE_CONTROL` and `FILE_DATA` implement
-Kaptan's generic virtual-file transport without standard MAVLink FTP
-compatibility. The configurator reads saved and boot-active KCFG values and can
-save replacements or reset saved settings to factory values. See
-[Configuration](configuration.md), [Logs](logs.md), and [Errors](errors.md) for
-the exact contracts.
 
-The board firmware's scheduled publications are native rather than
+Scheduled publications:
-projections into a broader dialect:
 
 | Publication | Current payload |
 | --- | --- |
@@ -117,8 +95,10 @@ projections into a broader dialect:
 | `RANGE` (51029) | Monotonic microseconds and downward distance in mm. |
 | `OPTICAL_FLOW` (51030) | Monotonic microseconds, integer ground-relative velocity in mm/s, distance in mm, and quality. |
 
-
 The IMU payload is 28 bytes with MAVLink extra CRC **22**. Wire field ordering
 places time at byte 0, angular rate at byte 8, acceleration at byte 20 and
 temperature at byte 26. The signed 32-bit gyro units preserve the full decoded
 MPU6000 ±2000°/s range through mounting, mailbox handoff and encoding.
+
+IMU publications retain the newest sample from each validated FIFO drain.
+Their timestamp is FC time at packet-read submission, not physical sampling time.

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

@@ -27,7 +27,19 @@ pub enum Error<BusError> {
     /// 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.
+    /// No checked, nonempty batch arrived within two output periods plus 10 ms.
     #[kaptan_error(id = 5108)]
     DataReadyTimeout,
+    /// FIFO full status or the lost-packet counter reports missing records.
+    #[kaptan_error(id = 5109)]
+    FifoOverflow,
+    /// The reported FIFO count exceeds the configured packet capacity.
+    #[kaptan_error(id = 5110)]
+    FifoCount(u16),
+    /// A packet has an invalid header, sensor code, or timestamp order.
+    #[kaptan_error(id = 5111)]
+    FifoPacket,
+    /// The sensor reset after its configuration had completed.
+    #[kaptan_error(id = 5112)]
+    UnexpectedReset,
 }

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

@@ -1,119 +1,158 @@
-//! ICM-42688-P burst-sample decoding.
+//! ICM-42688-P 16-byte FIFO packet 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;
 
 use super::SAMPLE_BYTES;
 
-pub(crate) fn decode(bytes: &[u8; SAMPLE_BYTES], timestamp: Timestamp) -> ImuSnapshot {
+pub(crate) fn decode(
-    ImuSnapshot::new(
+    bytes: &[u8; SAMPLE_BYTES],
-        timestamp,
+    timestamp: Timestamp,
-        [
+    previous_tick: Option<u16>,
-            scale_acceleration(read_i16(bytes, 2)),
+) -> Option<(ImuSnapshot, u16)> {
-            scale_acceleration(read_i16(bytes, 4)),
+    // Packet 3, both sensors, ordinary resolution, ODR timestamp. Only the
-            scale_acceleration(read_i16(bytes, 6)),
+    // first packet after explicit reset/configuration may report ODR changes.
-        ],
+    if bytes[0] & 0xfc != 0x68 || (previous_tick.is_some() && bytes[0] != 0x68) {
-        [
+        return None;
-            scale_angular_rate(read_i16(bytes, 8)),
+    }
-            scale_angular_rate(read_i16(bytes, 10)),
+    // InvenSense eMD Icm426xx: ordinary FIFO temperature 0x80 is invalid.
-            scale_angular_rate(read_i16(bytes, 12)),
+    if bytes[13] == 0x80 {
-        ],
+        return None;
-        scale_temperature(read_i16(bytes, 0)),
+    }
-    )
+    let tick = u16::from_be_bytes([bytes[14], bytes[15]]);
+    if let Some(previous) = previous_tick {
+        let delta = tick.wrapping_sub(previous);
+        // Ordering only: internal-clock ticks are not microseconds (§12.7).
+        // This does not establish sample spacing or exclude multiple wraps.
+        if delta == 0 || delta >= 0x8000 {
+            return None;
+        }
+    }
+    let raw = [
+        read_i16(bytes, 1),
+        read_i16(bytes, 3),
+        read_i16(bytes, 5),
+        read_i16(bytes, 7),
+        read_i16(bytes, 9),
+        read_i16(bytes, 11),
+    ];
+    // Invalid-code mode reserves -32768; valid readings below -32766 clamp
+    // to -32766 in the sensor (DS-000347 §12.8).
+    if raw.iter().any(|value| *value < -32766) {
+        return None;
+    }
+    Some((
+        ImuSnapshot::new(
+            timestamp,
+            [
+                scale_acceleration(raw[0]),
+                scale_acceleration(raw[1]),
+                scale_acceleration(raw[2]),
+            ],
+            [
+                scale_angular_rate(raw[3]),
+                scale_angular_rate(raw[4]),
+                scale_angular_rate(raw[5]),
+            ],
+            scale_temperature(bytes[13] as i8),
+        ),
+        tick,
+    ))
 }
 
 const fn read_i16(bytes: &[u8; SAMPLE_BYTES], offset: usize) -> i16 {
     i16::from_be_bytes([bytes[offset], bytes[offset + 1]])
 }
 
-// The configured ±4 g range has 8,192 counts per g.
+// ±4 g: 8192 counts/g. Products fit i32; division truncates toward zero.
 const fn scale_acceleration(raw: i16) -> i16 {
     ((raw as i32 * 1_000) / 8_192) as i16
 }
 
-// The configured ±500°/s range has 65.5 counts/(°/s).
+// ±500°/s: 65.5 counts/(°/s).
 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.
+// FIFO temperature uses signed 8-bit data, 2.07 counts/°C, and a 25°C offset.
-const fn scale_temperature(raw: i16) -> i16 {
+const fn scale_temperature(raw: i8) -> i16 {
-    ((raw as i32 * 10_000) / 13_248 + 2_500) as i16
+    ((raw as i32 * 10_000) / 207 + 2_500) as i16
 }
 
 #[cfg(test)]
 mod tests {
-    use super::{decode, scale_temperature};
+    use super::*;
-    use kaptan_time::Timestamp;
 
-    #[test]
+    fn packet() -> [u8; SAMPLE_BYTES] {
-    fn decodes_the_icm_temperature_first_layout() {
+        [
-        let bytes = [0, 0, 0x20, 0, 0, 0, 0, 0, 0x7f, 0xeb, 0, 0, 0, 0];
+            0x68, 0x20, 0, 0, 0, 0, 0, 0x7f, 0xeb, 0, 0, 0, 0, 0, 0xff, 0xfe,
-        let sample = decode(&bytes, Timestamp::from_microseconds(7));
+        ]
-        assert_eq!(sample.acceleration_milligravity[0], 1_000);
-        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() {
+    fn packet_layout_and_signed_scales() {
-        for (temperature_raw, temperature) in [(i16::MIN, -22_234), (i16::MAX, 27_233)] {
+        let timestamp = Timestamp::from_microseconds(7);
-            let mut bytes = [0; 14];
+        let (sample, tick) = decode(&packet(), timestamp, None).unwrap();
-            for (index, raw) in [
+        assert_eq!(sample.timestamp, timestamp);
-                temperature_raw,
+        assert_eq!(sample.acceleration_milligravity, [1000, 0, 0]);
-                i16::MIN,
+        assert_eq!(sample.angular_rate_milliradians_per_second, [8725, 0, 0]);
-                0,
+        assert_eq!(sample.temperature_centidegrees_celsius, 2500);
-                i16::MAX,
+        assert_eq!(tick, 65534);
-                i16::MIN,
+        let mut bytes = packet();
-                0,
+        for (offset, raw) in [(1, -32766i16), (5, 32767), (7, -32766), (11, 32767)] {
-                i16::MAX,
+            bytes[offset..offset + 2].copy_from_slice(&raw.to_be_bytes());
-            ]
+        }
-            .into_iter()
+        let (sample, _) = decode(&bytes, timestamp, None).unwrap();
-            .enumerate()
+        assert_eq!(sample.acceleration_milligravity, [-3999, 0, 3999]);
-            {
+        assert_eq!(
-                bytes[index * 2..index * 2 + 2].copy_from_slice(&raw.to_be_bytes());
+            sample.angular_rate_milliradians_per_second,
-            }
+            [-8730, 0, 8731]
-            let timestamp = Timestamp::from_microseconds(123);
+        );
-            let sample = decode(&bytes, timestamp);
+        for raw in i8::MIN..=i8::MAX {
-            assert_eq!(sample.timestamp, timestamp);
+            let reference = f64::from(raw) / 2.07 * 100.0 + 2500.0;
-            assert_eq!(sample.acceleration_milligravity, [-4_000, 0, 3_999]);
+            assert!((f64::from(scale_temperature(raw)) - reference).abs() < 1.0);
+        }
+        assert_eq!(scale_temperature(i8::MIN), -3683);
+        assert_eq!(scale_temperature(i8::MAX), 8635);
+    }
+
+    #[test]
+    fn header_and_invalid_codes() {
+        let mut bytes = packet();
+        let now = Timestamp::from_microseconds(0);
+        for header in 0..=u8::MAX {
+            bytes[0] = header;
             assert_eq!(
-                sample.angular_rate_milliradians_per_second,
+                decode(&bytes, now, None).is_some(),
-                [-8_731, 0, 8_731]
+                (0x68..=0x6b).contains(&header)
             );
-            assert_eq!(sample.temperature_centidegrees_celsius, temperature);
+            assert_eq!(decode(&bytes, now, Some(65000)).is_some(), header == 0x68);
         }
+        for offset in [1, 3, 5, 7, 9, 11] {
+            for invalid in [i16::MIN, i16::MIN + 1] {
+                bytes = packet();
+                bytes[offset..offset + 2].copy_from_slice(&invalid.to_be_bytes());
+                assert!(decode(&bytes, now, None).is_none());
+            }
+        }
+        bytes = packet();
+        bytes[13] = 0x80;
+        assert!(decode(&bytes, now, None).is_none());
     }
 
     #[test]
-    fn temperature_matches_datasheet_scale_and_rounding() {
+    fn tick_order_allows_wrap() {
-        // ±13,248 counts is ±100°C relative to the 25°C offset.
+        let mut bytes = packet();
-        for (raw, expected) in [
+        let now = Timestamp::from_microseconds(0);
-            (-13_248, -7_500),
+        for (previous, tick, valid) in [
-            (-132, 2_401),
+            (65534, 936u16, true),
-            (-1, 2_500),
+            (937, 1875, true),
-            (0, 2_500),
+            (937, 937, false),
-            (1, 2_500),
+            (937, 936, false),
-            (132, 2_599),
+            (937, 33705, false),
-            (13_248, 12_500),
         ] {
-            assert_eq!(scale_temperature(raw), expected);
+            bytes[14..].copy_from_slice(&tick.to_be_bytes());
-        }
+            assert_eq!(decode(&bytes, now, Some(previous)).is_some(), valid);
-
-        // 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

@@ -1,7 +1,7 @@
 //! Timestamped ICM-42688-P acquisition service.
 
 use super::{Error, Icm42688, Icm42688Event};
-use kaptan_sensor::imu::ImuSnapshot;
+use crate::ImuBatch;
 use kaptan_spi::{SpiCompletion, SpiDevice};
 use kaptan_time::MonotonicClock;
 
@@ -12,8 +12,8 @@ pub enum Icm42688ServiceEvent<BusError> {
     Idle,
     /// IMU configuration completed and the DRDY route may be enabled.
     Initialized,
-    /// A coherent IMU observation is ready for publication or control.
+    /// A checked FIFO batch is ready for publication.
-    Sample(ImuSnapshot),
+    Samples(ImuBatch),
     /// The ICM-42688-P path entered a terminal fault state.
     Fault(Error<BusError>),
 }
@@ -46,13 +46,13 @@ where
         service_event(self.imu.on_periodic_tick(self.clock.now()))
     }
 
-    /// Handles one coalesced data-ready notification. Time is captured here in
+    /// Starts a drain from one coalesced notification. Packet timestamps are
-    /// the worker, at read submission, not at IRQ entry or physical sampling.
+    /// captured at their read submissions, not at IRQ entry or sampling.
     pub fn on_data_ready(&mut self) -> Icm42688ServiceEvent<Bus::StartError> {
         if !self.imu.is_ready() {
             return Icm42688ServiceEvent::Idle;
         }
-        match self.imu.request_sample(self.clock.now()) {
+        match self.imu.request_samples(self.clock.now()) {
             Ok(()) | Err(Error::NotReady) => Icm42688ServiceEvent::Idle,
             Err(error) => Icm42688ServiceEvent::Fault(error),
         }
@@ -69,9 +69,9 @@ where
 
 fn service_event<E>(event: Icm42688Event<E>) -> Icm42688ServiceEvent<E> {
     match event {
-        Icm42688Event::NotPending | Icm42688Event::Initializing => Icm42688ServiceEvent::Idle,
+        Icm42688Event::NotPending | Icm42688Event::InProgress => Icm42688ServiceEvent::Idle,
         Icm42688Event::Initialized => Icm42688ServiceEvent::Initialized,
-        Icm42688Event::Sample(sample) => Icm42688ServiceEvent::Sample(sample),
+        Icm42688Event::Samples(batch) => Icm42688ServiceEvent::Samples(batch),
         Icm42688Event::Fault(error) => Icm42688ServiceEvent::Fault(error),
     }
 }
@@ -79,7 +79,7 @@ fn service_event<E>(event: Icm42688Event<E>) -> Icm42688ServiceEvent<E> {
 #[cfg(test)]
 mod tests {
     use super::*;
-    use crate::mpu6000::test_bus::FakeBus;
+    use crate::icm42688::tests::{initialized, packet};
     use kaptan_time::Timestamp;
 
     struct Clock(u64);
@@ -89,76 +89,62 @@ mod tests {
         }
     }
 
-    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() {
+    fn clock_labels_each_accepted_packet_read() {
-        let mut service = initialized();
+        let mut service = Icm42688Service::new(Clock(50_000), initialized());
-        service.clock.0 = 48_100;
+        service.imu.bus.response.fill(0);
-        service.imu.bus.busy = true;
         assert!(matches!(
             service.on_data_ready(),
             Icm42688ServiceEvent::Idle
         ));
-        service.clock.0 = 48_200;
+        service.imu.bus.response[1..3].copy_from_slice(&16u16.to_be_bytes());
+        service.clock.0 = 50_100;
+        service.on_transfer_complete(SpiCompletion::Complete);
+        service.imu.bus.busy = true;
+        service.clock.0 = 50_200;
+        service.on_transfer_complete(SpiCompletion::Complete);
         service.imu.bus.busy = false;
+        service.imu.bus.response.fill(0);
+        service.imu.bus.response[1..17].copy_from_slice(&packet(937, 0));
+        service.clock.0 = 50_300;
         service.on_periodic_tick();
-        service.clock.0 = 48_500;
+        service.imu.bus.response.fill(0);
-        assert!(
+        for now in [50_400, 50_500] {
-            matches!(service.on_transfer_complete(SpiCompletion::Complete),
+            service.clock.0 = now;
-            Icm42688ServiceEvent::Sample(sample) if sample.timestamp == Timestamp::from_microseconds(48_200))
+            assert!(matches!(
+                service.on_transfer_complete(SpiCompletion::Complete),
+                Icm42688ServiceEvent::Idle
+            ));
+        }
+        service.clock.0 = 50_600;
+        let Icm42688ServiceEvent::Samples(batch) =
+            service.on_transfer_complete(SpiCompletion::Complete)
+        else {
+            panic!("missing checked batch");
+        };
+        assert_eq!(
+            batch.as_slice()[0].timestamp,
+            Timestamp::from_microseconds(50_300)
         );
     }
 
     #[test]
-    fn fake_clock_reports_missing_drdy_and_dma_without_polling_samples() {
+    fn missing_drdy_and_dma_are_terminal() {
-        let mut service = initialized();
+        let mut service = Icm42688Service::new(Clock(61_699), initialized());
-        service.clock.0 = 59_999;
+        let starts = service.imu.bus.starts;
         assert!(matches!(
             service.on_periodic_tick(),
             Icm42688ServiceEvent::Idle
         ));
-        assert_eq!(service.imu.bus.starts, 11);
+        assert_eq!(service.imu.bus.starts, starts);
-        service.clock.0 = 60_000;
+        service.clock.0 = 61_700;
         assert!(matches!(
             service.on_periodic_tick(),
             Icm42688ServiceEvent::Fault(Error::DataReadyTimeout)
         ));
-        let mut service = initialized();
+        let mut service = Icm42688Service::new(Clock(50_000), initialized());
         service.on_data_ready();
-        service.clock.0 = 58_000;
+        service.clock.0 = 60_000;
         assert!(matches!(
             service.on_periodic_tick(),
             Icm42688ServiceEvent::Fault(Error::TransferTimeout)

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

@@ -11,3 +11,57 @@ pub mod mpu6000;
 pub mod lidar_lite_v3;
 /// MicoAir MTF-01 Microlink UART range and optical-flow driver.
 pub mod mtf01;
+
+use kaptan_sensor::imu::ImuSnapshot;
+
+/// Nonempty FIFO drain, published only after the driver checks for overflow.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct ImuBatch {
+    samples: [ImuSnapshot; Self::CAPACITY],
+    len: u8,
+}
+
+impl ImuBatch {
+    /// Maximum records reserved by one driver drain.
+    pub const CAPACITY: usize = 4;
+
+    pub const fn new(first: ImuSnapshot) -> Self {
+        Self {
+            samples: [first; Self::CAPACITY],
+            len: 1,
+        }
+    }
+
+    pub fn push(&mut self, sample: ImuSnapshot) -> Result<(), ImuSnapshot> {
+        let Some(slot) = self.samples.get_mut(usize::from(self.len)) else {
+            return Err(sample);
+        };
+        *slot = sample;
+        self.len += 1;
+        Ok(())
+    }
+
+    pub fn as_slice(&self) -> &[ImuSnapshot] {
+        &self.samples[..usize::from(self.len)]
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use kaptan_time::Timestamp;
+
+    #[test]
+    fn full_batch_preserves_accepted_samples() {
+        let sample = |time| ImuSnapshot::new(Timestamp::from_microseconds(time), [0; 3], [0; 3], 0);
+        let mut batch = ImuBatch::new(sample(0));
+        for time in 1..4 {
+            batch.push(sample(time)).unwrap();
+        }
+        assert_eq!(batch.push(sample(4)), Err(sample(4)));
+        assert_eq!(
+            batch.as_slice(),
+            &[sample(0), sample(1), sample(2), sample(3)]
+        );
+    }
+}

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

@@ -27,7 +27,16 @@ pub enum Error<BusError> {
     /// 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.
+    /// No checked, nonempty batch arrived within two output periods plus 10 ms.
     #[kaptan_error(id = 5008)]
     DataReadyTimeout,
+    /// FIFO overflow destroyed the ordering of unread sensor records.
+    #[kaptan_error(id = 5009)]
+    FifoOverflow,
+    /// The reported FIFO count exceeds its physical capacity.
+    #[kaptan_error(id = 5010)]
+    FifoCount(u16),
+    /// FIFO control does not match the required reset or acquisition state.
+    #[kaptan_error(id = 5011)]
+    FifoControl(u8),
 }

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

@@ -9,7 +9,7 @@ pub mod service;
 #[cfg(test)]
 pub(crate) mod test_bus;
 
-use kaptan_sensor::imu::ImuSnapshot;
+use crate::ImuBatch;
 use kaptan_spi::{CompletionError, SpiCompletion, SpiDevice};
 use kaptan_time::Timestamp;
 
@@ -18,10 +18,11 @@ pub use error::Error;
 pub use service::{Mpu6000Service, Mpu6000ServiceEvent};
 
 use registers::{
-    ACCEL_CONFIG, ACCEL_XOUT_H, CLOCK_SOURCE_PLL_X_GYRO, CONFIG, DATA_READY_INTERRUPT,
+    ACCEL_CONFIG, CLOCK_SOURCE_PLL_X_GYRO, CONFIG, DATA_READY_INTERRUPT, DEVICE_RESET,
-    DEVICE_RESET, ENABLE_ALL_SENSOR_AXES, EXPECTED_WHO_AM_I, GYRO_CONFIG, I2C_INTERFACE_DISABLE,
+    ENABLE_ALL_SENSOR_AXES, EXPECTED_WHO_AM_I, FIFO_CAPACITY, FIFO_COUNT_H, FIFO_EN, FIFO_ENABLE,
-    INT_ENABLE, INT_PIN_CFG, INTERRUPT_CLEAR_ON_ANY_READ, PWR_MGMT_1, PWR_MGMT_2, SAMPLE_BYTES,
+    FIFO_OVERFLOW_INTERRUPT, FIFO_R_W, FIFO_RESET, FIFO_SENSOR_CHANNELS, GYRO_CONFIG,
-    SMPLRT_DIV, USER_CTRL, WHO_AM_I,
+    I2C_INTERFACE_DISABLE, INT_ENABLE, INT_PIN_CFG, INT_STATUS, PWR_MGMT_1, PWR_MGMT_2,
+    SAMPLE_BYTES, SMPLRT_DIV, USER_CTRL, WHO_AM_I,
 };
 
 const TRANSFER_CAPACITY: usize = SAMPLE_BYTES + 1;
@@ -40,7 +41,7 @@ const WAKE_SETTLE_US: u64 = 100_000;
 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] = [
+const CONFIGURATION: [(u8, u8); 8] = [
     (PWR_MGMT_1, CLOCK_SOURCE_PLL_X_GYRO),
     (USER_CTRL, I2C_INTERFACE_DISABLE),
     (PWR_MGMT_2, ENABLE_ALL_SENSOR_AXES),
@@ -48,8 +49,13 @@ const CONFIGURATION: [(u8, u8); 9] = [
     (SMPLRT_DIV, 0),
     (ACCEL_CONFIG, 0),
     (GYRO_CONFIG, 0),
-    (INT_PIN_CFG, INTERRUPT_CLEAR_ON_ANY_READ),
+    // Only INT_STATUS reads clear overflow, including throughout FIFO drains.
-    (INT_ENABLE, DATA_READY_INTERRUPT),
+    (INT_PIN_CFG, 0),
+];
+const FIFO_CONFIGURATION: [(u8, u8); 3] = [
+    (FIFO_EN, FIFO_SENSOR_CHANNELS),
+    (USER_CTRL, I2C_INTERFACE_DISABLE | FIFO_ENABLE),
+    (INT_ENABLE, DATA_READY_INTERRUPT | FIFO_OVERFLOW_INTERRUPT),
 ];
 
 #[derive(Clone, Copy, Debug, Eq, PartialEq)]
@@ -71,9 +77,7 @@ enum State {
     Settling {
         since: Timestamp,
     },
-    Ready {
+    Ready,
-        since: Timestamp,
-    },
     Faulted,
 }
 
@@ -83,7 +87,25 @@ enum Operation {
     ResetSignalPaths,
     Identity,
     Configure(usize),
-    Sample,
+    ResetFifo,
+    CheckFifoReset,
+    ConfigureFifo(usize),
+    StartFifo,
+    FifoStatus,
+    FifoCount,
+    FifoSample(u8),
+    FifoCheck,
+    FifoControl,
+}
+
+impl Operation {
+    fn transfer_length(self) -> usize {
+        match self {
+            Self::FifoCount => 3,
+            Self::FifoSample(_) => TRANSFER_CAPACITY,
+            _ => 2,
+        }
+    }
 }
 
 /// Observable outcome of one timer or SPI terminal event.
@@ -91,12 +113,12 @@ enum Operation {
 pub enum Mpu6000Event<BusError> {
     /// No sample, initialization completion or terminal fault was produced.
     NotPending,
-    /// One initialization transfer completed; initialization continues.
+    /// One transfer completed; initialization or acquisition continues.
-    Initializing,
+    InProgress,
     /// The MPU6000 identity and configuration sequence completed.
     Initialized,
-    /// One coherent IMU sample completed.
+    /// A bounded, overflow-checked FIFO drain completed.
-    Sample(ImuSnapshot),
+    Samples(ImuBatch),
     /// Initialization or acquisition failed.
     Fault(Error<BusError>),
 }
@@ -109,6 +131,8 @@ pub struct Mpu6000<Bus> {
     bus: Bus,
     config: Mpu6000Config,
     state: State,
+    progress_at: Option<Timestamp>,
+    batch: Option<ImuBatch>,
     transmit: [u8; TRANSFER_CAPACITY],
     receive: [u8; TRANSFER_CAPACITY],
 }
@@ -120,6 +144,8 @@ impl<Bus> Mpu6000<Bus> {
             bus,
             config,
             state: State::Dormant,
+            progress_at: None,
+            batch: None,
             transmit: [0; TRANSFER_CAPACITY],
             receive: [0; TRANSFER_CAPACITY],
         }
@@ -154,8 +180,7 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
                 self.queue(next, now);
             }
             State::Settling { since } if elapsed(now, since, self.sample_period_us()) => {
-                self.state = State::Ready { since: now };
+                self.queue(Operation::ResetFifo, now);
-                return Mpu6000Event::Initialized;
             }
             _ => {}
         }
@@ -165,17 +190,16 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
         }
     }
 
-    /// Accepts a burst-read request. Busy admission remains pending for periodic
+    /// Accepts one bounded FIFO drain. Each snapshot records its packet-read
-    /// retry. The eventual accepted submission time becomes the snapshot
+    /// submission time, not the ISR time or a physical sampling epoch.
-    /// timestamp, not the ISR time or a physical sampling epoch.
+    pub fn request_samples(&mut self, now: Timestamp) -> Result<(), Error<Bus::StartError>> {
-    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.queue(Operation::Sample, now);
+        self.queue(Operation::FifoStatus, now);
         self.submit_pending(now)
     }
 
@@ -194,31 +218,21 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
         let State::Active { operation, since } = self.state else {
             return Mpu6000Event::NotPending;
         };
-        let length = if operation == Operation::Sample {
+        let length = operation.transfer_length();
-            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) => {
+            Err(CompletionError::TransferFault) => self.fault(Error::TransferFault),
-                self.state = State::Faulted;
+            Err(error) => self.fault(Error::Completion(error)),
-                Mpu6000Event::Fault(Error::TransferFault)
-            }
-            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 {
-        matches!(self.state, State::Ready { .. })
+        self.state == State::Ready
     }
 
     fn advance_after_completion(
@@ -233,8 +247,7 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
             Operation::Identity => {
                 let identity = self.receive[1];
                 if identity != EXPECTED_WHO_AM_I {
-                    self.state = State::Faulted;
+                    return self.fault(Error::UnexpectedIdentity(identity));
-                    return Mpu6000Event::Fault(Error::UnexpectedIdentity(identity));
                 }
 
                 self.queue(Operation::Configure(0), now);
@@ -244,22 +257,93 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
                 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
+                    // FIFO stays disabled until the configured output has settled.
-                    // accepting DRDY. RM §4.2 defines the configured sample rate.
                     self.state = State::Settling { since: now };
                 } else {
                     self.queue(Operation::Configure(next), now);
                 }
             }
-            Operation::Sample => {
+            Operation::ResetFifo => self.queue(Operation::CheckFifoReset, now),
+            Operation::CheckFifoReset => {
+                if self.receive[1] != I2C_INTERFACE_DISABLE {
+                    return self.fault(Error::FifoControl(self.receive[1]));
+                }
+                self.queue(Operation::ConfigureFifo(0), now);
+            }
+            Operation::ConfigureFifo(index) => {
+                self.queue(
+                    if index + 1 == FIFO_CONFIGURATION.len() {
+                        Operation::StartFifo
+                    } else {
+                        Operation::ConfigureFifo(index + 1)
+                    },
+                    now,
+                );
+            }
+            Operation::StartFifo | Operation::FifoStatus | Operation::FifoCheck => {
+                if self.receive[1] & FIFO_OVERFLOW_INTERRUPT != 0 {
+                    return self.fault(Error::FifoOverflow);
+                }
+                match operation {
+                    Operation::StartFifo => {
+                        self.state = State::Ready;
+                        self.progress_at = Some(now);
+                        return Mpu6000Event::Initialized;
+                    }
+                    Operation::FifoCheck => {
+                        self.queue(Operation::FifoControl, now);
+                    }
+                    _ => self.queue(Operation::FifoCount, now),
+                }
+            }
+            Operation::FifoCount => {
+                let count = u16::from_be_bytes([self.receive[1], self.receive[2]]);
+                if usize::from(count) > FIFO_CAPACITY {
+                    return self.fault(Error::FifoCount(count));
+                }
+                // The count is latched high-byte first. A trailing partial
+                // record may still be arriving; leave it in the FIFO.
+                let packets = (usize::from(count) / SAMPLE_BYTES).min(ImuBatch::CAPACITY);
+                if packets == 0 {
+                    self.state = State::Ready;
+                    return Mpu6000Event::NotPending;
+                }
+                self.queue(Operation::FifoSample(packets as u8), now);
+            }
+            Operation::FifoSample(remaining) => {
                 let mut bytes = [0; SAMPLE_BYTES];
                 bytes.copy_from_slice(&self.receive[1..]);
-                self.state = State::Ready { since: now };
+                let sample = sample::decode(&bytes, requested, self.config);
-                return Mpu6000Event::Sample(sample::decode(&bytes, requested, self.config));
+                match &mut self.batch {
+                    Some(batch) => batch
+                        .push(sample)
+                        .expect("count is bounded by batch capacity"),
+                    None => self.batch = Some(ImuBatch::new(sample)),
+                }
+                self.queue(
+                    if remaining == 1 {
+                        Operation::FifoCheck
+                    } else {
+                        Operation::FifoSample(remaining - 1)
+                    },
+                    now,
+                );
+            }
+            Operation::FifoControl => {
+                // A reset can disable FIFO after the count read. Empty FIFO
+                // reads repeat its last byte, and reset INT_STATUS is zero.
+                if self.receive[1] != (I2C_INTERFACE_DISABLE | FIFO_ENABLE) {
+                    return self.fault(Error::FifoControl(self.receive[1]));
+                }
+                self.state = State::Ready;
+                self.progress_at = Some(now);
+                return Mpu6000Event::Samples(
+                    self.batch.take().expect("checked drain contains a packet"),
+                );
             }
         }
         match self.submit_pending(now) {
-            Ok(()) => Mpu6000Event::Initializing,
+            Ok(()) => Mpu6000Event::InProgress,
             Err(error) => Mpu6000Event::Fault(error),
         }
     }
@@ -276,11 +360,10 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
         self.prepare_write(register, value);
     }
 
-    fn prepare_read(&mut self, register: u8, length: usize) {
+    fn prepare_read(&mut self, register: u8) {
         self.transmit = [0; TRANSFER_CAPACITY];
         self.receive = [0; TRANSFER_CAPACITY];
         self.transmit[0] = register | 0x80;
-        debug_assert!(length <= TRANSFER_CAPACITY);
     }
 
     fn prepare_write(&mut self, register: u8, value: u8) {
@@ -311,15 +394,23 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
         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::Identity => self.prepare_read(WHO_AM_I),
             Operation::Configure(index) => self.prepare_configuration(index),
-            Operation::Sample => self.prepare_read(ACCEL_XOUT_H, TRANSFER_CAPACITY),
+            Operation::ResetFifo => {
+                self.prepare_write(USER_CTRL, I2C_INTERFACE_DISABLE | FIFO_RESET)
+            }
+            Operation::CheckFifoReset | Operation::FifoControl => self.prepare_read(USER_CTRL),
+            Operation::ConfigureFifo(index) => {
+                let (register, value) = FIFO_CONFIGURATION[index];
+                self.prepare_write(register, value);
+            }
+            Operation::StartFifo | Operation::FifoStatus | Operation::FifoCheck => {
+                self.prepare_read(INT_STATUS);
+            }
+            Operation::FifoCount => self.prepare_read(FIFO_COUNT_H),
+            Operation::FifoSample(_) => self.prepare_read(FIFO_R_W),
         }
-        let length = if operation == Operation::Sample {
+        let length = operation.transfer_length();
-            TRANSFER_CAPACITY
-        } else {
-            2
-        };
         match self.bus.start_transfer(&self.transmit[..length]) {
             Ok(()) => {
                 self.state = State::Active {
@@ -330,7 +421,7 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
             }
             Err(error) if Bus::is_busy(&error) => Ok(()),
             Err(error) => {
-                self.state = State::Faulted;
+                self.stop();
                 Err(Error::Start(error))
             }
         }
@@ -348,35 +439,50 @@ impl<Bus: SpiDevice> Mpu6000<Bus> {
     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.
+                Error::TransferTimeout
-                // 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 self.progress_at.is_some_and(|since| {
-                if elapsed(
+                elapsed(
                     now,
                     since,
                     2 * self.sample_period_us() + TRANSFER_TIMEOUT_US,
-                ) =>
+                )
+            }) =>
             {
                 Error::DataReadyTimeout
             }
             _ => return None,
         };
-        self.state = State::Faulted;
+        // Every active timeout uses the bus cancellation contract, which proves
+        // quiescence or quarantines permanent DMA storage.
+        let error = if matches!(self.state, State::Active { .. }) {
+            match self
+                .bus
+                .complete_transfer(SpiCompletion::TransferFault, &mut [])
+            {
+                Err(CompletionError::TransferFault | CompletionError::NotActive) | Ok(()) => error,
+                Err(error) => Error::Completion(error),
+            }
+        } else {
+            error
+        };
+        self.stop();
         Some(error)
     }
+
+    fn stop(&mut self) {
+        self.state = State::Faulted;
+        self.batch = None;
+        self.progress_at = None;
+    }
+
+    fn fault(&mut self, error: Error<Bus::StartError>) -> Mpu6000Event<Bus::StartError> {
+        self.stop();
+        Mpu6000Event::Fault(error)
+    }
 }
 
 fn elapsed(now: Timestamp, since: Timestamp, duration_us: u64) -> bool {
@@ -390,154 +496,278 @@ mod tests {
     use super::test_bus::{FakeBus, FakeError};
     use super::*;
 
-    const TEST_CONFIG: Mpu6000Config = Mpu6000Config {
+    pub(super) const TEST_CONFIG: Mpu6000Config = Mpu6000Config {
         accelerometer: AccelerometerRange::PlusMinus4G,
         gyroscope: GyroscopeRange::PlusMinus1000DegreesPerSecond,
         digital_low_pass_filter: DigitalLowPassFilter::Bandwidth42Hz,
         sample_rate_divider: 9,
     };
+    const BURST: [u8; 14] = [
+        0x20, 0, 0xe0, 0, 0, 0, // 1g, -1g, 0g
+        0, 0, // 36.53 C
+        0x40, 0x10, 0xbf, 0xf0, 0, 0, // 500, -500, 0 degrees/s
+    ];
 
     fn t(us: u64) -> Timestamp {
-        Timestamp::from_microseconds(us + 100_000)
+        Timestamp::from_microseconds(us)
     }
 
-    fn start(driver: &mut Mpu6000<FakeBus>) {
+    fn response(driver: &mut Mpu6000<FakeBus>, bytes: &[u8]) {
-        driver
+        driver.bus.response.fill(0);
-            .begin_initialization(Timestamp::from_microseconds(0))
+        driver.bus.response[1..=bytes.len()].copy_from_slice(bytes);
-            .unwrap();
-        driver.on_periodic_tick(Timestamp::from_microseconds(99_999));
-        assert_eq!(driver.bus.starts, 0);
-        driver.on_periodic_tick(t(0));
     }
 
-    fn initialized() -> Mpu6000<FakeBus> {
+    fn complete(driver: &mut Mpu6000<FakeBus>, now: u64) -> Mpu6000Event<FakeError> {
-        let mut bus = FakeBus::ready();
+        driver.on_transfer_complete(SpiCompletion::Complete, t(now))
-        bus.response[1] = 0x68;
+    }
-        let mut driver = Mpu6000::new(bus, TEST_CONFIG);
+
-        start(&mut driver);
+    pub(super) fn initialized() -> Mpu6000<FakeBus> {
-        driver.on_transfer_complete(SpiCompletion::Complete, t(100));
+        let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
-        driver.on_periodic_tick(t(100_100));
+        driver.begin_initialization(t(0)).unwrap();
-        assert_eq!(&driver.bus.transmitted[..2], &[0x68, 7]);
+        for (at, command) in [(100_000, [0x6b, 0x80]), (200_000, [0x68, 7])] {
-        driver.on_transfer_complete(SpiCompletion::Complete, t(100_200));
+            driver.on_periodic_tick(t(at - 1));
-        driver.on_periodic_tick(t(200_200));
+            assert!(!driver.is_ready());
-        driver.on_transfer_complete(SpiCompletion::Complete, t(200_300));
+            driver.on_periodic_tick(t(at));
-        driver.on_transfer_complete(SpiCompletion::Complete, t(200_400));
+            assert_eq!(&driver.bus.transmitted[..2], &command);
-        driver.on_periodic_tick(t(300_400));
+            complete(&mut driver, at);
-        for index in 1..9 {
-            driver.on_transfer_complete(SpiCompletion::Complete, t(300_400 + index * 100));
         }
-        assert_eq!(
+        response(&mut driver, &[0x68]);
-            driver.on_periodic_tick(t(311_200)),
+        driver.on_periodic_tick(t(300_000));
-            Mpu6000Event::Initialized
+        assert_eq!(&driver.bus.transmitted[..2], &[0xf5, 0]);
-        );
+        response(&mut driver, &[0]);
-        assert!(driver.is_ready());
+        complete(&mut driver, 300_000);
+        assert_eq!(&driver.bus.transmitted[..2], &[0x6b, 1]);
+        complete(&mut driver, 300_000);
+        driver.on_periodic_tick(t(399_999));
+        assert_eq!(driver.bus.starts, 4);
+        driver.on_periodic_tick(t(400_000));
+        for command in [
+            [0x6a, 0x10],
+            [0x6c, 0],
+            [0x1a, 3],
+            [0x19, 9],
+            [0x1c, 8],
+            [0x1b, 0x10],
+            [0x37, 0],
+        ] {
+            assert_eq!(&driver.bus.transmitted[..2], &command);
+            complete(&mut driver, 400_000);
+        }
+        driver.on_periodic_tick(t(409_999));
+        assert_eq!(driver.bus.starts, 11);
+        assert_eq!(driver.request_samples(t(409_999)), Err(Error::NotReady));
+        driver.on_periodic_tick(t(410_000));
+        assert_eq!(&driver.bus.transmitted[..2], &[0x6a, 0x14]);
+        response(&mut driver, &[0x10]);
+        complete(&mut driver, 410_000);
+        assert_eq!(&driver.bus.transmitted[..2], &[0xea, 0]);
+        response(&mut driver, &[0]);
+        complete(&mut driver, 410_000);
+        for command in [[0x23, 0xf8], [0x6a, 0x50], [0x38, 0x11]] {
+            assert_eq!(&driver.bus.transmitted[..2], &command);
+            complete(&mut driver, 410_000);
+        }
+        assert_eq!(&driver.bus.transmitted[..2], &[0xba, 0]);
+        assert_eq!(complete(&mut driver, 410_000), Mpu6000Event::Initialized);
         driver
     }
 
+    fn begin_drain(driver: &mut Mpu6000<FakeBus>, count: u16, now: u64) -> Mpu6000Event<FakeError> {
+        response(driver, &[0]);
+        driver.request_samples(t(now)).unwrap();
+        assert_eq!(&driver.bus.transmitted[..2], &[0xba, 0]);
+        response(driver, &count.to_be_bytes());
+        complete(driver, now);
+        assert_eq!(&driver.bus.transmitted[..3], &[0xf2, 0, 0]);
+        response(driver, &BURST);
+        complete(driver, now)
+    }
+
+    fn operations() -> impl Iterator<Item = Operation> {
+        [
+            Operation::Reset,
+            Operation::ResetSignalPaths,
+            Operation::Identity,
+            Operation::ResetFifo,
+            Operation::CheckFifoReset,
+            Operation::StartFifo,
+            Operation::FifoStatus,
+            Operation::FifoCount,
+            Operation::FifoSample(1),
+            Operation::FifoCheck,
+            Operation::FifoControl,
+        ]
+        .into_iter()
+        .chain((0..CONFIGURATION.len()).map(Operation::Configure))
+        .chain((0..FIFO_CONFIGURATION.len()).map(Operation::ConfigureFifo))
+    }
+
     #[test]
-    fn reset_signal_reset_and_wake_wait_elapsed_time_from_completion() {
+    fn startup_keeps_fifo_disabled_until_sensor_settles() {
-        let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
+        let driver = initialized();
-        driver.bus.response[1] = 0x68;
+        assert!(driver.is_ready());
-        start(&mut driver);
+        assert_eq!(driver.batch, None);
-        driver.on_transfer_complete(SpiCompletion::Complete, t(100));
+        assert_eq!(driver.progress_at, Some(t(410_000)));
-        for now in [100, 100, 50_000, 100_099] {
+    }
-            driver.on_periodic_tick(t(now));
+
-            assert_eq!(driver.bus.starts, 1);
+    #[test]
+    fn invalid_identity_or_fifo_reset_faults() {
+        for (operation, value, error) in [
+            (Operation::Identity, 0x70, Error::UnexpectedIdentity(0x70)),
+            (Operation::CheckFifoReset, 0x14, Error::FifoControl(0x14)),
+            (Operation::CheckFifoReset, 0x50, Error::FifoControl(0x50)),
+        ] {
+            let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
+            response(&mut driver, &[value]);
+            driver.queue(operation, t(0));
+            driver.submit_pending(t(0)).unwrap();
+            assert_eq!(complete(&mut driver, 1), Mpu6000Event::Fault(error));
             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() {
+    fn packet_bytes_remain_owned_until_checked_publication() {
-        let mut bus = FakeBus::ready();
-        bus.response[1] = 0x70;
-        let mut driver = Mpu6000::new(bus, TEST_CONFIG);
-
-        driver.queue(Operation::Identity, t(0));
-        driver.submit_pending(t(0)).unwrap();
-        assert!(matches!(
-            driver.on_transfer_complete(SpiCompletion::Complete, t(100)),
-            Mpu6000Event::Fault(Error::UnexpectedIdentity(0x70))
-        ));
-    }
-
-    #[test]
-    fn converts_a_complete_burst_to_core_units() {
-        let burst = [
-            0x20, 0x00, 0xE0, 0x00, 0x00, 0x00, // accel: 1g, -1g, 0g
-            0x00, 0x00, // 36.53 C
-            0x40, 0x10, 0xBF, 0xF0, 0x00, 0x00, // gyro: 500, -500, 0 dps
-        ];
         let mut driver = initialized();
-        driver.bus.response[1..].copy_from_slice(&burst);
+        begin_drain(&mut driver, 14, 411_000);
-        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)),
+            &driver.bus.transmitted[..15],
+            &[0xf4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+        );
+        assert_eq!(
+            driver.on_transfer_complete(SpiCompletion::NotPending, t(411_010)),
             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) =
+        response(&mut driver, &[0]);
-            driver.on_transfer_complete(SpiCompletion::Complete, t(312_200))
+        assert_eq!(complete(&mut driver, 411_100), Mpu6000Event::InProgress);
-        else {
+        assert_eq!(&driver.bus.transmitted[..2], &[0xba, 0]);
-            panic!("sample transfer must complete");
+        response(&mut driver, &[0x50]);
+        assert_eq!(complete(&mut driver, 411_200), Mpu6000Event::InProgress);
+        assert_eq!(&driver.bus.transmitted[..2], &[0xea, 0]);
+        let Mpu6000Event::Samples(batch) = complete(&mut driver, 411_300) else {
+            panic!("checked batch");
         };
-
+        let sample = batch.as_slice()[0];
-        assert_eq!(sample.acceleration_milligravity, [1_000, -1_000, 0]);
+        assert_eq!(batch.as_slice().len(), 1);
+        assert_eq!(sample.acceleration_milligravity, [1000, -1000, 0]);
         assert_eq!(
             sample.angular_rate_milliradians_per_second,
-            [8_726, -8_726, 0]
+            [8726, -8726, 0]
         );
-        assert_eq!(sample.temperature_centidegrees_celsius, 3_653);
+        assert_eq!(sample.temperature_centidegrees_celsius, 3653);
-        assert_eq!(sample.timestamp, t(312_000));
+        assert_eq!(sample.timestamp, t(411_000));
+        assert_eq!(complete(&mut driver, 411_400), Mpu6000Event::NotPending);
+        assert_eq!(driver.batch, None);
+    }
+
+    #[test]
+    fn bounded_drain_leaves_excess_and_partial_bytes() {
+        for count in [14, 29, 1024] {
+            let mut driver = initialized();
+            begin_drain(&mut driver, count, 411_000);
+            let packets = (usize::from(count) / 14).min(ImuBatch::CAPACITY);
+            for index in 0..packets {
+                assert_eq!(driver.bus.transmitted[0], 0xf4);
+                response(
+                    &mut driver,
+                    if index + 1 == packets { &[0] } else { &BURST },
+                );
+                assert_eq!(
+                    complete(&mut driver, 411_100 + index as u64 * 100),
+                    Mpu6000Event::InProgress
+                );
+            }
+            assert_eq!(driver.bus.transmitted[0], 0xba);
+            response(&mut driver, &[0x50]);
+            assert_eq!(complete(&mut driver, 412_000), Mpu6000Event::InProgress);
+            let Mpu6000Event::Samples(batch) = complete(&mut driver, 412_100) else {
+                panic!("checked batch");
+            };
+            assert_eq!(batch.as_slice().len(), packets);
+            assert_eq!(driver.progress_at, Some(t(412_100)));
+            assert!(driver.is_ready());
+        }
+    }
+
+    #[test]
+    fn empty_or_partial_fifo_cannot_renew_progress() {
+        for count in [0, 1, 13] {
+            let mut driver = initialized();
+            assert_eq!(
+                begin_drain(&mut driver, count, 411_000),
+                Mpu6000Event::NotPending
+            );
+            assert_eq!(driver.bus.transmitted[0], 0xf2);
+            assert!(driver.is_ready());
+            assert_eq!(driver.progress_at, Some(t(410_000)));
+            begin_drain(&mut driver, count, 439_999);
+            assert_eq!(
+                driver.on_periodic_tick(t(440_000)),
+                Mpu6000Event::Fault(Error::DataReadyTimeout)
+            );
+        }
+    }
+
+    #[test]
+    fn impossible_count_and_overflow_discard_pending_records() {
+        let mut driver = initialized();
         assert_eq!(
-            driver.on_transfer_complete(SpiCompletion::Complete, t(312_300)),
+            begin_drain(&mut driver, 1025, 411_000),
-            Mpu6000Event::NotPending
+            Mpu6000Event::Fault(Error::FifoCount(1025))
         );
+        for operation in [Operation::StartFifo, Operation::FifoStatus] {
+            let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
+            response(&mut driver, &[0x10]);
+            driver.queue(operation, t(0));
+            driver.submit_pending(t(0)).unwrap();
+            assert_eq!(
+                complete(&mut driver, 1),
+                Mpu6000Event::Fault(Error::FifoOverflow)
+            );
+            assert_eq!(driver.batch, None);
+        }
+        let mut driver = initialized();
+        begin_drain(&mut driver, 14, 411_000);
+        response(&mut driver, &[0x10]);
+        complete(&mut driver, 411_001);
+        assert!(driver.batch.is_some());
+        assert_eq!(
+            complete(&mut driver, 411_002),
+            Mpu6000Event::Fault(Error::FifoOverflow)
+        );
+        assert_eq!(driver.batch, None);
+        assert_eq!(complete(&mut driver, 411_003), Mpu6000Event::NotPending);
     }
 
     #[test]
-    fn reports_a_route_transfer_fault() {
+    fn reset_during_drain_cannot_publish_repeated_fifo_bytes() {
-        let mut bus = FakeBus::ready();
+        let mut driver = initialized();
-        bus.response[1] = 0x68;
+        driver.request_samples(t(411_000)).unwrap();
-        let mut driver = Mpu6000::new(bus, TEST_CONFIG);
+        response(&mut driver, &14u16.to_be_bytes());
-
+        complete(&mut driver, 411_000);
-        start(&mut driver);
+        response(&mut driver, &[0xaa; 14]);
-        assert!(matches!(
+        complete(&mut driver, 411_000);
-            driver.on_transfer_complete(SpiCompletion::TransferFault, t(100)),
+        response(&mut driver, &[0]);
-            Mpu6000Event::Fault(Error::TransferFault)
+        complete(&mut driver, 411_001);
-        ));
+        assert!(driver.batch.is_some());
+        // Reset clears both overflow status and FIFO enable. A clean status
+        // alone must not accept bytes read after the buffer became disabled.
+        assert_eq!(complete(&mut driver, 411_002), Mpu6000Event::InProgress);
+        assert_eq!(&driver.bus.transmitted[..2], &[0xea, 0]);
+        assert_eq!(
+            complete(&mut driver, 411_003),
+            Mpu6000Event::Fault(Error::FifoControl(0))
+        );
+        assert_eq!(driver.batch, None);
+        assert_eq!(driver.progress_at, None);
     }
 
     #[test]
-    fn completion_errors_never_republish_a_previous_sample() {
+    fn completion_faults_discard_unpublished_records() {
         for (event, error) in [
             (
                 SpiCompletion::Complete,
@@ -551,74 +781,53 @@ mod tests {
             ),
             (SpiCompletion::TransferFault, None),
         ] {
-            let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
+            let mut driver = initialized();
-            driver.state = State::Ready { since: t(0) };
+            begin_drain(&mut driver, 14, 411_000);
-            driver.request_sample(t(1)).unwrap();
+            response(&mut driver, &[0]);
-            assert!(matches!(
+            complete(&mut driver, 411_001);
-                driver.on_transfer_complete(SpiCompletion::Complete, t(1)),
+            assert!(driver.batch.is_some());
-                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)),
+                driver.on_transfer_complete(event, t(411_002)),
                 Mpu6000Event::Fault(_)
             ));
             assert_eq!(driver.receive, [0xa5; TRANSFER_CAPACITY]);
-            assert!(!driver.is_ready());
+            assert_eq!(driver.batch, None);
-            assert!(!matches!(
+            assert_eq!(complete(&mut driver, 411_003), Mpu6000Event::NotPending);
-                driver.on_transfer_complete(SpiCompletion::Complete, t(3)),
+            assert_eq!(
-                Mpu6000Event::Sample(_)
+                driver.begin_initialization(t(411_004)),
-            ));
+                Err(Error::NotReady)
-            if error == Some(CompletionError::Quarantined) {
+            );
-                assert_eq!(driver.begin_initialization(t(4)), Err(Error::NotReady));
-            }
         }
     }
 
     #[test]
-    fn rejected_submission_cannot_complete_a_stale_sample() {
+    fn rejected_submission_discards_the_batch() {
-        let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
+        let mut driver = initialized();
-        driver.state = State::Ready { since: t(0) };
+        begin_drain(&mut driver, 14, 411_000);
-        driver.receive.fill(0xa5);
         driver.bus.fail_start = true;
         assert_eq!(
-            driver.request_sample(t(1)),
+            complete(&mut driver, 411_001),
-            Err(Error::Start(FakeError::Start))
+            Mpu6000Event::Fault(Error::Start(FakeError::Start))
-        );
-        assert_eq!(
-            driver.on_transfer_complete(SpiCompletion::Complete, t(2)),
-            Mpu6000Event::NotPending
         );
+        assert_eq!(driver.batch, None);
+        assert_eq!(complete(&mut driver, 411_002), Mpu6000Event::NotPending);
     }
 
     #[test]
-    fn every_transfer_phase_times_out_and_cancels_once_without_rearming() {
+    fn every_transfer_times_out_and_cancels_once() {
-        let operations = [
+        for operation in 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!(
+            assert_eq!(complete(&mut driver, 10_101), Mpu6000Event::NotPending);
-                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!(
@@ -629,20 +838,13 @@ mod tests {
     }
 
     #[test]
-    fn every_busy_phase_preserves_pending_operation_and_has_an_admission_deadline() {
+    fn busy_admission_preserves_the_operation_and_expires() {
-        for operation in [
+        for operation in operations() {
-            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();
+            driver.on_periodic_tick(t(200));
             assert_eq!(
                 driver.state,
                 State::Pending {
@@ -650,7 +852,6 @@ mod tests {
                     since: t(100)
                 }
             );
-            driver.on_periodic_tick(t(200));
             driver.bus.busy = false;
             driver.on_periodic_tick(t(300));
             assert_eq!(
@@ -662,38 +863,53 @@ mod tests {
             );
             assert_eq!(driver.bus.starts, 1);
         }
-        let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG);
+        let mut driver = initialized();
         driver.bus.busy = true;
-        start(&mut driver);
+        driver.request_samples(t(411_000)).unwrap();
         assert_eq!(
-            driver.on_periodic_tick(t(20_000)),
+            driver.on_periodic_tick(t(431_000)),
             Mpu6000Event::Fault(Error::AdmissionTimeout)
         );
         assert_eq!(driver.bus.cancellations, 0);
     }
 
     #[test]
-    fn missed_drdy_and_late_completions_fault_instead_of_polling_or_publishing() {
+    fn late_completion_cannot_publish() {
         let mut driver = initialized();
-        driver.on_periodic_tick(t(341_199));
+        begin_drain(&mut driver, 14, 411_000);
-        assert_eq!(driver.bus.starts, 12);
+        response(&mut driver, &[0]);
+        complete(&mut driver, 411_100);
+        assert!(driver.batch.is_some());
         assert_eq!(
-            driver.on_periodic_tick(t(341_200)),
+            complete(&mut driver, 421_100),
-            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)
         );
+        assert_eq!(driver.batch, None);
+        assert_eq!(driver.bus.cancellations, 1);
         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)),
+            driver.request_samples(t(440_000)),
-            Mpu6000Event::Fault(Error::Completion(CompletionError::Quarantined))
+            Err(Error::DataReadyTimeout)
         );
-        assert_eq!(driver.bus.start_transfer(&[1]), Err(FakeError::Quarantined));
+        assert!(!driver.is_ready());
+    }
+
+    #[test]
+    fn progress_expiry_cancels_active_drain() {
+        for completion_error in [None, Some(CompletionError::Quarantined)] {
+            let mut driver = initialized();
+            begin_drain(&mut driver, 14, 439_000);
+            driver.bus.fail_completion = completion_error;
+            let expected = completion_error
+                .map(Error::Completion)
+                .unwrap_or(Error::DataReadyTimeout);
+            assert_eq!(
+                driver.on_periodic_tick(t(440_000)),
+                Mpu6000Event::Fault(expected)
+            );
+            assert_eq!(driver.bus.cancellations, 1);
+            assert_eq!(driver.batch, None);
+            assert_eq!(complete(&mut driver, 440_001), Mpu6000Event::NotPending);
+        }
     }
 }

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

@@ -11,12 +11,19 @@ pub(crate) const GYRO_CONFIG: u8 = 0x1B;
 pub(crate) const ACCEL_CONFIG: u8 = 0x1C;
 pub(crate) const INT_PIN_CFG: u8 = 0x37;
 pub(crate) const INT_ENABLE: u8 = 0x38;
-pub(crate) const ACCEL_XOUT_H: u8 = 0x3B;
+pub(crate) const INT_STATUS: u8 = 0x3A;
+pub(crate) const FIFO_EN: u8 = 0x23;
+pub(crate) const FIFO_COUNT_H: u8 = 0x72;
+pub(crate) const FIFO_R_W: u8 = 0x74;
 
 pub(crate) const CLOCK_SOURCE_PLL_X_GYRO: u8 = 0x01;
 pub(crate) const DEVICE_RESET: u8 = 0x80;
 pub(crate) const I2C_INTERFACE_DISABLE: u8 = 0x10;
 pub(crate) const ENABLE_ALL_SENSOR_AXES: u8 = 0x00;
-pub(crate) const INTERRUPT_CLEAR_ON_ANY_READ: u8 = 0x10;
 pub(crate) const DATA_READY_INTERRUPT: u8 = 0x01;
+pub(crate) const FIFO_OVERFLOW_INTERRUPT: u8 = 0x10;
+pub(crate) const FIFO_RESET: u8 = 0x04;
+pub(crate) const FIFO_ENABLE: u8 = 0x40;
+pub(crate) const FIFO_SENSOR_CHANNELS: u8 = 0xF8;
+pub(crate) const FIFO_CAPACITY: usize = 1024;
 pub(crate) const SAMPLE_BYTES: usize = 14;

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

@@ -1,7 +1,7 @@
 //! Timestamped MPU6000 acquisition service.
 
 use super::{Error, Mpu6000, Mpu6000Event};
-use kaptan_sensor::imu::ImuSnapshot;
+use crate::ImuBatch;
 use kaptan_spi::{SpiCompletion, SpiDevice};
 use kaptan_time::MonotonicClock;
 
@@ -12,16 +12,13 @@ pub enum Mpu6000ServiceEvent<BusError> {
     Idle,
     /// MPU configuration completed and the DRDY route may be enabled.
     Initialized,
-    /// A coherent IMU observation is ready for publication or control.
+    /// A bounded, overflow-checked FIFO drain is ready for publication.
-    Sample(ImuSnapshot),
+    Samples(ImuBatch),
     /// The MPU6000 path entered a terminal fault state.
     Fault(Error<BusError>),
 }
 
-/// Owns MPU6000 state, its clock, and sample acceptance.
+/// Drives MPU6000 acquisition from deferred events using one monotonic clock.
-///
-/// Exactly one platform worker invokes this service. Hardware ISRs only report
-/// deferred DRDY, timer, and SPI-DMA terminal events to that worker.
 pub struct Mpu6000Service<Clock, Bus> {
     clock: Clock,
     imu: Mpu6000<Bus>,
@@ -49,14 +46,13 @@ where
         service_event(self.imu.on_periodic_tick(self.clock.now()))
     }
 
-    /// Handles one coalesced data-ready notification. Time is captured here in
+    /// Starts one bounded drain after a coalesced data-ready notification.
-    /// 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;
         }
 
-        match self.imu.request_sample(self.clock.now()) {
+        match self.imu.request_samples(self.clock.now()) {
             Ok(()) | Err(Error::NotReady) => Mpu6000ServiceEvent::Idle,
             Err(error) => Mpu6000ServiceEvent::Fault(error),
         }
@@ -73,32 +69,23 @@ where
 
 fn service_event<E>(event: Mpu6000Event<E>) -> Mpu6000ServiceEvent<E> {
     match event {
-        Mpu6000Event::NotPending | Mpu6000Event::Initializing => Mpu6000ServiceEvent::Idle,
+        Mpu6000Event::NotPending | Mpu6000Event::InProgress => Mpu6000ServiceEvent::Idle,
         Mpu6000Event::Initialized => Mpu6000ServiceEvent::Initialized,
-        Mpu6000Event::Sample(sample) => Mpu6000ServiceEvent::Sample(sample),
+        Mpu6000Event::Samples(batch) => Mpu6000ServiceEvent::Samples(batch),
         Mpu6000Event::Fault(error) => Mpu6000ServiceEvent::Fault(error),
     }
 }
 
 #[cfg(test)]
 mod tests {
-    use super::{Mpu6000Service, Mpu6000ServiceEvent};
+    use super::*;
-    use crate::mpu6000::test_bus::FakeBus;
     use crate::mpu6000::{
-        AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000, Mpu6000Config,
+        test_bus::FakeBus,
-    };
+        tests::{TEST_CONFIG, initialized},
-    use kaptan_spi::SpiCompletion;
-    use kaptan_time::{MonotonicClock, Timestamp};
-
-    const TEST_CONFIG: Mpu6000Config = Mpu6000Config {
-        accelerometer: AccelerometerRange::PlusMinus4G,
-        gyroscope: GyroscopeRange::PlusMinus1000DegreesPerSecond,
-        digital_low_pass_filter: DigitalLowPassFilter::Bandwidth42Hz,
-        sample_rate_divider: 9,
     };
+    use kaptan_time::Timestamp;
 
     struct Clock(u64);
-
     impl MonotonicClock for Clock {
         fn now(&mut self) -> Timestamp {
             Timestamp::from_microseconds(self.0)
@@ -106,56 +93,47 @@ mod tests {
     }
 
     #[test]
-    fn accepts_a_sample_only_after_initialization() {
+    fn publication_waits_for_post_status() {
-        let mut bus = FakeBus::ready();
+        let mut service = Mpu6000Service::new(Clock(411_000), initialized());
-        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.imu.bus.response[1..3].copy_from_slice(&14u16.to_be_bytes());
-        service.on_periodic_tick();
+        service.on_transfer_complete(SpiCompletion::Complete);
-        assert_eq!(service.imu.bus.starts, 0);
+        service.imu.bus.response.fill(0);
-        service.clock.0 = 100_000;
+        service.clock.0 = 411_100;
+        service.imu.bus.busy = true;
+        service.on_transfer_complete(SpiCompletion::Complete);
+        service.clock.0 = 411_200;
+        service.imu.bus.busy = false;
         service.on_periodic_tick();
+        service.clock.0 = 411_300;
         assert!(matches!(
             service.on_transfer_complete(SpiCompletion::Complete),
             Mpu6000ServiceEvent::Idle
         ));
-        service.clock.0 = 200_000;
+        service.clock.0 = 411_400;
-        service.on_periodic_tick();
+        service.imu.bus.response[1] = 0x50;
-        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_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)
+            Mpu6000ServiceEvent::Idle
         ));
+        service.clock.0 = 411_500;
+        let Mpu6000ServiceEvent::Samples(batch) =
+            service.on_transfer_complete(SpiCompletion::Complete)
+        else {
+            panic!("checked batch");
+        };
+        assert_eq!(
+            batch.as_slice()[0].timestamp,
+            Timestamp::from_microseconds(411_200)
+        );
     }
 
     #[test]
-    fn fake_clock_expires_active_work_without_any_irq() {
+    fn clock_expires_active_work_without_an_irq() {
         let mut service =
             Mpu6000Service::new(Clock(50), Mpu6000::new(FakeBus::ready(), TEST_CONFIG));
         service.begin().unwrap();
+        assert!(matches!(service.on_data_ready(), Mpu6000ServiceEvent::Idle));
         service.clock.0 = 100_050;
         service.on_periodic_tick();
         service.clock.0 = 110_049;
@@ -166,7 +144,7 @@ mod tests {
         service.clock.0 = 110_050;
         assert!(matches!(
             service.on_periodic_tick(),
-            Mpu6000ServiceEvent::Fault(super::Error::TransferTimeout)
+            Mpu6000ServiceEvent::Fault(Error::TransferTimeout)
         ));
         assert!(matches!(
             service.on_transfer_complete(SpiCompletion::Complete),

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

@@ -11,27 +11,27 @@ pub(crate) enum FakeError {
 }
 
 pub(crate) struct FakeBus {
-    pub response: [u8; 15],
+    pub response: [u8; 32],
     pub fail_start: bool,
     pub fail_completion: Option<CompletionError>,
     pub busy: bool,
     pub starts: usize,
     pub cancellations: usize,
-    pub transmitted: [u8; 15],
+    pub transmitted: [u8; 32],
-    pending: Option<([u8; 15], usize)>,
+    pending: Option<([u8; 32], usize)>,
     quarantined: bool,
 }
 
 impl FakeBus {
     pub fn ready() -> Self {
         Self {
-            response: [0; 15],
+            response: [0; 32],
             fail_start: false,
             fail_completion: None,
             busy: false,
             starts: 0,
             cancellations: 0,
-            transmitted: [0; 15],
+            transmitted: [0; 32],
             pending: None,
             quarantined: false,
         }

firmware/libs/kaptan-sensor/src/imu.rs

@@ -2,28 +2,21 @@
 
 use kaptan_time::Timestamp;
 
-/// Coherent IMU observation in the board body frame.
+/// Coherent IMU observation with explicit units.
 ///
 /// Acceleration is milligravity, angular rate is milliradians per second, and
-/// temperature is centidegrees Celsius. The board-frame transform is complete
+/// temperature is centidegrees Celsius. Chip drivers return sensor-frame axes.
-/// before this value is published.
+/// The platform applies the board mounting transform before publication, so
-///
+/// published observations use the body frame. This type does not validate values.
-/// Current MPU6000 and ICM-42688-P decoders round integer conversions toward
-/// zero. Across their configured ranges, decoded acceleration is within
-/// ±16,000 mg and angular rate within ±34,871 mrad/s, including mounting sign
-/// changes. Temperature is within -22,234..=27,233 cdegC. These are backend
-/// representation bounds over every raw i16 value, not physical operating
-/// limits or validation performed by this public snapshot type.
 #[derive(Clone, Copy, Debug, Eq, PartialEq)]
 pub struct ImuSnapshot {
-    /// Host monotonic acquisition-request time, captured immediately before the
+    /// Producer timestamp in the controller's monotonic clock domain.
-    /// accepted register read is submitted by the worker. This is neither DRDY
+    /// The acquisition source defines its meaning. It need not represent the
-    /// ISR entry nor the sensor's physical sampling epoch. Register updates,
+    /// sensor's physical sampling epoch.
-    /// filtering and deferred/coalesced interrupts prevent that inference.
     pub timestamp: Timestamp,
-    /// Body-frame acceleration in milligravity.
+    /// Acceleration in milligravity, in the producer's frame described above.
     pub acceleration_milligravity: [i16; 3],
-    /// Body-frame angular rate in milliradians per second.
+    /// Angular rate in milliradians per second, in the same frame as acceleration.
     pub angular_rate_milliradians_per_second: [i32; 3],
     /// Sensor temperature in centidegrees Celsius.
     pub temperature_centidegrees_celsius: i16,

firmware/platforms/stm32f7/src/f722/sensors.rs

@@ -2,6 +2,7 @@
 
 use super::peripherals::{Spi1, Spi1Error, Spi3, Spi3Error, TimerClock};
 use kaptan_drivers::{
+    ImuBatch,
     icm42688::{Icm42688, Icm42688Service, Icm42688ServiceEvent},
     mpu6000::{Mpu6000, Mpu6000Config, Mpu6000Service, Mpu6000ServiceEvent},
 };
@@ -14,8 +15,7 @@ use crate::ImuMount;
 
 use super::routes::{Icm42688Spi3Route, Mpu6000Spi1Route};
 
-/// A profile-neutral IMU worker result.  Device protocol errors are consumed
+/// IMU worker result with device errors mapped to reportable IDs.
-/// here because flight policy only needs the terminal healthy/fault outcome.
 pub enum ImuEvent {
     Idle,
     Initialized,
@@ -23,8 +23,7 @@ pub enum ImuEvent {
     Fault(ErrorId),
 }
 
-/// Operations used by the deferred IMU worker, statically dispatched to the
+/// Sensor operations invoked by the deferred IMU worker.
-/// route's concrete service. No peripheral ownership crosses this interface.
 pub trait ImuService: Send + 'static {
     fn begin(&mut self) -> ImuEvent;
     fn on_periodic_tick(&mut self) -> ImuEvent;
@@ -82,7 +81,6 @@ impl ImuService for Mpu6000Imu {
     }
 }
 
-/// Concrete ICM owner, including its board-declared mounting transform.
 pub struct Icm42688Imu {
     inner: Icm42688Service<TimerClock, Icm42688Spi3Route>,
     mount: ImuMount,
@@ -136,7 +134,7 @@ fn mpu_event(event: Mpu6000ServiceEvent<Spi1Error>, mount: ImuMount) -> ImuEvent
     match event {
         Mpu6000ServiceEvent::Idle => ImuEvent::Idle,
         Mpu6000ServiceEvent::Initialized => ImuEvent::Initialized,
-        Mpu6000ServiceEvent::Sample(sample) => ImuEvent::Sample(align_mount(sample, mount)),
+        Mpu6000ServiceEvent::Samples(samples) => latest_sample(samples, mount),
         Mpu6000ServiceEvent::Fault(error) => ImuEvent::Fault(error.error_id()),
     }
 }
@@ -145,14 +143,21 @@ fn icm_event(event: Icm42688ServiceEvent<Spi3Error>, mount: ImuMount) -> ImuEven
     match event {
         Icm42688ServiceEvent::Idle => ImuEvent::Idle,
         Icm42688ServiceEvent::Initialized => ImuEvent::Initialized,
-        Icm42688ServiceEvent::Sample(sample) => ImuEvent::Sample(align_mount(sample, mount)),
+        Icm42688ServiceEvent::Samples(samples) => latest_sample(samples, mount),
         Icm42688ServiceEvent::Fault(error) => ImuEvent::Fault(error.error_id()),
     }
 }
 
-/// Applies the board-declared fixed IMU-to-body transform.
+fn latest_sample(samples: ImuBatch, mount: ImuMount) -> ImuEvent {
-///
+    // Scheduled MAVLink publication retains only the newest validated record.
-/// This is immutable board metadata, not an airframe calibration setting.
+    let sample = samples
+        .as_slice()
+        .last()
+        .expect("driver batches are nonempty");
+    ImuEvent::Sample(align_mount(*sample, mount))
+}
+
+/// Applies the fixed IMU-to-body transform from the board wiring.
 fn align_mount(mut sample: ImuSnapshot, mount: ImuMount) -> ImuSnapshot {
     match mount {
         ImuMount::BodyAligned => sample,
@@ -183,6 +188,7 @@ mod tests {
     use crate::ImuMount;
     use crate::f722::peripherals::{Spi1Error, Spi3Error};
     use kaptan_drivers::{
+        ImuBatch,
         icm42688::{Error as Icm42688Error, Icm42688ServiceEvent},
         mpu6000::{Error as Mpu6000Error, Mpu6000ServiceEvent},
     };
@@ -208,7 +214,7 @@ mod tests {
     }
 
     #[test]
-    fn mpu_samples_apply_declared_mount_without_changing_timestamp_or_temperature() {
+    fn aligns_latest_fifo_sample() {
         let sample = ImuSnapshot::new(
             Timestamp::from_microseconds(123),
             [10, 20, 30],
@@ -220,18 +226,23 @@ mod tests {
             (ImuMount::Yaw180, [-10, -20, 30], [-40, -50, 60]),
             (ImuMount::Yaw270, [-20, 10, 30], [-50, 40, 60]),
         ] {
-            let ImuEvent::Sample(aligned) = mpu_event(Mpu6000ServiceEvent::Sample(sample), mount)
+            let mut samples = ImuBatch::new(sample);
+            let later = ImuSnapshot {
+                timestamp: Timestamp::from_microseconds(456),
+                ..sample
+            };
+            samples.push(later).unwrap();
+            let ImuEvent::Sample(aligned) = mpu_event(Mpu6000ServiceEvent::Samples(samples), mount)
             else {
-                panic!("sample event must stay a sample");
+                panic!("validated batch must produce a sample");
             };
             assert_eq!(
                 aligned,
-                ImuSnapshot::new(
+                ImuSnapshot {
-                    Timestamp::from_microseconds(123),
+                    acceleration_milligravity: acceleration,
-                    acceleration,
+                    angular_rate_milliradians_per_second: angular_rate,
-                    angular_rate,
+                    ..later
-                    25,
+                }
-                )
             );
         }
     }