refactor: move firmware workspace under firmware

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

@@ -0,0 +1,310 @@
+//! ICM-42688-P register protocol and asynchronous sample driver.
+
+mod error;
+mod sample;
+pub mod service;
+
+use kaptan_sensor::imu::ImuSnapshot;
+use kaptan_spi::{SpiCompletion, SpiDevice};
+use kaptan_time::Timestamp;
+
+pub use error::Error;
+pub use service::{Icm42688Service, Icm42688ServiceEvent};
+
+const WHO_AM_I: u8 = 0x75;
+const EXPECTED_WHO_AM_I: u8 = 0x47;
+const DEVICE_CONFIG: u8 = 0x11;
+const SOFT_RESET: u8 = 0x01;
+const PWR_MGMT0: u8 = 0x4e;
+const GYRO_CONFIG0: u8 = 0x4f;
+const ACCEL_CONFIG0: u8 = 0x50;
+const GYRO_ACCEL_CONFIG0: u8 = 0x52;
+const INT_CONFIG: u8 = 0x14;
+const INT_CONFIG0: u8 = 0x63;
+const INT_CONFIG1: u8 = 0x64;
+const INT_SOURCE0: u8 = 0x65;
+const INTF_CONFIG1: u8 = 0x4d;
+const TEMP_DATA1: u8 = 0x1d;
+const SAMPLE_BYTES: usize = 14;
+const TRANSFER_CAPACITY: usize = SAMPLE_BYTES + 1;
+
+// 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] = [
+    (INT_CONFIG, 0x03),
+    (INT_CONFIG0, 0x00),
+    (INT_CONFIG1, 0x60),
+    (INTF_CONFIG1, 0x40),
+    (GYRO_ACCEL_CONFIG0, 0xff),
+    (INT_SOURCE0, 0x08),
+    (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)]
+enum State {
+    Dormant,
+    Resetting,
+    WaitingForReset,
+    ReadingIdentity,
+    WritingConfiguration(usize),
+    WaitingForSensorStart,
+    Ready,
+    ReadingSample { timestamp: Timestamp },
+}
+
+/// Observable outcome of one ICM-42688-P SPI terminal event.
+#[derive(Debug, Eq, PartialEq)]
+pub enum Icm42688Event<BusError> {
+    /// The selected SPI interrupt had no terminal transfer event.
+    NotPending,
+    /// One initialization transfer completed; initialization continues.
+    Initializing,
+    /// The ICM-42688-P identity and configuration sequence completed.
+    Initialized,
+    /// One coherent IMU sample completed.
+    Sample(ImuSnapshot),
+    /// Initialization or acquisition failed.
+    Fault(Error<BusError>),
+}
+
+/// Configured ICM-42688-P driver.
+///
+/// The driver owns fixed transfer buffers and progresses exclusively from
+/// timer, data-ready and SPI-DMA completion events. It never waits for a
+/// peripheral status flag.
+pub struct Icm42688<Bus> {
+    bus: Bus,
+    state: State,
+    transmit: [u8; TRANSFER_CAPACITY],
+    receive: [u8; TRANSFER_CAPACITY],
+}
+
+impl<Bus> Icm42688<Bus> {
+    /// Creates an inactive ICM-42688-P state machine.
+    pub const fn new(bus: Bus) -> Self {
+        Self {
+            bus,
+            state: State::Dormant,
+            transmit: [0; TRANSFER_CAPACITY],
+            receive: [0; TRANSFER_CAPACITY],
+        }
+    }
+}
+
+impl<Bus: SpiDevice> Icm42688<Bus> {
+    /// Starts the sensor reset. A subsequent periodic tick provides the
+    /// datasheet-required reset-settling interval before identification.
+    pub fn begin_initialization(&mut self) -> Result<(), Error<Bus::StartError>> {
+        if self.state != State::Dormant {
+            return Err(Error::NotReady);
+        }
+        self.prepare_write(DEVICE_CONFIG, SOFT_RESET);
+        self.submit(2)?;
+        self.state = State::Resetting;
+        Ok(())
+    }
+
+    /// Advances reset/wake delays from the board periodic event.
+    pub fn advance_initialization(&mut self) -> Result<(), Error<Bus::StartError>> {
+        match self.state {
+            State::WaitingForReset => {
+                self.prepare_read(WHO_AM_I, 2);
+                self.submit(2)?;
+                self.state = State::ReadingIdentity;
+            }
+            State::WaitingForSensorStart => {
+                self.start_configuration(7)?;
+            }
+            _ => {}
+        }
+        Ok(())
+    }
+
+    /// Starts one temperature, accelerometer and gyroscope burst read.
+    pub fn request_sample(&mut self, timestamp: Timestamp) -> Result<(), Error<Bus::StartError>> {
+        if self.state != State::Ready {
+            return Err(Error::NotReady);
+        }
+        self.prepare_read(TEMP_DATA1, TRANSFER_CAPACITY);
+        self.submit(TRANSFER_CAPACITY)?;
+        self.state = State::ReadingSample { timestamp };
+        Ok(())
+    }
+
+    /// Advances the state machine from an already-acknowledged SPI completion.
+    pub fn on_transfer_complete(
+        &mut self,
+        completion: SpiCompletion,
+    ) -> Icm42688Event<Bus::StartError> {
+        self.bus.complete_transfer(completion);
+        match completion {
+            SpiCompletion::NotPending => Icm42688Event::NotPending,
+            SpiCompletion::TransferFault => {
+                self.state = State::Dormant;
+                Icm42688Event::Fault(Error::TransferFault)
+            }
+            SpiCompletion::Complete => self.advance_after_completion(),
+        }
+    }
+
+    /// Returns whether an IMU data-ready event can start a sensor transfer.
+    pub fn is_ready(&self) -> bool {
+        self.state == State::Ready
+    }
+
+    fn advance_after_completion(&mut self) -> Icm42688Event<Bus::StartError> {
+        match self.state {
+            State::Dormant => Icm42688Event::NotPending,
+            State::Resetting => {
+                self.state = State::WaitingForReset;
+                Icm42688Event::Initializing
+            }
+            State::WaitingForReset | State::WaitingForSensorStart => Icm42688Event::NotPending,
+            State::ReadingIdentity => {
+                let identity = self.receive[1];
+                if identity != EXPECTED_WHO_AM_I {
+                    self.state = State::Dormant;
+                    return Icm42688Event::Fault(Error::UnexpectedIdentity(identity));
+                }
+                self.start_configuration_event(0)
+            }
+            State::WritingConfiguration(index) => {
+                let next = index + 1;
+                if next == 7 {
+                    // PWR_MGMT0 starts both sensor paths.  The following
+                    // timer tick gives them more than the required 200 µs
+                    // before their output-rate configuration is written.
+                    self.state = State::WaitingForSensorStart;
+                    Icm42688Event::Initializing
+                } else if next == CONFIGURATION.len() {
+                    self.state = State::Ready;
+                    Icm42688Event::Initialized
+                } else {
+                    self.start_configuration_event(next)
+                }
+            }
+            State::Ready => Icm42688Event::NotPending,
+            State::ReadingSample { timestamp } => {
+                let mut bytes = [0; SAMPLE_BYTES];
+                bytes.copy_from_slice(&self.receive[1..]);
+                self.state = State::Ready;
+                Icm42688Event::Sample(sample::decode(&bytes, timestamp))
+            }
+        }
+    }
+
+    fn start_configuration_event(&mut self, index: usize) -> Icm42688Event<Bus::StartError> {
+        match self.start_configuration(index) {
+            Ok(()) => Icm42688Event::Initializing,
+            Err(error) => Icm42688Event::Fault(error),
+        }
+    }
+
+    fn start_configuration(&mut self, index: usize) -> Result<(), Error<Bus::StartError>> {
+        let (register, value) = CONFIGURATION[index];
+        self.prepare_write(register, value);
+        if let Err(error) = self.submit(2) {
+            self.state = State::Dormant;
+            return Err(error);
+        }
+        self.state = State::WritingConfiguration(index);
+        Ok(())
+    }
+
+    fn prepare_read(&mut self, register: u8, length: usize) {
+        self.transmit = [0; TRANSFER_CAPACITY];
+        self.receive = [0; TRANSFER_CAPACITY];
+        self.transmit[0] = register | 0x80;
+        debug_assert!(length <= TRANSFER_CAPACITY);
+    }
+
+    fn prepare_write(&mut self, register: u8, value: u8) {
+        self.transmit = [0; TRANSFER_CAPACITY];
+        self.receive = [0; TRANSFER_CAPACITY];
+        self.transmit[..2].copy_from_slice(&[register, value]);
+    }
+
+    fn submit(&mut self, length: usize) -> Result<(), Error<Bus::StartError>> {
+        let (bus, transmit, receive) = (&mut self.bus, &self.transmit, &mut self.receive);
+        bus.start_transfer(&transmit[..length], &mut receive[..length])
+            .map_err(Error::Start)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::{Error, Icm42688, Icm42688Event, TRANSFER_CAPACITY};
+    use kaptan_spi::{SpiCompletion, SpiDevice};
+    use kaptan_time::Timestamp;
+
+    struct Bus {
+        response: [u8; TRANSFER_CAPACITY],
+    }
+
+    impl SpiDevice for Bus {
+        type StartError = ();
+
+        fn start_transfer(&mut self, _: &[u8], receive: &mut [u8]) -> Result<(), Self::StartError> {
+            receive.copy_from_slice(&self.response[..receive.len()]);
+            Ok(())
+        }
+
+        fn complete_transfer(&mut self, _: SpiCompletion) {}
+    }
+
+    #[test]
+    fn accepts_a_sample_only_after_identity_and_wake_delays() {
+        let mut response = [0; TRANSFER_CAPACITY];
+        response[1] = 0x47;
+        let mut imu = Icm42688::new(Bus { response });
+        imu.begin_initialization().unwrap();
+        assert!(matches!(
+            imu.on_transfer_complete(SpiCompletion::Complete),
+            Icm42688Event::Initializing
+        ));
+        assert!(matches!(imu.advance_initialization(), Ok(())));
+        assert!(matches!(
+            imu.on_transfer_complete(SpiCompletion::Complete),
+            Icm42688Event::Initializing
+        ));
+        for _ in 0..7 {
+            assert!(matches!(
+                imu.on_transfer_complete(SpiCompletion::Complete),
+                Icm42688Event::Initializing
+            ));
+        }
+        assert!(matches!(imu.advance_initialization(), Ok(())));
+        assert!(matches!(
+            imu.on_transfer_complete(SpiCompletion::Complete),
+            Icm42688Event::Initializing
+        ));
+        assert!(matches!(
+            imu.on_transfer_complete(SpiCompletion::Complete),
+            Icm42688Event::Initialized
+        ));
+        assert!(imu.is_ready());
+        imu.request_sample(Timestamp::from_microseconds(1)).unwrap();
+        assert!(matches!(
+            imu.on_transfer_complete(SpiCompletion::Complete),
+            Icm42688Event::Sample(_)
+        ));
+    }
+
+    #[test]
+    fn rejects_a_wrong_identity() {
+        let mut imu = Icm42688::new(Bus {
+            response: [0; TRANSFER_CAPACITY],
+        });
+        imu.begin_initialization().unwrap();
+        let _ = imu.on_transfer_complete(SpiCompletion::Complete);
+        imu.advance_initialization().unwrap();
+        assert!(matches!(
+            imu.on_transfer_complete(SpiCompletion::Complete),
+            Icm42688Event::Fault(Error::UnexpectedIdentity(0))
+        ));
+    }
+}