From d32b03c87ff37ace70158495000fb8a44a329529 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Onur=20=C3=96zkan?= Date: Sat, 7 Nov 2026 18:12:11 +0300 Subject: [PATCH] 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 | 64 +- .../libs/kaptan-drivers/src/icm42688/error.rs | 14 +- .../libs/kaptan-drivers/src/icm42688/mod.rs | 779 +++++++++++------- .../kaptan-drivers/src/icm42688/sample.rs | 201 +++-- .../kaptan-drivers/src/icm42688/service.rs | 104 +-- firmware/libs/kaptan-drivers/src/lib.rs | 54 ++ .../libs/kaptan-drivers/src/mpu6000/error.rs | 11 +- .../libs/kaptan-drivers/src/mpu6000/mod.rs | 714 ++++++++++------ .../kaptan-drivers/src/mpu6000/registers.rs | 11 +- .../kaptan-drivers/src/mpu6000/service.rs | 100 +-- .../kaptan-drivers/src/mpu6000/test_bus.rs | 10 +- firmware/libs/kaptan-sensor/src/imu.rs | 25 +- .../platforms/stm32f7/src/f722/sensors.rs | 49 +- 13 files changed, 1303 insertions(+), 833 deletions(-) diff --git a/docs/src/mavlink.md b/docs/src/mavlink.md index 7cd1110..268f06f 100644 --- a/docs/src/mavlink.md +++ b/docs/src/mavlink.md @@ -1,8 +1,7 @@ # MAVLink and telemetry -Kaptan uses MAVLink 2 framing exclusively with the generated Kaptan dialect. -It neither emits nor accepts messages from `common`, `minimal`, or another -dialect. Inbound processing is narrowly scoped: +Kaptan uses MAVLink 2 with its private dialect. It does not support `common`, +`minimal` or other dialects. Reception follows these rules: - 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 -remote-control channels. Any transmitter physically connected to either link -can issue file requests, so use a direct USB cable or USB-UART adapter and -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. +Any connected transmitter can issue file requests without authentication. Use a +direct USB cable or USB-UART adapter and disconnect other transmitters while +configuring. Motor pins remain low throughout configuration. 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 -independent: EP0 shares the RX FIFO and cannot bypass a blocked bulk packet at -its head. Once buffered bytes are consumed, RXFLVLM is restored. An incomplete -MAVLink frame alone does not keep RX masked. +reset and suspend interrupt sources. EP0 shares the RX FIFO and cannot bypass +a blocked bulk packet at its head. Once buffered bytes are consumed, RXFLVLM is +restored. An incomplete 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 -frames. It groups packets by source and message ID and measures receive -frequency in one-second windows. Selecting a message reveals the latest decoded -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. +The web configurator groups CRC-valid packets by source and message ID and +measures receive frequency in one-second windows. Selecting a message shows +its latest decoded fields. This view has bounded storage and keeps no history. -Diagnostic events also feed a separate bounded Runtime logs view at -`http://127.0.0.1:4861`. That view retains individual `LOG` and -`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 separate Runtime logs view keeps a bounded history of `LOG` and `ERROR` +events in receive order, with local receive time, level and text. 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 -index files or separate enum catalogs. Placement is a wire-contract -classification, while bindgen still generates one flat `generated::kaptan` -module. Bare message names are scoped by that sole dialect and must not be -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 root includes each message leaf directly and generates one flat +`generated::kaptan` module. These names must not be merged with another dialect. +See [Configuration](configuration.md), [Logs](logs.md) and [Errors](errors.md) +for the file-transfer and diagnostic contracts. -The board firmware's scheduled publications are native rather than -projections into a broader dialect: +Scheduled publications: | 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. diff --git a/firmware/libs/kaptan-drivers/src/icm42688/error.rs b/firmware/libs/kaptan-drivers/src/icm42688/error.rs index 31b3a61..918c4e1 100644 --- a/firmware/libs/kaptan-drivers/src/icm42688/error.rs +++ b/firmware/libs/kaptan-drivers/src/icm42688/error.rs @@ -27,7 +27,19 @@ pub enum Error { /// 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, } diff --git a/firmware/libs/kaptan-drivers/src/icm42688/mod.rs b/firmware/libs/kaptan-drivers/src/icm42688/mod.rs index 3aeeb2d..6ac5fee 100644 --- a/firmware/libs/kaptan-drivers/src/icm42688/mod.rs +++ b/firmware/libs/kaptan-drivers/src/icm42688/mod.rs @@ -1,10 +1,10 @@ -//! ICM-42688-P register protocol and asynchronous sample driver. +//! ICM-42688-P register protocol and asynchronous FIFO driver. mod error; mod sample; pub mod service; -use kaptan_sensor::imu::ImuSnapshot; +use crate::ImuBatch; use kaptan_spi::{CompletionError, SpiCompletion, SpiDevice}; use kaptan_time::Timestamp; @@ -23,38 +23,47 @@ const INT_CONFIG: u8 = 0x14; const INT_CONFIG0: u8 = 0x63; const INT_CONFIG1: u8 = 0x64; const INT_SOURCE0: u8 = 0x65; +const INTF_CONFIG0: u8 = 0x4c; const INTF_CONFIG1: u8 = 0x4d; -const TEMP_DATA1: u8 = 0x1d; -const SAMPLE_BYTES: usize = 14; +const TMST_CONFIG: u8 = 0x54; +const FIFO_CONFIG: u8 = 0x16; +const FIFO_CONFIG1: u8 = 0x5f; +const INT_STATUS: u8 = 0x2d; +const FIFO_COUNTH: u8 = 0x2e; +const FIFO_DATA: u8 = 0x30; +const FIFO_LOST_PKT0: u8 = 0x6c; +const FIFO_FULL: u8 = 0x02; +const RESET_DONE: u8 = 0x10; +const SAMPLE_BYTES: usize = 16; 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. +// DS-000347 rev 1.6 §6.3: 2048 bytes minus one reserved packet plus two cached packets. +const FIFO_CAPACITY: usize = 2048 + SAMPLE_BYTES; +// DS-000347 §14.1/3.3.2: 1 ms reset/power-on access delay. §14.36 requires +// 200 us without writes after OFF -> ON and at least 45 ms gyro ON time. +// This allowance is not a measured 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; +const PROGRESS_TIMEOUT_US: u64 = 2 * SAMPLE_PERIOD_US + TRANSFER_TIMEOUT_US; -// 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. -const CONFIGURATION: [(u8, u8); 9] = [ +// Configure while both sensors are off; enable FIFO sources only after settling. +// INT_STATUS alone clears overflow, so FIFO reads cannot erase loss evidence. +const CONFIGURATION: [(u8, u8); 13] = [ (INT_CONFIG, 0x03), (INT_CONFIG0, 0x00), (INT_CONFIG1, 0x60), + (INTF_CONFIG0, 0x33), // Big-endian byte count/data, invalid codes, SPI only. (INTF_CONFIG1, 0x40), + (TMST_CONFIG, 0x21), // Absolute ODR ticks; retain reserved reset bit 5. + (FIFO_CONFIG1, 0x44), // Partial reads and temperature; neither sensor enabled. + (FIFO_CONFIG, 0x40), (GYRO_ACCEL_CONFIG0, 0xff), (INT_SOURCE0, 0x08), + (GYRO_CONFIG0, 0x46), // ±500°/s at 1 kHz. + (ACCEL_CONFIG0, 0x46), // ±4 g at 1 kHz. (PWR_MGMT0, 0x0f), - // ±500°/s and ±4 g at 1 kHz. The two range fields use the ICM-42688-P - // encoding in bits 7:5 and the output data rate is in bits 3:0. - (GYRO_CONFIG0, 0x46), - (ACCEL_CONFIG0, 0x46), ]; #[derive(Clone, Copy, Debug, Eq, PartialEq)] @@ -73,12 +82,7 @@ enum State { since: Timestamp, delay_us: u64, }, - Settling { - since: Timestamp, - }, - Ready { - since: Timestamp, - }, + Ready, Faulted, } @@ -87,33 +91,49 @@ enum Operation { Reset, Identity, Configure(usize), - Sample, + EnableFifo, + StartFifo, + FifoStatus, + FifoCount, + FifoSample(u8), + FifoLost, + FifoCheck, +} + +impl Operation { + const fn length(self) -> usize { + match self { + Self::FifoSample(_) => TRANSFER_CAPACITY, + Self::FifoCount | Self::FifoLost => 3, + _ => 2, + } + } } /// Observable outcome of one timer or ICM-42688-P SPI terminal event. #[derive(Debug, Eq, PartialEq)] pub enum Icm42688Event { - /// No sample, initialization completion or terminal fault was produced. + /// No batch, initialization completion or terminal fault was produced. NotPending, - /// One initialization transfer completed; initialization continues. - Initializing, + /// A transfer completed; the current transaction continues. + InProgress, /// The ICM-42688-P identity and configuration sequence completed. Initialized, - /// One coherent IMU sample completed. - Sample(ImuSnapshot), + /// A FIFO drain passed the final loss and status checks. + Samples(ImuBatch), /// Initialization or acquisition failed. Fault(Error), } -/// Configured ICM-42688-P driver. -/// -/// The driver owns ordinary movable staging buffers and progresses from timer, -/// data-ready and completion events. Hardware never addresses these buffers. +/// Owns movable staging buffers; hardware never addresses this storage. pub struct Icm42688 { bus: Bus, state: State, transmit: [u8; TRANSFER_CAPACITY], receive: [u8; TRANSFER_CAPACITY], + batch: Option, + previous_tick: Option, + progress_at: Option, } impl Icm42688 { @@ -124,14 +144,16 @@ impl Icm42688 { state: State::Dormant, transmit: [0; TRANSFER_CAPACITY], receive: [0; TRANSFER_CAPACITY], + batch: None, + previous_tick: None, + progress_at: None, } } } impl Icm42688 { - /// 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. + /// Accepts initialization once. Supplied instants must use one non-regressing + /// boot clock. Power must be stable; register access waits another 1 ms. pub fn begin_initialization(&mut self, now: Timestamp) -> Result<(), Error> { if self.state != State::Dormant { return Err(Error::NotReady); @@ -140,25 +162,19 @@ impl Icm42688 { Ok(()) } - /// Services elapsed settling, admission and acquisition deadlines. Call - /// periodically even with an active transfer. No unsolicited reads occur. + /// Services settling, admission and progress deadlines, including active DMA. pub fn on_periodic_tick(&mut self, now: Timestamp) -> Icm42688Event { if let Some(error) = self.check_deadline(now) { return Icm42688Event::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, SAMPLE_PERIOD_US) => { - self.state = State::Ready { since: now }; - return Icm42688Event::Initialized; - } - _ => {} + if let State::Waiting { + next, + since, + delay_us, + } = self.state + && elapsed(now, since, delay_us) + { + self.queue(next, now); } match self.submit_pending(now) { Ok(()) => Icm42688Event::NotPending, @@ -166,17 +182,16 @@ impl Icm42688 { } } - /// 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> { + /// Requests a bounded FIFO drain. Each packet retains its accepted SPI-read + /// submission time, not an ISR time or physical sampling epoch. + pub fn request_samples(&mut self, now: Timestamp) -> Result<(), Error> { 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) } @@ -186,40 +201,29 @@ impl Icm42688 { completion: SpiCompletion, now: Timestamp, ) -> Icm42688Event { - if completion == SpiCompletion::NotPending { - return Icm42688Event::NotPending; - } if let Some(error) = self.check_deadline(now) { return Icm42688Event::Fault(error); } + if completion == SpiCompletion::NotPending { + return Icm42688Event::NotPending; + } 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]) + .complete_transfer(completion, &mut self.receive[..operation.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) - } - Err(error) => { - self.state = State::Faulted; - Icm42688Event::Fault(Error::Completion(error)) - } + Err(CompletionError::TransferFault) => self.fault(Error::TransferFault), + Err(error) => self.fault(Error::Completion(error)), } } - /// Returns whether an IMU data-ready event can start a sensor transfer. + /// Returns whether a data-ready event can start a FIFO drain. pub fn is_ready(&self) -> bool { - matches!(self.state, State::Ready { .. }) + self.state == State::Ready } fn advance_after_completion( @@ -233,40 +237,100 @@ impl Icm42688 { Operation::Identity => { let identity = self.receive[1]; if identity != EXPECTED_WHO_AM_I { - self.state = State::Faulted; - return Icm42688Event::Fault(Error::UnexpectedIdentity(identity)); + return self.fault(Error::UnexpectedIdentity(identity)); } self.queue(Operation::Configure(0), now); } Operation::Configure(index) => { let next = index + 1; - if next == 7 { - // 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::Settling { since: now }; + if next == CONFIGURATION.len() { + self.wait( + Operation::EnableFifo, + now, + SENSOR_SETTLE_US + SAMPLE_PERIOD_US, + ); } else { self.queue(Operation::Configure(next), now); } } - Operation::Sample => { + Operation::EnableFifo => self.queue(Operation::StartFifo, now), + Operation::StartFifo | Operation::FifoStatus | Operation::FifoCheck => { + if self.receive[1] & FIFO_FULL != 0 { + return self.fault(Error::FifoOverflow); + } + if operation != Operation::StartFifo && self.receive[1] & RESET_DONE != 0 { + return self.fault(Error::UnexpectedReset); + } + match operation { + Operation::StartFifo => { + self.state = State::Ready; + self.progress_at = Some(now); + return Icm42688Event::Initialized; + } + Operation::FifoCheck => { + self.state = State::Ready; + self.progress_at = Some(now); + return Icm42688Event::Samples( + self.batch.take().expect("checked drain contains a packet"), + ); + } + _ => 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)); + } + // Count is latched high-byte first. Leave any partial tail and + // records beyond this drain's bound in the FIFO. + let packets = (usize::from(count) / SAMPLE_BYTES).min(ImuBatch::CAPACITY); + if packets == 0 { + self.state = State::Ready; + return Icm42688Event::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 }; - return Icm42688Event::Sample(sample::decode(&bytes, requested)); + let Some((sample, tick)) = sample::decode(&bytes, requested, self.previous_tick) + else { + return self.fault(Error::FifoPacket); + }; + self.previous_tick = Some(tick); + 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::FifoLost + } else { + Operation::FifoSample(remaining - 1) + }, + now, + ); + } + Operation::FifoLost => { + if self.receive[1] != 0 || self.receive[2] != 0 { + return self.fault(Error::FifoOverflow); + } + self.queue(Operation::FifoCheck, now); } } match self.submit_pending(now) { - Ok(()) => Icm42688Event::Initializing, + Ok(()) => Icm42688Event::InProgress, Err(error) => Icm42688Event::Fault(error), } } - 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) { @@ -296,19 +360,25 @@ impl Icm42688 { }; match operation { Operation::Reset => self.prepare_write(DEVICE_CONFIG, SOFT_RESET), - Operation::Identity => self.prepare_read(WHO_AM_I, 2), + Operation::Identity => self.prepare_read(WHO_AM_I), Operation::Configure(index) => { let (register, value) = CONFIGURATION[index]; self.prepare_write(register, value); } - Operation::Sample => self.prepare_read(TEMP_DATA1, TRANSFER_CAPACITY), + // DS-000347 §6.4 permits changing these FIFO sensor-enable bits + // during operation. Both were off, so no startup records exist. + Operation::EnableFifo => self.prepare_write(FIFO_CONFIG1, 0x47), + Operation::StartFifo | Operation::FifoStatus | Operation::FifoCheck => { + self.prepare_read(INT_STATUS) + } + Operation::FifoCount => self.prepare_read(FIFO_COUNTH), + Operation::FifoSample(_) => self.prepare_read(FIFO_DATA), + Operation::FifoLost => self.prepare_read(FIFO_LOST_PKT0), } - let length = if operation == Operation::Sample { - TRANSFER_CAPACITY - } else { - 2 - }; - match self.bus.start_transfer(&self.transmit[..length]) { + match self + .bus + .start_transfer(&self.transmit[..operation.length()]) + { Ok(()) => { self.state = State::Active { operation, @@ -318,38 +388,52 @@ impl Icm42688 { } Err(error) if Bus::is_busy(&error) => Ok(()), Err(error) => { - self.state = State::Faulted; + self.enter_fault(); Err(Error::Start(error)) } } } + fn enter_fault(&mut self) { + self.state = State::Faulted; + self.batch = None; + self.progress_at = None; + } + + fn fault(&mut self, error: Error) -> Icm42688Event { + self.enter_fault(); + Icm42688Event::Fault(error) + } + fn check_deadline(&mut self, now: Timestamp) -> Option> { - let error = match self.state { + let mut 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), - } + Error::TransferTimeout } 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) => + _ if self + .progress_at + .is_some_and(|since| elapsed(now, since, PROGRESS_TIMEOUT_US)) => { Error::DataReadyTimeout } _ => return None, }; - self.state = State::Faulted; + // Cancellation must establish quiescence or quarantine permanent DMA storage. + if matches!(self.state, State::Active { .. }) + && let Err(failure) = self + .bus + .complete_transfer(SpiCompletion::TransferFault, &mut []) + && !matches!( + failure, + CompletionError::TransferFault | CompletionError::NotActive + ) + { + error = Error::Completion(failure); + } + self.enter_fault(); Some(error) } } @@ -364,248 +448,349 @@ fn elapsed(now: Timestamp, since: Timestamp, duration_us: u64) -> bool { mod tests { use super::*; use crate::mpu6000::test_bus::{FakeBus, FakeError}; - use kaptan_spi::{CompletionError, SpiCompletion}; - use kaptan_time::Timestamp; fn t(us: u64) -> Timestamp { - Timestamp::from_microseconds(us + 1_000) + Timestamp::from_microseconds(us) } - fn start(driver: &mut Icm42688) { - 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)); + fn respond(imu: &mut Icm42688, bytes: &[u8]) { + imu.bus.response.fill(0); + imu.bus.response[1..1 + bytes.len()].copy_from_slice(bytes); } - fn initialized() -> Icm42688 { - 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, t(100)), - Icm42688Event::Initializing - )); - imu.on_periodic_tick(t(1_099)); + fn complete(imu: &mut Icm42688, next: &[u8], us: u64) -> Icm42688Event { + respond(imu, next); + imu.on_transfer_complete(SpiCompletion::Complete, t(us)) + } + + pub(super) fn initialized() -> Icm42688 { + let mut imu = Icm42688::new(FakeBus::ready()); + imu.begin_initialization(t(0)).unwrap(); + imu.on_periodic_tick(t(999)); + assert_eq!(imu.bus.starts, 0); + imu.on_periodic_tick(t(1000)); + assert_eq!(&imu.bus.transmitted[..2], &[0x11, 1]); + complete(&mut imu, &[0x47], 1100); + imu.on_periodic_tick(t(2099)); assert_eq!(imu.bus.starts, 1); - imu.on_periodic_tick(t(1_100)); - assert!(matches!( - imu.on_transfer_complete(SpiCompletion::Complete, t(1_200)), - Icm42688Event::Initializing - )); - for index in 0..7 { - assert!(matches!( - imu.on_transfer_complete(SpiCompletion::Complete, t(1_300 + index * 100)), - Icm42688Event::Initializing - )); + imu.on_periodic_tick(t(2100)); + complete(&mut imu, &[], 2200); + for (index, command) in [ + [0x14, 0x03], + [0x63, 0x00], + [0x64, 0x60], + [0x4c, 0x33], + [0x4d, 0x40], + [0x54, 0x21], + [0x5f, 0x44], + [0x16, 0x40], + [0x52, 0xff], + [0x65, 0x08], + [0x4f, 0x46], + [0x50, 0x46], + [0x4e, 0x0f], + ] + .into_iter() + .enumerate() + { + assert_eq!(&imu.bus.transmitted[..2], &command); + complete(&mut imu, &[], 2300 + index as u64 * 100); } - 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, t(47_000)), - Icm42688Event::Initializing - )); - imu.on_transfer_complete(SpiCompletion::Complete, t(47_100)); - imu.on_periodic_tick(t(48_099)); + assert_eq!(&imu.bus.transmitted[..2], &[0x4e, 0x0f]); + imu.on_periodic_tick(t(49_499)); 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()); + assert_eq!(imu.request_samples(t(49_499)), Err(Error::NotReady)); + imu.on_periodic_tick(t(49_500)); + assert_eq!(&imu.bus.transmitted[..2], &[0x5f, 0x47]); + complete(&mut imu, &[RESET_DONE], 49_600); + assert_eq!(&imu.bus.transmitted[..2], &[0xad, 0]); + assert_eq!(complete(&mut imu, &[], 49_700), Icm42688Event::Initialized); 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, 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() { + fn ready() -> Icm42688 { 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, t(1_100)), - Icm42688Event::Fault(Error::UnexpectedIdentity(0)) - )); + imu.state = State::Ready; + imu.progress_at = Some(t(0)); + imu + } + + pub(super) fn packet(tick: u16, acceleration: i16) -> [u8; SAMPLE_BYTES] { + let mut packet = [0; SAMPLE_BYTES]; + packet[0] = 0x68; + packet[1..3].copy_from_slice(&acceleration.to_be_bytes()); + packet[14..].copy_from_slice(&tick.to_be_bytes()); + packet + } + + fn start_drain(imu: &mut Icm42688, count: u16, first: &[u8; SAMPLE_BYTES]) { + imu.request_samples(t(0)).unwrap(); + complete(imu, &count.to_be_bytes(), 1); + complete(imu, first, 2); } #[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)); + fn startup_keeps_fifo_empty_until_settled() { + let imu = initialized(); + assert!(imu.is_ready()); + let mut imu = Icm42688::new(FakeBus::ready()); + imu.queue(Operation::Identity, t(0)); + imu.submit_pending(t(0)).unwrap(); + assert_eq!( + complete(&mut imu, &[], 1), + Icm42688Event::Fault(Error::UnexpectedIdentity(0)) + ); + assert_eq!(imu.begin_initialization(t(2)), Err(Error::NotReady)); + } + + #[test] + fn publishes_ordered_batch_after_status() { + let mut imu = ready(); + start_drain(&mut imu, 33, &packet(65000, 8192)); + assert_eq!( + &imu.bus.transmitted[..17], + &[0xb0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + ); + assert_eq!( + imu.on_transfer_complete(SpiCompletion::NotPending, t(2)), + Icm42688Event::NotPending + ); + assert_eq!( + complete(&mut imu, &packet(402, -8192), 3), + Icm42688Event::InProgress + ); + // Accepted transport storage survives moving the driver and mutating staging bytes. + imu.transmit.fill(0xff); + let mut imu = core::mem::replace(&mut imu, ready()); + assert_eq!(complete(&mut imu, &[], 4), Icm42688Event::InProgress); + assert_eq!(complete(&mut imu, &[], 5), Icm42688Event::InProgress); + let Icm42688Event::Samples(batch) = complete(&mut imu, &[], 6) else { + panic!("missing checked batch"); + }; + assert_eq!(batch.as_slice().len(), 2); + assert_eq!(batch.as_slice()[0].acceleration_milligravity[0], 1000); + assert_eq!(batch.as_slice()[1].acceleration_milligravity[0], -1000); + assert_eq!(batch.as_slice()[0].timestamp, t(2)); + assert_eq!(batch.as_slice()[1].timestamp, t(3)); + assert!(imu.is_ready()); + assert_eq!(complete(&mut imu, &[], 7), Icm42688Event::NotPending); + } + + #[test] + fn leaves_excess_packets_and_partial_tail() { + for count in [80u16, 2064] { + let mut imu = ready(); + start_drain(&mut imu, count, &packet(0, 0)); + for index in 0..ImuBatch::CAPACITY { + let next = packet((index as u16 + 1) * 938, 0); + let response = if index + 1 == ImuBatch::CAPACITY { + &[][..] + } else { + &next[..] + }; + assert_eq!( + complete(&mut imu, response, 3 + index as u64), + Icm42688Event::InProgress + ); } + assert_eq!( + imu.state, + State::Active { + operation: Operation::FifoLost, + since: t(6) + } + ); + complete(&mut imu, &[], 7); + let Icm42688Event::Samples(batch) = complete(&mut imu, &[], 8) else { + panic!("missing bounded batch"); + }; + assert_eq!(batch.as_slice().len(), ImuBatch::CAPACITY); + } + for count in [0u16, 15] { + let mut imu = ready(); + start_drain(&mut imu, count, &packet(0, 0)); + assert!(imu.is_ready()); + assert_eq!(imu.bus.starts, 2); + assert_eq!(imu.progress_at, Some(t(0))); } } #[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; + fn malformed_count_or_packet_is_terminal() { + for (count, bytes, expected) in [ + (2065u16, packet(0, 0), Error::FifoCount(2065)), + (16, [0; SAMPLE_BYTES], Error::FifoPacket), + (16, packet(0, i16::MIN), Error::FifoPacket), + ] { + let mut imu = ready(); + imu.request_samples(t(0)).unwrap(); + complete(&mut imu, &count.to_be_bytes(), 1); + let event = complete(&mut imu, &bytes, 2); + let event = if count <= 2064 { + complete(&mut imu, &[], 3) + } else { + event + }; + assert_eq!(event, Icm42688Event::Fault(expected)); + assert!(imu.batch.is_none()); + assert_eq!(imu.begin_initialization(t(4)), Err(Error::NotReady)); + } + let mut imu = ready(); + start_drain(&mut imu, 32, &packet(42, 0)); + complete(&mut imu, &packet(42, 0), 3); assert_eq!( - driver.request_sample(t(1)), - Err(Error::Start(FakeError::Start)) - ); - assert_eq!( - driver.on_transfer_complete(SpiCompletion::Complete, t(2)), - Icm42688Event::NotPending + complete(&mut imu, &[], 4), + Icm42688Event::Fault(Error::FifoPacket) ); + assert!(imu.batch.is_none()); } #[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(); + fn loss_or_reset_discards_pending_batch() { + for (lost, status, error) in [ + ([0, 0], FIFO_FULL, Error::FifoOverflow), + ([0, 0], RESET_DONE, Error::UnexpectedReset), + ([1, 0], 0, Error::FifoOverflow), + ([0, 1], 0, Error::FifoOverflow), + ] { + let mut imu = ready(); + start_drain(&mut imu, 16, &packet(0, 0)); + complete(&mut imu, &lost, 3); + let event = complete(&mut imu, &[status], 4); + let event = if lost == [0, 0] { + complete(&mut imu, &[], 5) + } else { + event + }; + assert_eq!(event, Icm42688Event::Fault(error)); + assert!(imu.batch.is_none()); + assert_eq!(complete(&mut imu, &[], 6), Icm42688Event::NotPending); + } + } + + fn operations() -> impl Iterator { + [ + Operation::Reset, + Operation::Identity, + Operation::EnableFifo, + Operation::StartFifo, + Operation::FifoStatus, + Operation::FifoCount, + Operation::FifoSample(1), + Operation::FifoLost, + Operation::FifoCheck, + ] + .into_iter() + .chain((0..CONFIGURATION.len()).map(Operation::Configure)) + } + + #[test] + fn transfer_timeout_cancels_once() { + for operation in operations() { + let mut imu = Icm42688::new(FakeBus::ready()); + imu.queue(operation, t(100)); + imu.submit_pending(t(100)).unwrap(); + assert_eq!(imu.on_periodic_tick(t(10_099)), Icm42688Event::NotPending); 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)), + imu.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) - ); + assert_eq!(imu.bus.cancellations, 1); + assert_eq!(complete(&mut imu, &[], 10_101), Icm42688Event::NotPending); + assert_eq!(imu.begin_initialization(t(10_102)), 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(); + fn busy_admission_retains_operation() { + for operation in operations() { + let mut imu = Icm42688::new(FakeBus::ready()); + imu.bus.busy = true; + imu.queue(operation, t(100)); + imu.submit_pending(t(100)).unwrap(); assert_eq!( - driver.state, + imu.state, State::Pending { operation, since: t(100) } ); - driver.on_periodic_tick(t(200)); - driver.bus.busy = false; - driver.on_periodic_tick(t(300)); + imu.on_periodic_tick(t(200)); + imu.bus.busy = false; + imu.on_periodic_tick(t(300)); assert_eq!( - driver.state, + imu.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); + let mut imu = Icm42688::new(FakeBus::ready()); + imu.bus.busy = true; + imu.queue(Operation::Reset, t(0)); assert_eq!( - driver.on_periodic_tick(t(20_000)), + imu.on_periodic_tick(t(20_000)), Icm42688Event::Fault(Error::AdmissionTimeout) ); - assert_eq!(driver.bus.cancellations, 0); + assert_eq!(imu.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); + fn empty_drains_and_active_reads_do_not_renew_progress() { + let mut imu = ready(); + start_drain(&mut imu, 0, &packet(0, 0)); + imu.request_samples(t(9000)).unwrap(); + complete(&mut imu, &[0, 16], 9001); + complete(&mut imu, &packet(1, 0), 9002); assert_eq!( - driver.on_periodic_tick(t(60_100)), + imu.on_periodic_tick(t(12_000)), Icm42688Event::Fault(Error::DataReadyTimeout) ); - let mut driver = initialized(); - driver.request_sample(t(49_000)).unwrap(); + assert_eq!(imu.bus.cancellations, 1); + assert!(imu.batch.is_none()); + let mut imu = ready(); + imu.bus.busy = true; + imu.request_samples(t(9000)).unwrap(); assert_eq!( - driver.on_transfer_complete(SpiCompletion::Complete, t(59_000)), - Icm42688Event::Fault(Error::TransferTimeout) + imu.on_periodic_tick(t(12_000)), + Icm42688Event::Fault(Error::DataReadyTimeout) ); - let mut driver = initialized(); - driver.request_sample(t(49_000)).unwrap(); - driver.bus.fail_completion = Some(CompletionError::Quarantined); + assert_eq!(imu.bus.cancellations, 0); + } + + #[test] + fn completion_and_submission_errors_never_publish() { + for error in [ + CompletionError::InvalidLength, + CompletionError::Quarantined, + CompletionError::NotActive, + CompletionError::TransferFault, + ] { + let mut imu = ready(); + start_drain(&mut imu, 16, &packet(0, 0)); + complete(&mut imu, &[], 3); + imu.bus.fail_completion = Some(error); + assert!(matches!( + complete(&mut imu, &[], 4), + Icm42688Event::Fault(_) + )); + assert!(imu.batch.is_none()); + assert_eq!(complete(&mut imu, &[], 5), Icm42688Event::NotPending); + } + let mut imu = ready(); + imu.bus.fail_start = true; assert_eq!( - driver.on_periodic_tick(t(59_000)), + imu.request_samples(t(0)), + Err(Error::Start(FakeError::Start)) + ); + assert_eq!(complete(&mut imu, &[], 1), Icm42688Event::NotPending); + let mut imu = ready(); + imu.request_samples(t(0)).unwrap(); + imu.bus.fail_completion = Some(CompletionError::Quarantined); + assert_eq!( + imu.on_periodic_tick(t(10_000)), Icm42688Event::Fault(Error::Completion(CompletionError::Quarantined)) ); - assert_eq!(driver.bus.start_transfer(&[1]), Err(FakeError::Quarantined)); } } diff --git a/firmware/libs/kaptan-drivers/src/icm42688/sample.rs b/firmware/libs/kaptan-drivers/src/icm42688/sample.rs index 50c1fba..2cf7d3d 100644 --- a/firmware/libs/kaptan-drivers/src/icm42688/sample.rs +++ b/firmware/libs/kaptan-drivers/src/icm42688/sample.rs @@ -1,119 +1,158 @@ -//! 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. +//! ICM-42688-P 16-byte FIFO packet decoding. use kaptan_sensor::imu::ImuSnapshot; use kaptan_time::Timestamp; use super::SAMPLE_BYTES; -pub(crate) fn decode(bytes: &[u8; SAMPLE_BYTES], timestamp: Timestamp) -> ImuSnapshot { - ImuSnapshot::new( - timestamp, - [ - scale_acceleration(read_i16(bytes, 2)), - scale_acceleration(read_i16(bytes, 4)), - scale_acceleration(read_i16(bytes, 6)), - ], - [ - scale_angular_rate(read_i16(bytes, 8)), - scale_angular_rate(read_i16(bytes, 10)), - scale_angular_rate(read_i16(bytes, 12)), - ], - scale_temperature(read_i16(bytes, 0)), - ) +pub(crate) fn decode( + bytes: &[u8; SAMPLE_BYTES], + timestamp: Timestamp, + previous_tick: Option, +) -> Option<(ImuSnapshot, u16)> { + // Packet 3, both sensors, ordinary resolution, ODR timestamp. Only the + // first packet after explicit reset/configuration may report ODR changes. + if bytes[0] & 0xfc != 0x68 || (previous_tick.is_some() && bytes[0] != 0x68) { + return None; + } + // InvenSense eMD Icm426xx: ordinary FIFO temperature 0x80 is invalid. + if bytes[13] == 0x80 { + return None; + } + 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. -const fn scale_temperature(raw: i16) -> i16 { - ((raw as i32 * 10_000) / 13_248 + 2_500) as i16 +// FIFO temperature uses signed 8-bit data, 2.07 counts/°C, and a 25°C offset. +const fn scale_temperature(raw: i8) -> i16 { + ((raw as i32 * 10_000) / 207 + 2_500) as i16 } #[cfg(test)] mod tests { - use super::{decode, scale_temperature}; - use kaptan_time::Timestamp; + use super::*; - #[test] - fn decodes_the_icm_temperature_first_layout() { - let bytes = [0, 0, 0x20, 0, 0, 0, 0, 0, 0x7f, 0xeb, 0, 0, 0, 0]; - 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); + fn packet() -> [u8; SAMPLE_BYTES] { + [ + 0x68, 0x20, 0, 0, 0, 0, 0, 0x7f, 0xeb, 0, 0, 0, 0, 0, 0xff, 0xfe, + ] } #[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]); + fn packet_layout_and_signed_scales() { + let timestamp = Timestamp::from_microseconds(7); + let (sample, tick) = decode(&packet(), timestamp, None).unwrap(); + assert_eq!(sample.timestamp, timestamp); + assert_eq!(sample.acceleration_milligravity, [1000, 0, 0]); + assert_eq!(sample.angular_rate_milliradians_per_second, [8725, 0, 0]); + assert_eq!(sample.temperature_centidegrees_celsius, 2500); + assert_eq!(tick, 65534); + let mut bytes = packet(); + for (offset, raw) in [(1, -32766i16), (5, 32767), (7, -32766), (11, 32767)] { + bytes[offset..offset + 2].copy_from_slice(&raw.to_be_bytes()); + } + let (sample, _) = decode(&bytes, timestamp, None).unwrap(); + assert_eq!(sample.acceleration_milligravity, [-3999, 0, 3999]); + assert_eq!( + sample.angular_rate_milliradians_per_second, + [-8730, 0, 8731] + ); + for raw in i8::MIN..=i8::MAX { + let reference = f64::from(raw) / 2.07 * 100.0 + 2500.0; + 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, - [-8_731, 0, 8_731] + decode(&bytes, now, None).is_some(), + (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() { - // ±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), + fn tick_order_allows_wrap() { + let mut bytes = packet(); + let now = Timestamp::from_microseconds(0); + for (previous, tick, valid) in [ + (65534, 936u16, true), + (937, 1875, true), + (937, 937, false), + (937, 936, false), + (937, 33705, false), ] { - 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; + bytes[14..].copy_from_slice(&tick.to_be_bytes()); + assert_eq!(decode(&bytes, now, Some(previous)).is_some(), valid); } } } diff --git a/firmware/libs/kaptan-drivers/src/icm42688/service.rs b/firmware/libs/kaptan-drivers/src/icm42688/service.rs index 8bdfd3a..007c924 100644 --- a/firmware/libs/kaptan-drivers/src/icm42688/service.rs +++ b/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 { Idle, /// IMU configuration completed and the DRDY route may be enabled. Initialized, - /// A coherent IMU observation is ready for publication or control. - Sample(ImuSnapshot), + /// A checked FIFO batch is ready for publication. + Samples(ImuBatch), /// The ICM-42688-P path entered a terminal fault state. Fault(Error), } @@ -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 - /// the worker, at read submission, not at IRQ entry or physical sampling. + /// Starts a drain from one coalesced notification. Packet timestamps are + /// captured at their read submissions, not at IRQ entry or sampling. pub fn on_data_ready(&mut self) -> Icm42688ServiceEvent { 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(event: Icm42688Event) -> Icm42688ServiceEvent { 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(event: Icm42688Event) -> Icm42688ServiceEvent { #[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 { - 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; + fn clock_labels_each_accepted_packet_read() { + let mut service = Icm42688Service::new(Clock(50_000), initialized()); + service.imu.bus.response.fill(0); 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; - assert!( - matches!(service.on_transfer_complete(SpiCompletion::Complete), - Icm42688ServiceEvent::Sample(sample) if sample.timestamp == Timestamp::from_microseconds(48_200)) + service.imu.bus.response.fill(0); + for now in [50_400, 50_500] { + service.clock.0 = now; + 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() { - let mut service = initialized(); - service.clock.0 = 59_999; + fn missing_drdy_and_dma_are_terminal() { + let mut service = Icm42688Service::new(Clock(61_699), initialized()); + let starts = service.imu.bus.starts; assert!(matches!( service.on_periodic_tick(), Icm42688ServiceEvent::Idle )); - assert_eq!(service.imu.bus.starts, 11); - service.clock.0 = 60_000; + assert_eq!(service.imu.bus.starts, starts); + 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) diff --git a/firmware/libs/kaptan-drivers/src/lib.rs b/firmware/libs/kaptan-drivers/src/lib.rs index 6320c26..013f8cd 100644 --- a/firmware/libs/kaptan-drivers/src/lib.rs +++ b/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)] + ); + } +} diff --git a/firmware/libs/kaptan-drivers/src/mpu6000/error.rs b/firmware/libs/kaptan-drivers/src/mpu6000/error.rs index 2703567..ffc8226 100644 --- a/firmware/libs/kaptan-drivers/src/mpu6000/error.rs +++ b/firmware/libs/kaptan-drivers/src/mpu6000/error.rs @@ -27,7 +27,16 @@ pub enum Error { /// 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), } diff --git a/firmware/libs/kaptan-drivers/src/mpu6000/mod.rs b/firmware/libs/kaptan-drivers/src/mpu6000/mod.rs index 119d9f5..1397cfe 100644 --- a/firmware/libs/kaptan-drivers/src/mpu6000/mod.rs +++ b/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, - DEVICE_RESET, ENABLE_ALL_SENSOR_AXES, EXPECTED_WHO_AM_I, GYRO_CONFIG, I2C_INTERFACE_DISABLE, - INT_ENABLE, INT_PIN_CFG, INTERRUPT_CLEAR_ON_ANY_READ, PWR_MGMT_1, PWR_MGMT_2, SAMPLE_BYTES, - SMPLRT_DIV, USER_CTRL, WHO_AM_I, + ACCEL_CONFIG, CLOCK_SOURCE_PLL_X_GYRO, CONFIG, DATA_READY_INTERRUPT, DEVICE_RESET, + ENABLE_ALL_SENSOR_AXES, EXPECTED_WHO_AM_I, FIFO_CAPACITY, FIFO_COUNT_H, FIFO_EN, FIFO_ENABLE, + FIFO_OVERFLOW_INTERRUPT, FIFO_R_W, FIFO_RESET, FIFO_SENSOR_CHANNELS, GYRO_CONFIG, + 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), - (INT_ENABLE, DATA_READY_INTERRUPT), + // Only INT_STATUS reads clear overflow, including throughout FIFO drains. + (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 { - since: Timestamp, - }, + Ready, 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 { /// No sample, initialization completion or terminal fault was produced. NotPending, - /// One initialization transfer completed; initialization continues. - Initializing, + /// One transfer completed; initialization or acquisition continues. + InProgress, /// The MPU6000 identity and configuration sequence completed. Initialized, - /// One coherent IMU sample completed. - Sample(ImuSnapshot), + /// A bounded, overflow-checked FIFO drain completed. + Samples(ImuBatch), /// Initialization or acquisition failed. Fault(Error), } @@ -109,6 +131,8 @@ pub struct Mpu6000 { bus: Bus, config: Mpu6000Config, state: State, + progress_at: Option, + batch: Option, transmit: [u8; TRANSFER_CAPACITY], receive: [u8; TRANSFER_CAPACITY], } @@ -120,6 +144,8 @@ impl Mpu6000 { bus, config, state: State::Dormant, + progress_at: None, + batch: None, transmit: [0; TRANSFER_CAPACITY], receive: [0; TRANSFER_CAPACITY], } @@ -154,8 +180,7 @@ impl Mpu6000 { self.queue(next, now); } State::Settling { since } if elapsed(now, since, self.sample_period_us()) => { - self.state = State::Ready { since: now }; - return Mpu6000Event::Initialized; + self.queue(Operation::ResetFifo, now); } _ => {} } @@ -165,17 +190,16 @@ impl Mpu6000 { } } - /// 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> { + /// Accepts one bounded FIFO drain. Each snapshot records its packet-read + /// submission time, not the ISR time or a physical sampling epoch. + pub fn request_samples(&mut self, now: Timestamp) -> Result<(), Error> { 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 Mpu6000 { let State::Active { operation, since } = self.state else { return Mpu6000Event::NotPending; }; - let length = if operation == Operation::Sample { - TRANSFER_CAPACITY - } else { - 2 - }; + let length = operation.transfer_length(); 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) - } - Err(error) => { - self.state = State::Faulted; - Mpu6000Event::Fault(Error::Completion(error)) - } + Err(CompletionError::TransferFault) => self.fault(Error::TransferFault), + Err(error) => self.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 Mpu6000 { Operation::Identity => { let identity = self.receive[1]; if identity != EXPECTED_WHO_AM_I { - self.state = State::Faulted; - return Mpu6000Event::Fault(Error::UnexpectedIdentity(identity)); + return self.fault(Error::UnexpectedIdentity(identity)); } self.queue(Operation::Configure(0), now); @@ -244,22 +257,93 @@ impl Mpu6000 { 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. + // FIFO stays disabled until the configured output has settled. 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 }; - return Mpu6000Event::Sample(sample::decode(&bytes, requested, self.config)); + let 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 Mpu6000 { 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 Mpu6000 { 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 { - TRANSFER_CAPACITY - } else { - 2 - }; + let length = operation.transfer_length(); match self.bus.start_transfer(&self.transmit[..length]) { Ok(()) => { self.state = State::Active { @@ -330,7 +421,7 @@ impl Mpu6000 { } 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 Mpu6000 { fn check_deadline(&mut self, now: Timestamp) -> Option> { 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), - } + Error::TransferTimeout } State::Pending { since, .. } if elapsed(now, since, ADMISSION_TIMEOUT_US) => { Error::AdmissionTimeout } - State::Ready { since } - if elapsed( + _ if self.progress_at.is_some_and(|since| { + 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) -> Mpu6000Event { + 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) { - 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)); + fn response(driver: &mut Mpu6000, bytes: &[u8]) { + driver.bus.response.fill(0); + driver.bus.response[1..=bytes.len()].copy_from_slice(bytes); } - fn initialized() -> Mpu6000 { - let mut bus = FakeBus::ready(); - bus.response[1] = 0x68; - let mut driver = Mpu6000::new(bus, TEST_CONFIG); - 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)); + fn complete(driver: &mut Mpu6000, now: u64) -> Mpu6000Event { + driver.on_transfer_complete(SpiCompletion::Complete, t(now)) + } + + pub(super) fn initialized() -> Mpu6000 { + let mut driver = Mpu6000::new(FakeBus::ready(), TEST_CONFIG); + driver.begin_initialization(t(0)).unwrap(); + for (at, command) in [(100_000, [0x6b, 0x80]), (200_000, [0x68, 7])] { + driver.on_periodic_tick(t(at - 1)); + assert!(!driver.is_ready()); + driver.on_periodic_tick(t(at)); + assert_eq!(&driver.bus.transmitted[..2], &command); + complete(&mut driver, at); } - assert_eq!( - driver.on_periodic_tick(t(311_200)), - Mpu6000Event::Initialized - ); - assert!(driver.is_ready()); + response(&mut driver, &[0x68]); + driver.on_periodic_tick(t(300_000)); + assert_eq!(&driver.bus.transmitted[..2], &[0xf5, 0]); + response(&mut driver, &[0]); + 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, count: u16, now: u64) -> Mpu6000Event { + 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 { + [ + 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() { - 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); + fn startup_keeps_fifo_disabled_until_sensor_settles() { + let driver = initialized(); + assert!(driver.is_ready()); + assert_eq!(driver.batch, None); + assert_eq!(driver.progress_at, Some(t(410_000))); + } + + #[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() { - 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 - ]; + fn packet_bytes_remain_owned_until_checked_publication() { 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]); + begin_drain(&mut driver, 14, 411_000); 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) = - driver.on_transfer_complete(SpiCompletion::Complete, t(312_200)) - else { - panic!("sample transfer must complete"); + response(&mut driver, &[0]); + assert_eq!(complete(&mut driver, 411_100), Mpu6000Event::InProgress); + assert_eq!(&driver.bus.transmitted[..2], &[0xba, 0]); + 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"); }; - - assert_eq!(sample.acceleration_milligravity, [1_000, -1_000, 0]); + let sample = batch.as_slice()[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.timestamp, t(312_000)); + assert_eq!(sample.temperature_centidegrees_celsius, 3653); + 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)), - Mpu6000Event::NotPending + begin_drain(&mut driver, 1025, 411_000), + 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() { - let mut bus = FakeBus::ready(); - bus.response[1] = 0x68; - let mut driver = Mpu6000::new(bus, TEST_CONFIG); - - start(&mut driver); - assert!(matches!( - driver.on_transfer_complete(SpiCompletion::TransferFault, t(100)), - Mpu6000Event::Fault(Error::TransferFault) - )); + fn reset_during_drain_cannot_publish_repeated_fifo_bytes() { + let mut driver = initialized(); + driver.request_samples(t(411_000)).unwrap(); + response(&mut driver, &14u16.to_be_bytes()); + complete(&mut driver, 411_000); + response(&mut driver, &[0xaa; 14]); + complete(&mut driver, 411_000); + response(&mut driver, &[0]); + 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); - 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(); + let mut driver = initialized(); + begin_drain(&mut driver, 14, 411_000); + response(&mut driver, &[0]); + complete(&mut driver, 411_001); + assert!(driver.batch.is_some()); 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!(!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)); - } + assert_eq!(driver.batch, None); + assert_eq!(complete(&mut driver, 411_003), Mpu6000Event::NotPending); + assert_eq!( + driver.begin_initialization(t(411_004)), + 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); + fn rejected_submission_discards_the_batch() { + let mut driver = initialized(); + begin_drain(&mut driver, 14, 411_000); 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 + complete(&mut driver, 411_001), + Mpu6000Event::Fault(Error::Start(FakeError::Start)) ); + 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() { - let operations = [ - Operation::Reset, - Operation::ResetSignalPaths, - Operation::Identity, - Operation::Sample, - ]; - for operation in operations - .into_iter() - .chain((0..9).map(Operation::Configure)) - { + fn every_transfer_times_out_and_cancels_once() { + for operation in operations() { 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 - ); + assert_eq!(complete(&mut driver, 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() { - for operation in [ - Operation::Reset, - Operation::ResetSignalPaths, - Operation::Identity, - Operation::Sample, - ] - .into_iter() - .chain((0..9).map(Operation::Configure)) - { + fn busy_admission_preserves_the_operation_and_expires() { + for operation in operations() { 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)); - assert_eq!(driver.bus.starts, 12); + begin_drain(&mut driver, 14, 411_000); + response(&mut driver, &[0]); + complete(&mut driver, 411_100); + assert!(driver.batch.is_some()); 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)), + complete(&mut driver, 421_100), 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)), - Mpu6000Event::Fault(Error::Completion(CompletionError::Quarantined)) + driver.request_samples(t(440_000)), + 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); + } } } diff --git a/firmware/libs/kaptan-drivers/src/mpu6000/registers.rs b/firmware/libs/kaptan-drivers/src/mpu6000/registers.rs index b552921..c46fa3a 100644 --- a/firmware/libs/kaptan-drivers/src/mpu6000/registers.rs +++ b/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; diff --git a/firmware/libs/kaptan-drivers/src/mpu6000/service.rs b/firmware/libs/kaptan-drivers/src/mpu6000/service.rs index 1005fee..1e286e3 100644 --- a/firmware/libs/kaptan-drivers/src/mpu6000/service.rs +++ b/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 { Idle, /// MPU configuration completed and the DRDY route may be enabled. Initialized, - /// A coherent IMU observation is ready for publication or control. - Sample(ImuSnapshot), + /// A bounded, overflow-checked FIFO drain is ready for publication. + Samples(ImuBatch), /// The MPU6000 path entered a terminal fault state. Fault(Error), } -/// Owns MPU6000 state, its clock, and sample acceptance. -/// -/// Exactly one platform worker invokes this service. Hardware ISRs only report -/// deferred DRDY, timer, and SPI-DMA terminal events to that worker. +/// Drives MPU6000 acquisition from deferred events using one monotonic clock. pub struct Mpu6000Service { clock: Clock, imu: Mpu6000, @@ -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 - /// the worker, at read submission, not at IRQ entry or physical sampling. + /// Starts one bounded drain after a coalesced data-ready notification. pub fn on_data_ready(&mut self) -> Mpu6000ServiceEvent { 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(event: Mpu6000Event) -> Mpu6000ServiceEvent { 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 crate::mpu6000::test_bus::FakeBus; + use super::*; use crate::mpu6000::{ - AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000, Mpu6000Config, - }; - 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, + test_bus::FakeBus, + tests::{TEST_CONFIG, initialized}, }; + 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() { - let mut bus = FakeBus::ready(); - bus.response[1] = 0x68; - let mut service = Mpu6000Service::new(Clock(0), Mpu6000::new(bus, TEST_CONFIG)); - - service.begin().unwrap(); + fn publication_waits_for_post_status() { + let mut service = Mpu6000Service::new(Clock(411_000), initialized()); 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.imu.bus.response[1..3].copy_from_slice(&14u16.to_be_bytes()); + service.on_transfer_complete(SpiCompletion::Complete); + service.imu.bus.response.fill(0); + 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.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_periodic_tick(), - Mpu6000ServiceEvent::Initialized - )); - service.clock.0 = 410_042; - assert!(matches!(service.on_data_ready(), Mpu6000ServiceEvent::Idle)); - service.clock.0 = 410_123; + service.clock.0 = 411_400; + service.imu.bus.response[1] = 0x50; 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), diff --git a/firmware/libs/kaptan-drivers/src/mpu6000/test_bus.rs b/firmware/libs/kaptan-drivers/src/mpu6000/test_bus.rs index c35ec31..4f2a097 100644 --- a/firmware/libs/kaptan-drivers/src/mpu6000/test_bus.rs +++ b/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, pub busy: bool, pub starts: usize, pub cancellations: usize, - pub transmitted: [u8; 15], - pending: Option<([u8; 15], usize)>, + pub transmitted: [u8; 32], + 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, } diff --git a/firmware/libs/kaptan-sensor/src/imu.rs b/firmware/libs/kaptan-sensor/src/imu.rs index 7ee5021..10e4e07 100644 --- a/firmware/libs/kaptan-sensor/src/imu.rs +++ b/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 -/// before this value is published. -/// -/// 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. +/// temperature is centidegrees Celsius. Chip drivers return sensor-frame axes. +/// The platform applies the board mounting transform before publication, so +/// published observations use the body frame. This type does not validate values. #[derive(Clone, Copy, Debug, Eq, PartialEq)] pub struct ImuSnapshot { - /// Host monotonic acquisition-request time, captured immediately before the - /// accepted register read is submitted by the worker. This is neither DRDY - /// ISR entry nor the sensor's physical sampling epoch. Register updates, - /// filtering and deferred/coalesced interrupts prevent that inference. + /// Producer timestamp in the controller's monotonic clock domain. + /// The acquisition source defines its meaning. It need not represent the + /// sensor's physical sampling epoch. 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, diff --git a/firmware/platforms/stm32f7/src/f722/sensors.rs b/firmware/platforms/stm32f7/src/f722/sensors.rs index dc19673..8add11f 100644 --- a/firmware/platforms/stm32f7/src/f722/sensors.rs +++ b/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 -/// here because flight policy only needs the terminal healthy/fault outcome. +/// IMU worker result with device errors mapped to reportable IDs. 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 -/// route's concrete service. No peripheral ownership crosses this interface. +/// Sensor operations invoked by the deferred IMU worker. 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, mount: ImuMount, @@ -136,7 +134,7 @@ fn mpu_event(event: Mpu6000ServiceEvent, 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, 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. -/// -/// This is immutable board metadata, not an airframe calibration setting. +fn latest_sample(samples: ImuBatch, mount: ImuMount) -> ImuEvent { + // Scheduled MAVLink publication retains only the newest validated record. + 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( - Timestamp::from_microseconds(123), - acceleration, - angular_rate, - 25, - ) + ImuSnapshot { + acceleration_milligravity: acceleration, + angular_rate_milliradians_per_second: angular_rate, + ..later + } ); } }